一. 工作空间1. 创建工作空间创建的步骤可以看前面的文章工作空间命名为sor_ws 【ROS入门】3. 创建工作区间与功能包2.创建功能包cd ~/sor_ws/src catkin_create_pkg learning_tf rospy roscpp tf turtlesimlearning_tf目录下生成以下文件及文件夹二、实现tf广播器首先创建turtle_tf_broadcaster.cpp文件cd ~/sor_ws/src/learning_tf/src touch turtle_tf_broadcaster.cpp实现一个tf广播器的步骤定义TF广播器创建坐标变换值发布坐标变换/*********************************************************************** Copyright 2020 GuYueHome (www.guyuehome.com). ***********************************************************************/ /** * 该例程产生tf数据并计算、发布turtle2的速度指令 */ #include ros/ros.h #include tf/transform_broadcaster.h #include turtlesim/Pose.h std::string turtle_name; void poseCallback(const turtlesim::PoseConstPtr msg) { // 创建tf的广播器 static tf::TransformBroadcaster br; // 初始化tf数据 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); // 广播world与海龟坐标系之间的tf数据 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), world, turtle_name)); } int main(int argc, char** argv) { // 初始化ROS节点 ros::init(argc, argv, my_tf_broadcaster); // 输入参数作为海龟的名字 if (argc ! 2) { ROS_ERROR(need turtle name as argument); return -1; } turtle_name argv[1]; // 订阅海龟的位姿话题 ros::NodeHandle node; ros::Subscriber sub node.subscribe(turtle_name/pose, 10, poseCallback); // 循环等待回调函数 ros::spin(); return 0; };首先程序要注意头文件的书写能够是海龟正常运行不管是海龟turtle1或海龟turtle2与word坐标系之间的位置关系都是通用的程序在程序中通过输入参数确定当前小海龟的名称并且去跟word坐标系去建立关系因此程序会运行两遍但是我们需要注意的是任意的一个ros节点它的节点名在整个ros环境中只能有一个如果程序运行两遍会因为节点名字一样冲突因此用到了重映射。后面会详细说明程序中将创建一个tf广播器初始化tf数据广播word与海龟坐标系之间的tf数据时需要设置一个时间戳(一般为秒)。三.实现tf监听器首先创建turtle_tf_listener.cpp文件cd ~/sor_ws/src/learning_tf/src touch turtle_tf_listener.cpp实现一个TF监听器的步骤定义TF监听器查找坐标变换/*********************************************************************** Copyright 2020 GuYueHome (www.guyuehome.com). ***********************************************************************/ /** * 该例程监听tf数据并计算、发布turtle2的速度指令 */ #include ros/ros.h #include tf/transform_listener.h #include geometry_msgs/Twist.h #include turtlesim/Spawn.h int main(int argc, char** argv) { // 初始化ROS节点 ros::init(argc, argv, my_tf_listener); // 创建节点句柄 ros::NodeHandle node; // 请求产生turtle2 ros::service::waitForService(/spawn); ros::ServiceClient add_turtle node.serviceClientturtlesim::Spawn(/spawn); turtlesim::Spawn srv; add_turtle.call(srv); // 创建发布turtle2速度控制指令的发布者 ros::Publisher turtle_vel node.advertisegeometry_msgs::Twist(/turtle2/cmd_vel, 10); // 创建tf的监听器 tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()) { // 获取turtle1与turtle2坐标系之间的tf数据 tf::StampedTransform transform; try { 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的速度控制指令 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; };通过tf监听器去监听turtle1和turtle2之间的坐标系关系根据turtle与turtle2坐标之间的关系发布turtle2的速度控制指令。四.配置编译规则在learning_tf文件夹下双击打开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})五.运行1.编译cd ~/sor_ws catkin_make编译完成。2.运行1打开新终端输入roscore2再打开一个新终端启动可视化海龟界面rosrun turtlesim turtlesim_node3发布海龟1与world的坐标系关系source ~/sor_ws/devel/setup.bash rosrun learning_tf turtle_tf_broadcaster __name:turtle1_tf_broadcaster /turtle14发布海龟2与world的坐标系关系source ~/sor_ws/devel/setup.bash rosrun learning_tf turtle_tf_broadcaster __name:turtle2_tf_broadcaster /turtle25创建海龟2发布速度指令然后让它跟着海龟1运动source ~/sor_ws/devel/setup.bash rosrun learning_tf turtle_tf_listener6运行指令用键盘上的上下左右按键来控制海龟1运动海龟2会跟随海龟1运动rosrun turtlesim turtle_teleop_key