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>