【ROS】官方tf教程turtle_tf2源码原理解读

最新推荐文章于 2023-12-01 22:56:43 发布

Wzt_Robot_Dream

最新推荐文章于 2023-12-01 22:56:43 发布

阅读量1.2k

点赞数 1

文章标签：大数据

本文链接：https://blog.csdn.net/weixin_43171162/article/details/125470636

版权

程序需要沉淀沉淀再沉淀！！！

前言

主要是学习tf坐标变换时候太吃力了，因此先学习官方给的小乌龟跟随这里的tf坐标变换。

直接上源码吧！！！turtle_tf2_broadcaster.cpp 内容如下:

先从主函数开始吧：

35行：ros::init(argc, argv, "my_tf2_broadcaster");这一句就不说了，稍微学过ros的就知道是啥了。

37行：ros::NodeHandle private_node("~");创建一个节点的句柄，用来参数服务器语句的判断。此处和我们定义node的方式不一样。以往我们定义node基本都是直接ros::NodeHandle nh。这里咋就多了一个参数～呢？；表示这个nodehandle是读取局部参数。

38行：if (! private_node.hasParam("turtle"))；判断参数服务器中是否存在turtle这个参数，如果没有，则执行else语句。

先看if语句里面的内容：if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};判断argc里面的个数是否为2，如果不为2，那么打印ROS_ERROR("need turtle name as argument");并退出节点。如果argc==2，那么将argv[1]赋值给turtle_name，这里有同学可能会问，为什么不讲argv[0]里面的值给到turtle_name；是因为不管有没有参数输入进来，argv[0]永远都是可执行程序的名字。

48行：private_node.getParam("turtle", turtle_name);就是将参数服务器里面turtle的值赋值给到turtle_name这个字符串（第7行定义了turtle_name是一个字符串类型的数据）。

51行：ros::NodeHandle node;创建一个节点句柄，用来订阅回调函数poseCallback。

poseCallback

54行：ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);// 第一个参数指定订阅话题topic；第二个参数设置消息缓冲区的大小；第三个参数指定回调函数。这里需要注意的是turtle_name是一个变量；turtle_tf2_broadcaster_cpp.launch文件里面有说明，最后再讲。

57行：ros::spin();这个就不说了。

接下来看回调函数poseCallback：

13行：static tf2_ros::TransformBroadcaster br;// 定义一个广播TransformBroadcaster,将坐标变换通过话题发布出去，这里为什么是用static，是为了防止重复，因为turtle_tf2_listener.cpp也用到了。

14行：geometry_msgs::TransformStamped transformStamped;实例化transformStamped结构体

16-21行：

    transformStamped.header.stamp = ros::Time::now();
   transformStamped.header.frame_id = "world";
   transformStamped.child_frame_id = turtle_name;
   transformStamped.transform.translation.x = msg->x;
   transformStamped.transform.translation.y = msg->y;
   transformStamped.transform.translation.z = 0.0;

ros::Time::now();表示ros里面当前的时间；world是父坐标；turtle_name是子坐标。

具体两个坐标之间的关系是怎么样的呢？自然就涉及到给坐标系之间的rotation和translation赋值。我们说回调函数接收到的是turtle的位置和方向，那么msg中的位置和方向我们就需要赋值transformStamped。先赋值 translation，后赋值 rotation 。19-20行这里将x轴y轴的坐标赋值给了transformStamped。这里的理解讲得很清楚了。

23行：tf2::Quaternion q; //头文件部分包含了#include <tf2/LinearMath/Quaternion.h>，代码有一行定义了tf::quaternion的对象tf::Quaternion q需要通过包含这个头文件来实现。

24行：q.setRPY(0, 0, msg->theta);调用了Quaternion中setRPY函数来获取角度值。

25-28行：这里是获取角度，以四元数的形式来获取。

30行：br.sendTransform(transformStamped);将获取的transformStamped发送出去，主函数来订阅。

最后讲一下turtle_tf2_broadcaster_cpp.launch里面的内容吧：