1 TF坐标转换概念
tf：TransForm Frame,坐标变换
坐标系：ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。
TF左边转换作用：在 ROS 中用于实现不同坐标系之间的点或向量的转换。
说明
在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:
tf2_geometry_msgs:可以将ROS消息转换成tf2消息。
tf2: 封装了坐标变换的常用消息。
tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。
2 坐标msg消息
订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped。前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。
2.1 1.geometry_msgs/TransformStamped
std_msgs/Header header #头信息 uint32 seq #|-- 序列号 time stamp #|-- 时间戳 string frame_id #|-- 坐标 ID string child_frame_id #子坐标系的 id geometry_msgs/Transform transform #坐标信息 geometry_msgs/Vector3 translation #偏移量 float64 x #|-- X 方向的偏移量 float64 y #|-- Y 方向的偏移量 float64 z #|-- Z 方向上的偏移量 geometry_msgs/Quaternion rotation #四元数 float64 x float64 y float64 z float64 w
2.2 2.geometry_msgs/PointStamped
std_msgs/Header header #头 uint32 seq #|-- 序号 time stamp #|-- 时间戳 string frame_id #|-- 所属坐标系的 id geometry_msgs/Point point #点坐标 float64 x #|-- x y z 坐标 float64 y float64 z
3 静态坐标变换
所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。
需求描述:
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
实现分析:
坐标系相对关系，可以通过发布方发布
订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出
程序实现：
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
2.发布方 tf_pub.cpp
/* 静态坐标变换发布方: 发布关于 laser 坐标系的位置信息 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建静态坐标转换广播器 4.创建坐标系信息 5.广播器发布坐标系信息 6.spin() */ // 1.包含头文件 #include "ros/ros.h" #include "tf2_ros/static_transform_broadcaster.h" #include "geometry_msgs/TransformStamped.h" #include "tf2/LinearMath/Quaternion.h" int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"static_brocast"); // 3.创建静态坐标转换广播器 tf2_ros::StaticTransformBroadcaster broadcaster; // 4.创建坐标系信息 geometry_msgs::TransformStamped ts; //----设置头信息 ts.header.seq = 100; ts.header.stamp = ros::Time::now(); ts.header.frame_id = "base_link"; //----设置子级坐标系 ts.child_frame_id = "laser"; //----设置子级相对于父级的偏移量 ts.transform.translation.x = 0.2; ts.transform.translation.y = 0.0; ts.transform.translation.z = 0.5; //----设置四元数:将 欧拉角数据转换成四元数 tf2::Quaternion qtn; qtn.setRPY(0,0,0); ts.transform.rotation.x = qtn.getX(); ts.transform.rotation.y = qtn.getY(); ts.transform.rotation.z = qtn.getZ(); ts.transform.rotation.w = qtn.getW(); // 5.广播器发布坐标系信息 broadcaster.sendTransform(ts); ros::spin(); return 0; }
3.订阅方tf_sub.cpp
/* 订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建 TF 订阅节点 4.生成一个坐标点(相对于子级坐标系) 5.转换坐标点(相对于父级坐标系) 6.spin() */ //1.包含头文件 #include "ros/ros.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" #include "geometry_msgs/PointStamped.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件 int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"tf_sub"); ros::NodeHandle nh; // 3.创建 TF 订阅节点 tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer); ros::Rate r(1); while (ros::ok()) { // 4.生成一个坐标点(相对于子级坐标系) geometry_msgs::PointStamped point_laser; point_laser.header.frame_id = "laser"; point_laser.header.stamp = ros::Time::now(); point_laser.point.x = 1; point_laser.point.y = 2; point_laser.point.z = 7.3; // 5.转换坐标点(相对于父级坐标系) //新建一个坐标点，用于接收转换结果 //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------ try { geometry_msgs::PointStamped point_base; point_base = buffer.transform(point_laser,"base_link"); ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str()); } catch(const std::exception& e) { // std::cerr << e.what() << '\n'; ROS_INFO("程序异常....."); } r.sleep(); ros::spinOnce(); } return 0; }
4 动态坐标变换
所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。
需求描述:
启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。
实现分析:
乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
将 pose 信息转换成 坐标系相对信息并发布
程序实现：
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
2.发布方
/* 动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的) 需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘 控制乌龟运动，将两个坐标系的相对位置动态发布 实现分析: 1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点 2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度 3.将 pose 信息转换成 坐标系相对信息并发布 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建 ROS 句柄 4.创建订阅对象 5.回调函数处理订阅到的数据(实现TF广播) 5-1.创建 TF 广播器 5-2.创建 广播的数据(通过 pose 设置) 5-3.广播器发布数据 6.spin */ // 1.包含头文件 #include "ros/ros.h" #include "turtlesim/Pose.h" #include "tf2_ros/transform_broadcaster.h" #include "geometry_msgs/TransformStamped.h" #include "tf2/LinearMath/Quaternion.h" void doPose(const turtlesim::Pose::ConstPtr& pose){ // 5-1.创建 TF 广播器 static tf2_ros::TransformBroadcaster broadcaster; // 5-2.创建 广播的数据(通过 pose 设置) geometry_msgs::TransformStamped tfs; // |----头设置 tfs.header.frame_id = "world"; tfs.header.stamp = ros::Time::now(); // |----坐标系 ID tfs.child_frame_id = "turtle1"; // |----坐标系相对信息设置 tfs.transform.translation.x = pose->x; tfs.transform.translation.y = pose->y; tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0 // |--------- 四元数设置 tf2::Quaternion qtn; qtn.setRPY(0,0,pose->theta); tfs.transform.rotation.x = qtn.getX(); tfs.transform.rotation.y = qtn.getY(); tfs.transform.rotation.z = qtn.getZ(); tfs.transform.rotation.w = qtn.getW(); // 5-3.广播器发布数据 broadcaster.sendTransform(tfs); } int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"dynamic_tf_pub"); // 3.创建 ROS 句柄 ros::NodeHandle nh; // 4.创建订阅对象 ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose); // 5.回调函数处理订阅到的数据(实现TF广播) // // 6.spin ros::spin(); return 0; }
3.订阅方
//1.包含头文件 #include "ros/ros.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" #include "geometry_msgs/PointStamped.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件 int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"dynamic_tf_sub"); ros::NodeHandle nh; // 3.创建 TF 订阅节点 tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer); ros::Rate r(1); while (ros::ok()) { // 4.生成一个坐标点(相对于子级坐标系) geometry_msgs::PointStamped point_laser; point_laser.header.frame_id = "turtle1"; point_laser.header.stamp = ros::Time(); point_laser.point.x = 1; point_laser.point.y = 1; point_laser.point.z = 0; // 5.转换坐标点(相对于父级坐标系) //新建一个坐标点，用于接收转换结果 //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------ try { geometry_msgs::PointStamped point_base; point_base = buffer.transform(point_laser,"world"); ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z); } catch(const std::exception& e) { // std::cerr << e.what() << '\n'; ROS_INFO("程序异常:%s",e.what()); } r.sleep(); ros::spinOnce(); } return 0; }
5 TF2与TF
5.1 TF2与TF比较_简介
TF2已经替换了TF，TF2是TF的超集，建议学习 TF2 而非 TF
TF2 功能包的增强了内聚性，TF 与 TF2 所依赖的功能包是不同的，TF 对应的是tf包，TF2 对应的是tf2和tf2_ros包，在 TF2 中不同类型的 API 实现做了分包处理。
TF2 实现效率更高，比如在:TF2 的静态坐标实现、TF2 坐标变换监听器中的 Buffer 实现等
5.2 TF2与TF比较_静态坐标变换演示
接下来，我们通过静态坐标变换来演示TF2的实现效率。
5.2.1 启动 TF2 与 TF 两个版本的静态坐标变换
TF2 版静态坐标变换:rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 /base_link /laser
TF 版静态坐标变换:rosrun tf static_transform_publisher 0 0 0 0 0 0 /base_link /laser 100
会发现，TF 版本的启动中最后多一个参数，该参数是指定发布频率
5.2.2 运行结果比对
使用rostopic查看话题，包含/tf与/tf_static, 前者是 TF 发布的话题，后者是 TF2 发布的话题，分别调用命令打印二者的话题消息
rostopic echo /tf: 当前会循环输出坐标系信息
rostopic echo /tf_static: 坐标系信息只有一次
5.3 结论
如果是静态坐标转换，那么不同坐标系之间的相对状态是固定的，既然是固定的，那么没有必要重复发布坐标系的转换消息，很显然的，tf2 实现较之于 tf 更为高效
