对于ros中的tf其实一直理解不是很深,最近工作上一直在用,就很懵逼,出来混果然是要还的~于是这两天把ros官方提供的小乌龟版的你追我,如果你追到我,我就让你xxx,一个一个命令跑了一下,现在有了一个较深的理解,如果哪里说的不对,还请指正。
首先看launch文件内容
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<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" />
<node pkg="learning_tf" type="turtle_tf_listener"
name="listener" />
</launch>
分析一下,首先启动了turtlesim的节点,然后是键盘控制,这里有一点需要注意一下,至少我是一直没搞得很清楚
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
转化为命令行即为:
rosrun turtlesim turtlesim_node __name:=sim
最后那个 __name:=sim 即给节点起名为sim,如果不改名的话,就是你代码里起的名字了,但是需要注意的是因为在例程中因为要启动 /turtle1 和 /turtle2,如果不起名的话,第二只就把第一只干掉了,所以起名还是很重要的~
然后就是启动两只乌龟的learning_tf_broadcaster,注意这里启动完之后,就会有第一只和第二只和世界坐标系之间的关系,当然我们也可以定义第三只第四只乌龟。启动三只乌龟后的tf图如下所示:
这时候,其实,三只乌龟以及世界坐标系的关系就都可以相互转换了,比如第二只要去追第一只,得到第二只相对第一只的transform就可以求啦~同理,第三只追第二只,第三只追第一只,以及第n只追第k只都可以定义啦~
listener.waitForTransform("/turtle3", "/turtle2", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle3", "/turtle2", ros::Time(0), transform3);
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform2);
然后分别发布到 “turtle2/cmd_vel” “turtle3/cmd_vel” 再加上键盘发布 “turtle1/cmd_vel” 的消息,于是三只乌龟就开始了愉快的,你追我的小游戏。乌龟的世界太疯狂,而且追上了还在上面扭来扭去的,咳咳咳…
对了,记得也要调用服务生成乌龟3,原来代码里只生成了乌龟2。
补充一些关于广播tf和接收tf的内容
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// tf广播器
static tf::TransformBroadcaster br;
// 根据乌龟当前的位姿,设置相对于世界坐标系的坐标变换
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);
// 发布坐标变换
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
对于广播tf的话,回调函数传入的是turtle1/pose,会发现只调用了一次这个回调函数,也即为了初始化。
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
首先setOrigin为turtle1当前的x,y坐标,也即把自己当前的坐标认为自己在全局坐标系下的坐标,以此来确定父坐标系的原点。同理,设定父坐标系的方向,自己当前的方向认为是自己相对于父坐标系的方向。然后发布出去。
对于接收者,只需要
listener.waitForTransform("/turtle3", "/turtle2", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle3", "/turtle2", ros::Time(0), transform3);
就可以得到转换以后的关系啦~
突然奇想,如果两个乌龟定义的世界坐标系不一样会怎么样呢,于是改了改试了一下~
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
所有的乌龟setOrigin都是这么设置的,那么如果第二个乌龟的世界坐标系改一下呢,于是改成了
transform.setOrigin( tf::Vector3(msg->x + 1, msg->y + 1, 0.0) );
即第二个乌龟的世界原点是(1,1),不是(0,0),也即第二个乌龟应该都会滞后第一个乌龟(1,1),第一个乌龟在(5,5)的话,在第二个乌龟看来,就是在(4,4),跑一下看看效果~
果然是这样子的~
然后官网上有一个添加一个carrot框架到现有的坐标系下的例子,代码是酱紫的~
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
ros::NodeHandle node;
tf::TransformBroadcaster br;
tf::Transform transform;
ros::Rate rate(10.0);
while (node.ok()){
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
rate.sleep();
}
return 0;
};
跟broadcaster很像是不是,这里的话,尤其要注意一下
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
相当于设置turtle1这个动坐标系认为是父坐标系,然后以父坐标系向右偏移两个单位为carrot坐标系的原点,然后carrot坐标系就要以这个动坐标系为基准。所以listen里面把
listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);
此时小乌龟实际上就会以一个2的偏移开始运动~
还有个小疑问,关于ros::Time::now() 和 ros::Time(0),晚点把时间这块琢磨一下,一起补充。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)