介绍这两只海龟跟随背后的原理,怎样通过tf坐标系来完成广播与监听的编程实现
第一步,创建功能包learning_tf
cd ~/catkin_ws/src 在工作空间src下进入终端
catkin_create_pkg learning_tf roscpp rospy tf turtlesim
这里我们需要去依赖roscpp,rospy,tf工具,还有和小海龟相关的turtlesim功能包
我们看到两个海海龟与world之间都有位置关系,这个位置关系要通过tf去广播出来的,看一下在程序里如何来实现
首先看一下如何通过tf来去广播任意两个坐标系之间的位置关系,称之为tf的广播器
第二步,创建tf广播器代码(C++)
将课件代码/learning_tf/src里的turtle_tf_broadcaster.cpp代码文件放置于catkin_ws/src/learning_tf/src里
turtle_tf_broadcaster.cpp代码文件内容
/**
* 该例程产生tf数据,并计算、发布turtle2的速度指令
*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>//包括tf变换的一个broadcaster
#include <turtlesim/Pose.h> //包括海龟的位置,它是通过海龟仿真器turtlesim不断地往外去发布的,和前面去创建监听者去监听海龟的位置subscribe关系是一样,这里也是有一个subscribe来监听turtlesim里面海龟的实时位置的
std::string turtle_name;
//在函数里会传入海龟的位置,这个位置是海龟在整个坐标系下的X,Y坐标还有#theta(希腊字母)
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// 创建tf的广播器,实例TransformBroadcaster
//通过这个实例把海龟1或者海龟2相对于world坐标系的位置关系发出去
static tf::TransformBroadcaster br;
// 初始化tf数据
//填充坐标系之间的映射关系;以turtle1为例,turtle1与world两个坐标系之间的位置关系包括平移和旋转,所以这两个数据组成了坐标变换transform;transform即是在机器人导论里学到的4×4矩阵数据,在transform里填数据:首先是平移即通过transform里的setOrigin来设置平移参数,包括围绕X,Y,Z三轴的平移;通过海龟的位置关系可以得到turtle1相对于world的X轴平移,y轴平移,由于是平面的所以z轴平移永远是0,他们组成向量Vector3,所以我们通过setOrigin把平移关系给设进来;;接下来设置旋转,通过Quaternion即四元数,通过setRPY即围绕X,Y,Z轴三轴的旋转可以设置姿态,这里X,Y方向都是0,只有Z轴有旋转关系,所以通过setRPY完成了两个坐标系之间姿态变化的数据的设置,然后通过setRotation把它设置到transform变量里面来;transform到目前为止,就保存了平移保存了旋转的关系,整个两个坐标系之间的位置关系就可以完全的描述出来,接下来通过Broadcaster br,通过sendTransform这样一个方法就可以把位置关系去发布广播出去,之后ros后台TF树就会把这两个坐标系的关系插入到对应的树中,上节看到的树就是这里的sendTransform广播之后变到树里来的;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
// 广播world与海龟坐标系之间的tf数据
//添加一个时间戳,知道TF是有一个时间的概念的,默认可以保存10秒钟之内,ros::Time::now()表示当前时间;描述的是"world"和turtle_name(就是turtle1和turtle2)两个坐标系之间的位置关系,transform就是前面描述的这两个坐标系之间的位置关系。所以这里通过sendTransform广播出去了两个坐标系是谁,关系怎么样,以及两个坐标系关系是哪个时刻的,因为两个坐标系可能会发生变化;通过这句话后,TF树终究会出现world和turtle1或turtle2的位置关系了
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_broadcaster");
// 输入参数作为海龟的名字
//判断输入参数,因为不管是海龟1还是海龟2,他和world之间的位置关系都是通用的一个程序不需要写两遍,所以通过main函数的输入参数来确定当前的这个程序执行的是turtle1还是turtle2,前者则建立前者和world的关系,后者则建立后者与world的关系。但注意,任意的ROS节点它的节点名在整个ROS环境当中只能有一个,想象一下如果这个程序执行两遍的话肯定会有节点名冲突的问题,后面会发现想办法让节点名换一个名字,这样程序就可以执行两遍了,在ROS中成为重映射
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
//在这获取海龟的名字,argv[1]是字符串
turtle_name = argv[1];
// 订阅海龟的位姿话题
ros::NodeHandle node; //创建一个句柄,通过句柄来创建一个话题subscribe的订阅者,来订阅海龟仿真器里不断在发布的海龟位置信息turtle_name+"/pose",记得在turtlesim里默认发布的是turtle1/pose这样一个话题,如果是海龟2的话就是turtle2/pose,所以这里的turtle_name不是turtle1就是turtle2,和后面的pose接到一起就直接订阅仿真器里海龟的位置了,如果一旦有海龟位置在发布的话就会跳到回调函数poseCallback里来
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
// 循环等待回调函数
//spin会不断在等待队列的数据,一旦有数据进来就会进到poseCallback
ros::spin();
return 0;
};
第二步,创建tf监听器代码(C++) 8.59