编写一个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发送一个变换需要四个参数。

  1. 首先,我们通过转换本身。
  2. 现在我们需要给发布的变换一个时间戳,我们会用当前时间ros :: Time :: now()来标记它

  3. 然后,我们需要传递我们创建的链接的父框架的名称,在这种情况下,“world”

  4. 最后,我们需要传递我们正在创建的链接的子框架的名称,在这种情况下,这是乌龟本身的名称。

注意: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>



评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值