ROS::TF2教程中的例程详解：Writing a tf2 broadcaster (C++)

最新推荐文章于 2024-07-12 14:39:22 发布

Hill0227

最新推荐文章于 2024-07-12 14:39:22 发布

阅读量819

点赞数

分类专栏： ROS TF 文章标签： linux

本文链接：https://blog.csdn.net/weixin_53266173/article/details/110943283

版权

ROS 同时被 2 个专栏收录

3 篇文章 0 订阅

订阅专栏

3 篇文章 1 订阅

订阅专栏

原教程链接：
Writing a tf2 broadcaster (C++)

注释如下：

#include <ros/ros.h>
//线型数学，四元数
#include <tf2/LinearMath/Quaternion.h>
//广播支持，提供tf2_ros::TransformBroadcaster类
#include <tf2_ros/transform_broadcaster.h>
//坐标系变换支持
#include <geometry_msgs/TransformStamped.h>
//位置变量
#include <turtlesim/Pose.h>

std::string turtle_name;

//回调函数：
//主要用途是获得turtle的位姿，并将turtle的位姿进行广播从而发布到TF2。
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
  //广播对象
  static tf2_ros::TransformBroadcaster br;
  //声明坐标变换消息
  geometry_msgs::TransformStamped transformStamped;
  //填充消息
  //用3D龟龟的位姿填充位姿消息
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "world";
  transformStamped.child_frame_id = turtle_name;
  //位移部分来自输入消息的x,y
  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
  //旋转的艏向部分来自输入消息中的theta
  q.setRPY(0, 0, msg->theta);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();
  //发送变换消息
  br.sendTransform(transformStamped);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_tf2_broadcaster");

  ros::NodeHandle private_node("~");
  if (! private_node.hasParam("turtle"))
  {
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); return -1;
    };
    turtle_name = argv[1];
  }
  else
  {
    private_node.getParam("turtle", turtle_name);
  }
    
  ros::NodeHandle node;
  //订阅turtle_name-node的"pose"消息，队列长度10，
  //当名为turtle_name+"/pose"的消息发布的时候会调用回调函数：“&poseCallback”
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

  ros::spin();
  return 0;
};