T t u r t l e 1 _ t u r t l e 2 = T t u r t l e 1 _ w o r l d × T w o r l d _ t u r t l e 2 T_{\}=T_{\}\times T_{world\ } ?=?×?
使用 工具在TF树中查找乌龟坐标系之间的变换关系：
rosrun tf tf_echo turtle1 turtle2
图4-5 乌龟坐标系之间的变换关系
也可以通过rviz的图形界面更加形象地看到这三者之间的坐标关系：
rosrun rviz rviz -d 'rospack find turtle_tf' //rviz/turtle_rviz.rviz
在打开的rviz界面中 ， 将 选项下的Fixed Frame后面的坐标系改成world ， 然后通过左下角add按键 ， 在Bytype下面找到TF并添加 。即可显示两只乌龟和世界坐标系之间的坐标关系 。
在得到与之间的坐标变换后 ， 就可以计算两只乌龟间的距离和角度 ， 即可控制向移动了 。
接下来 ， 我们以这个例程为目标 ， 学习如何实现TF的广播和监听功能 。
4.2.4 创建TF广播器
首先 ， 我们需要创建一个发布乌龟坐标系与世界坐标系之间TF变换的节点 ， 实现源码///src/r.cpp如下：
#include #include #include std::string turtle_name;void poseCallback(const turtlesim::PoseConstPtr& msg){// tf广播器static tf::TransformBroadcaster br;// 根据乌龟当前的位姿 ， 设置相对于世界坐标系的坐标变换tf::Transform transform;transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );tf::Quaternion q;q.setRPY(0, 0, msg->theta);transform.setRotation(q);// 发布坐标变换br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));}int main(int argc, char** argv){// 初始化节点ros::init(argc, argv, "my_tf_broadcaster");if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};turtle_name = argv[1];// 订阅乌龟的pose信息ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);ros::spin();return 0;};
以上代码的关键部分是处理乌龟pose消息的回调函数 ， 在广播TF消息之前需要定义tf::广播器 ， 然后根据乌龟当前的位姿设置tf::类型的坐标变换 ， 包含设置的平移变换以及设置的旋转变换 。
然后使用广播器将坐标变换插入TF树并进行发布 ， 这里发布的TF消息类型是tf:: ， 不仅包含tf::类型的坐标变换、时间戳、而且需要指定坐标变换的源坐标系（）和目标坐标系（child） 。
4.2.5 创建TF监听器
TF消息广播之后 ， 其他节点就可以监听该TF消息了 ， 从而获取需要的坐标变换了 。目前我们已经将乌龟相对于world坐标系的TF变换进行了广播 ， 接下来需要监听TF消息 ， 并从中获取相对于坐标系的变换 ， 从而控制移动 。实现源码///src/.cpp的详细内容如下：
#include #include #include #include int main(int argc, char** argv){// 初始化节点ros::init(argc, argv, "my_tf_listener");ros::NodeHandle node;// 通过服务调用 ， 产生第二只乌龟turtle2ros::service::waitForService("spawn");ros::ServiceClient add_turtle =node.serviceClient("spawn");turtlesim::Spawn srv;add_turtle.call(srv);// 定义turtle2的速度控制发布器ros::Publisher turtle_vel =node.advertise("turtle2/cmd_vel", 10);// tf监听器tf::TransformListener listener;ros::Rate rate(10.0);while (node.ok()){tf::StampedTransform transform;try{// 查找turtle2与turtle1的坐标变换listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);}catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}// 根据turtle1和turtle2之间的坐标变换 ， 计算turtle2需要运动的线速度和角速度// 并发布速度控制指令 ， 使turtle2向turtle1移动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;};