在机器人系统中，我们经常会遇到坐标变换相关的问题。比如说，在一个导航系统中，我们通过激光雷达、双目摄像头等传感器对周围环境进行感知，但是这些传感器直接采集的数据是相对于传感器而言的， 而传感器在机器人身上的安装位置又各不相同，我们关心的则是外部环境与机器人本体之间的关系，所以还需要把相对于传感器的数据变换成为相对于机器人的数据。这个过程就是所谓的坐标变换。

如果涉及到多个机器人之间的协同工作，则需要维护两两之间的相对位置关系，在机器人很多的情况下，或者说是自由度很多的情况下，这种坐标系之间的变换关系就相对比较复杂。 ROS系统提供了一个叫做tf的坐标变换库，这个库使得我们能够在各种坐标系之间自由地切换。

在ROS中有一个turtle的例子，一个通过键盘控制turtle1跑，另外一个turtle2跟在turtle1后面，跟随其坐标进行跟踪。

这是：官方例子

这一过程就是通过tf来实现的。一个tf系统由tf广播者(tf broadcaster)和tf监听者(tf listener)两个部分构成的。这里的白龟(turtle1)和红龟(turtle2)就是两个tf广播者，它们不断的向外发送自己相对于世界坐标系的位置(也就是自己坐标系的原点在世界坐标系下的位置)。这里的是广播，所以所有的都会接收到他们发送的。
另外有一个监听者负责维护两个乌龟的坐标系，通过tf的API系统计算白龟(turtle1)相对红龟(turtle2)的位置，并控制红龟(turtle2)跟踪白龟(turtle1)。为了跟随，就需要知道turtle1相对于turtle2的坐标位置，也就是说turtle1的坐标原点在turtle2下的位置，也就是说turtle1在turtle2下的位置

官方broadcaster例子C++版

创建功能包及其依赖
catkin_create_pkg learning_tf tf roscpp rospy turtlesim

broadcaster.cpp，我们发布的节点就是通过这个来广播自己在世界坐标系的位置

//这里有我们访问master的接口
#include <ros/ros.h>     //提供了tf广播的各种API 后面的listen就是根据这个来的
#include <tf/transform_broadcaster.h>   //turtlesim是一个依赖包，里面有将要订阅pose主题的消息类型
#include <turtlesim/Pose.h>std::string turtle_name;//这里的const turtlesim::PoseConstPtr& msg 是一个套路，目前还不知道这个
//是啥意思？
void poseCallback(const turtlesim::PoseConstPtr& msg){//tf广播器，发布话题相当于定义一个发布器static tf::TransformBroadcaster br;/**根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换** 位姿就是海龟仿真器发出的，也就是/sim 这个sim就是海龟仿真器 <node pkg="turtlesim" type="turtlesim_node" name="sim"/>**这里启动时的名字**///定义存放转换信息(平动 转动)的变量tf::Transform transform;/**设置坐标原点**setOrigin()函数的参数类型需要为tf::Vector3类型**(msg->x, msg->y, 0.0) 为turtle的坐标原点在world世界下的坐标/transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );//定义旋转tf::Quaternion q;//setRPY函数的参数为turtle在world坐标系下的roll,pitch,yawq.setRPY(0, 0, msg->theta);transform.setRotation(q);/**br有成员函数send**  这是 Transform自带的** 第一个参数是坐标变换内容，第二个参数是时间戳** world 和 turtle_name 表示本次坐标变换相对于世界坐标系的变换**这里world就是父坐标系 turtle_name就是子坐标系**同时，这里子坐标系有两个实际上 turtle1 turtle2所以这里取了个变量**父坐标系只有一个，就是world*/br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}int main(int argc, char** argv){//初始化一个节点，名字叫做my_tf_broadcasterros::init(argc, argv, "my_tf_broadcaster");/**输入参数有两个类似于这个:rosrun turtlesim turtle1  这个经过测试不行**但是，rosrun turtlesim turtlsim_node 却是可以，不知道这里为什么**所以，这里输入参数是2个，第二个参数就是turtle_name/if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};turtle_name = argv[1];ros::NodeHandle node;/**这里turtle_name 就是：turtle1, turtle2**这里&poseCallback 前面可以不用加&/ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);//循环等待ros::spin();return 0;
};

官方listener例子C++版

turtle_tf_listener.cpp

#include <ros/ros.h>
//这里跟前面的不一样，前面是tf/transform_broadcaster.h
#include <tf/transform_listener.h>  
#include <geometry_msgs/Twist.h>   //坐标变换好像要用到这个
#include <turtlesim/Spawn.h>int main(int argc, char** argv){ros::init(argc, argv, "my_tf_listener");ros::NodeHandle node;/****这段就是在图中生成一个turtle,这里猜测，启动的时候已经启动了一个turtle1**但是，还需要一个turtle2,所以这里生成一个**但是，经过测试发现，这里的话，如果在后面设置的roslaunch 里面没有写的话**那么就是只是生成一个turtle，而这个节点是没有启动的，那就更加不用说控制了**同理，如果只是在roslaunch里面写了节点，这里不写的话，那么rosnode 里面会有**但是，图中不会显示乌龟*/ros::service::waitForService("spawn");ros::ServiceClient add_turtle =node.serviceClient<turtlesim::Spawn>("spawn");turtlesim::Spawn srv;add_turtle.call(srv);//前面是生成一个turtle2，然后我们要发布cmd_vel速度控制指令来控制ros::Publisher turtle_vel =node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);//在程序中具现化监听器对象。只要创建了这个对象，它就开始监听系统中的各个坐标变换，并将之缓存10秒。这里也就是存放变换关系的变量tf::TransformListener listener;ros::Rate rate(10.0);while (node.ok()){tf::StampedTransform transform;try{/**通过Transform的API 查询turtle1相对于turtle2的坐标变换**跟前面一样，就是turtle1的原点在turtle2下的位置 **第三个参数'ros::Time(0)'则指获取当前时刻的坐标关系。 最后一个参数**tranform则记录了具体的变换内容，它是一个tf::StampedTransform对象。**且这里tf会把监听的内容放到一个缓存中，然后再读取相关的内容。而这个过程会有几毫秒的误差.所以这里不能像先前的那样，使用 ros::Time::now()*/listener.lookupTransform("/turtle2", "/turtle1",ros::Time(0), transform);}catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}/**监听到这两个的坐标变换后，得到turtle2关于turtle1的坐标变换后**就可以计算turtle2的速度了，并发布速度指令控制其运动**下面的这个还不知道是怎么计算的*/geometry_msgs::Twist vel_msg;vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +pow(transform.getOrigin().y(), 2));turtle_vel.publish(vel_msg);rate.sleep();}return 0;
};

最后，在learning_tf下新建launch文件turtle_tf_demo.launch

  <launch><!-- 启动海龟仿真，光这一个就可以生成一个海龟turtle1--><node pkg="turtlesim" type="turtlesim_node" name="sim"/><!-- 启动键盘控制海龟运动--><node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/><!-- Axes --><param name="scale_linear" value="2" type="double"/><param name="scale_angular" value="2" type="double"/><!-- 启动两个turtle的广播-->
<!-- pkg是报名,type是boardcaster.cpp编译CMakeLists里面编译的可执行文件的名字-->
<!-- args就是传递的参数-->
<!-- name就是运行时节点的名字，本来一开始ros::init里面有名字，这里会把那个覆盖-->
<!-- 但是，这里name也是必须的，不能删除，同时,ros::init初始化那里也不能删除
感觉就是一个占位的，反正这里也会把它进行覆盖--><node pkg="learning_tf" type="turtle_tf_broadcaster"args="/turtle1" name="turtle1_tf_broadcaster" /><node pkg="learning_tf" type="turtle_tf_broadcaster"args="/turtle2" name="turtle2_tf_broadcaster" /><!--启动监听节点，观察发现，这里就没有args参数，是因为这里不需要进行参数的传递前面是需要得到turtle在世界坐标系的位置并发布出去需要传递turtle的名字进去--><node pkg="learning_tf" type="turtle_tf_listener"name="listener" /></launch>

最后在CMakeLists.txt中添加

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

我的博客地址： github