编写一个tf广播

最新推荐文章于 2024-07-22 16:02:15 发布

追风筝的鱼

最新推荐文章于 2024-07-22 16:02:15 发布

阅读量814

点赞数

分类专栏： ros程序记录文章标签： ros

ros程序记录专栏收录该内容

10 篇文章

订阅专栏

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>