编写一个tf广播

tf broadcaster

   1 #include <ros/ros.h>   2 #include <tf/transform_broadcaster.h>//专用头文件   3 #include <turtlesim/Pose.h>   4    5 std::string turtle_name;   6    7    8    9 void poseCallback(const turtlesim::PoseConstPtr& msg){  10   static tf::TransformBroadcaster br;//穿件transform broadcaster对象  11   tf::Transform transform;  12   transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );  13   tf::Quaternion q;  14   q.setRPY(0, 0, msg->theta);//创建一个transform对象，将消息从2d 乌龟位姿赋值到3d转换中  15   transform.setRotation(q);//设置旋转  16   br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));  17 }  18 
//这是真正的工作完成的地方。用TransformBroadcaster发送一个变换需要四个参数。
首先，我们通过转换本身。
现在我们需要给发布的变换一个时间戳，我们会用当前时间ros :: Time :: now（）来标记它。
然后，我们需要传递我们创建的链接的父框架的名称，在这种情况下，“world”
最后，我们需要传递我们正在创建的链接的子框架的名称，在这种情况下，这是乌龟本身的名称。
注意：sendTransform和StampedTransform具有父母和子女相反的顺序。
  19 int main(int argc, char** argv){
  20   ros::init(argc, argv, "my_tf_broadcaster");  21   if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};  22   turtle_name = argv[1];  23   24   ros::NodeHandle node;  25   ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);  26   27   ros::spin();  28   return 0;  29 };
CMakeList.txt
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

launch文件 start_demo.launch
  <launch>    <!-- Turtlesim Node-->    <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"/>    <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" />  </launch>