文章目录1、前言2、程序2.1、新建功能包2.2、广播tf程序2.3、监听tf程序2.4、配置CMakeLists3、编译运行3.1、回到工作空间目录下进行编译3.2、运行1、前言实现tf广播器思路定义TF广播器TransformBroadcaster创建坐标变换值发布坐标变换sendTransform实现tf监听器思路定义tf监听器TransformListener查找坐标变换waitForTransForm 、lookupTransform2、程序2.1、新建功能包到src目录下新建一个功能包cd src catkin_create_pkg demo_tf roscpp rospy tf turtlesim2.2、广播tf程序在src目录下新建tf.broadcaste.cpp代码如下#includeros/ros.h#includetf/transform_broadcaster.h#includeturtlesim/Pose.hstd::string turtle_name;voidposeCallback(constturtlesim::PoseConstPtrmsg){//创建tf的广播器statictf::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);//广播word与海龟坐标之间的tf数据br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),world,turtle_name));}intmain(intargc,char**argv){//初始化ROS节点ros::init(argc,argv,my_tf_broadcaster);//输入参数作为海龟的名字if(argc!2){ROS_ERROR(need turtle name as argument);return-1;}turtle_nameargv[1];//订阅海龟的位置话题ros::NodeHandle node;ros::Subscriber subnode.subscribe(turtle_name/pose,10,poseCallback);//死循环等待回调函数ros::spin();return0;}2.3、监听tf程序在src目录下新建tf_listener.cpp#includeros/ros.h#includetf/transform_listener.h#includegeometry_msgs/Twist.h#includeturtlesim/Spawn.hintmain(intargc,char**argv){//初始化ROS节点ros::init(argc,argv,tf_listener);//创建节点句柄ros::NodeHandle node;//请求产生turtle2ros::service::waitForService(/spawn);ros::ServiceClient add_turtlenode.serviceClientturtlesim::Spawn(/spawn);turtlesim::Spawn srv;add_turtle.call(srv);//创建发布turtle2速度控制指令的发布者ros::Publisher turtle_velnode.advertisegeometry_msgs::Twist(/turtle2/cmd_vel,10);//创建tf的监听器tf::TransformListener listener;ros::Raterate(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(conststd::exceptione){ROS_ERROR(%s,e.what());ros::Duration(1.0).sleep();continue;}//根据turtle1与turtle2坐标系之间的位置关系发布turtle2的速度控制指令geometry_msgs::Twist vel_msg;vel_msg.angular.z4.0*atan2(transform.getOrigin().y(),transform.getOrigin().x());vel_msg.linear.x0.5*sqrt(pow(transform.getOrigin().x(),2)pow(transform.getOrigin().y(),2));turtle_vel.publish(vel_msg);rate.sleep();}return0;}2.4、配置CMakeListsadd_executable(tf_broadcaster src/tf_broadcaster.cpp)target_link_libraries(tf_broadcaster${catkin_LIBRARIES})add_executable(tf_listener src/tf_listener.cpp)target_link_libraries(tf_listener${catkin_LIBRARIES})3、编译运行3.1、回到工作空间目录下进行编译cd..catkin_make编译成功后检查是否出现可执行文件3.2、运行这里我们需要打开多个终端终端1运行roscoreroscore终端2运行海龟模拟器节点rosrun turtlesim turtlesim_node终端3改变环境变量并运行tf广播代码source devel/setup.bash rosrun demo_tf tf_broadcaster __name:turtle1_tf_broadcaster/turtle1终端4改变环境变量并再次运行tf广播代码source devel/setup.bash rosrun demo_tf tf_broadcaster __name:turtle2_tf_broadcaster/turtle2值得注意的是因为这里运行的是同一个节点代码防止节点名冲突这里用到了ros的重映射机制。终端5改变环境变量并运行tf监听代码source devel/setup.bash rosrun demo_tf tf_listenser回车运行后就出现了第二只海龟并进行跟随终端6这时我们还可以运行海龟模拟器的键盘控制节点控制一只海龟让另外一只跟随。