机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系

83
0
2020年10月15日 11时31分

在机器人领域里,多个坐标系统应该是最常见的事之一了。

一个机械臂,每一个关节都需要一个坐标系,一个无人小车不同的传感器一般都需要自己的坐标系。必不可少的,他们都需要世界坐标系。
有些坐标系是移动的,比如在一个车子上的传感器的自己的坐标系,随着车子的移动而移动;有的坐标系是静止的,比如世界坐标系。那些固定在机器人身上的传感器,他们之间的位置是固定的,有了一者的位置,我们应该就能知道另一者的位置。
追踪不同的坐标系,看起来是个很简单的活,但是实现起来挺复杂的,各个坐标系之间需要良好的及时的通讯,而ROS最牛叉的就是用来通讯嘛。
在这一讲,我们会使用ROS的tf包,去解决不同坐标系追踪的问题。我们首先使用官网的第一个例子入门,然后再用我自己编写的例子更深入的了解。我自己的例子要实现的功能是:假设有一个在移动的机器人,它上面有两个能定位的传感器(比如GPS和相机,相机定位通过SLAM实现)。相机和GPS都有自己的坐标系,这两坐标系要一直附着在我们的机器人上移动即位置相对机器人固定,但是相对世界坐标系移动。结果会在rviz上显示出来。你们将会看到如下内容:

 

机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系插图
moving_robot.gif

 

从动图中我们可以看到在机器人身上有两个坐标系,一个叫camera一个叫gps,他们的位置相对固定,然后整个机器人相对世界坐标系移动。(说好的机器人怎么是个绿方块啊喂!!恩…你就假装它是个机器人好了…)。下面我们就来看一下怎么实现上面描述的功能。
之前我们讲坐标系变换是通过tf这个package实现的,不过我才发现tf的更新版tf2都出来不少时间了,他们之间大同小异。我们就使用最新的tf2吧。由于上一讲我们通过

 

catkin_create_pkg learn_rviz_tf roscpp rospy std_msgs geometry_msgs visualization_msgs tf

 

创建了learn_rviz_tf这个package,但是现在我们想添加新的依赖项tf2进去,需要修改CMakeLists.txt和package.xml,具体如果和修改我们在机器人操作系统ROS:从入门到放弃(三) 发布接收不同消息2
里讲过,这里不赘述了。你如果从我的github上直接下载这个package当然就不用修改什么了,我已经改好了。

tf2基础例子

ROS官网例子链接如下
http://wiki.ros.org/tf2/Tutorials
我们使用第二个 Writing a tf2 broadcaster (C++)来讲解。
进入链接后我们直接从2.1,the code部分开始,在我们的learn_rviz_tf/src中创建turtle_tf2_broadcaster.cpp并把代码复制进去。这儿为了方便讲解我也复制过来了

 

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf2_ros::TransformBroadcaster br;
  geometry_msgs::TransformStamped transformStamped;
  
  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;
  tf2::Quaternion q;
  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;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

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

 

官网的代码解读不甚详细,我赘述一遍并加点东西。
首先坐标系之间转换的发布不再是定义一个publisher然后用publisher.publish()函数来发布了,而是在tf2_ros::TransformBroadcaster这个类下定义一个专门的发布坐标转换的对象,使用对象.sendTransform(msg_type)来发布坐标之间的关系。发布的msg_type是特定的消息类型geometry_msgs/TransformStamped.h。基于这一点我们来看代码。
1:
头文件部分包含了#include <tf2/LinearMath/Quaternion.h>,代码有一行定义了tf::quaternion的对象tf::Quaternion q需要通过包含这个头文件来实现。
回想我们在使用geometry_msgs/PoseStamped时曾需要定义消息的pose,而pose包含position和orientation,其中orientation的消息类型是geometry/Quaternion,也就是说我们也可以通过geometry::Quaternion q定义一个四元数对象q,这也是四元数。这两种四元数的定义的对象是有很大的区别的。后者只能通过q.w, q.x, q.y, q.z调用四元数包含的数据。而前者拥有更广泛的作用。具体可以参考
tf: tf::Quaternion Class Reference
点进去, public member function显示了tf::Quaternion对象可以调用的一切函数。其中就有

 

机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系插图(1)

 

我们上面的代码中使用了这个函数,q.setRPY(0, 0, msg->theta),这个函数完成了从roll pitch yaw到四元数的转换功能,通过设定roll pitch yaw来设定四元数的具体数据。tf::Quaternion定义的对象q获取四元数的具体方法是q.x(),q.y()…有别于geometry::Quaternion定义的q通过q.x,q.y…的获取方式。x(),y()..这几个函数我们并没有在上面的链接看到,原因是tf::Quaternion继承了QuadWord这个类的,在后者的定义中我们能看到能通过x(),y()这类型的函数来获取quaternion的具体数据。这需要读者去仔细寻找资料了,在多使用ROS的过程中你会获取到更多的经验。
另外很重要的是,在链接中我们可以看到下面的内容

 

机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系插图(2)

这意味着,利用tf定义的四元数是重载了计算符号的,即你如果有如下定义

tf::Quaternion q_AB //A坐标到B坐标的的旋转变换
tf::Quaternion q_BC //B坐标到C坐标的旋转变换

 

那么你可以通过

 

q_AB * q_BC

 

来获取到A坐标到C坐标的旋转变换。geometry_msgs定义的四元数是不具备上面的任何功能的,仅仅是储存数据并用来发布或者接收。
这里啰嗦了这么多,主要是希望大家通过例子能学会自己找到相关资料,这是最重要的。比如以后使用了tf::Transform定义了对象tf1,那么tf1是什么东西,能包含哪些功能,有哪些函数可以调用?你在google上搜寻tf::Transform进入 tf: tf::Transform Class Reference就自然可以查到。
2:
头文件包含了<tf2_ros/transform_broadcaster.h>。包含了这个头文件,就可以用代码中的tf2_ros::TransformBroadcaster br来定义一个发布坐标转换的对象了。
3:
头文件包含了<geometry_msgs/TransformStamped.h>,即我们之前说的br发布的内容需要是geometry_msgs命名空间下,TransformStamped类型的消息。可以看到要发布的消息类型在代码中这么定义的
geometry_msgs::TransformStamped transformStamped;
另外在代码中有一行br.sendTransform(transformStamped)就是用来发布坐标转换的消息的代码。
总的来说,这和我们一般定义publisher并发布消息的过程很像

 

ros::Publisher pub_abc = .... //定义publisher
std_msgs::Float64 abc; //定义要发布的消息类型
abc.data = .....; //给要发布的消息赋值 
pub_abc.publish(abc);//发布消息

 

只不过到发布坐标转换这儿变成了

 

tf2_ros::TransformBroadcaster br; //定义坐标转换的publisher
geometry_msgs::TransformStamped transformStamped; //定义坐标转换要发布的消息类型(只能是此类型)
...//赋值,赋值部分是最上面代码中位于poseCallback()函数中transformedStamed.header.stamp = ...以及后面的是来行的内容,稍后再讲
br.sendTransform(transformStamped);//发布坐标转换

 

可以看到发布坐标变换的整体过程和发布普通消息是类似的,只不过使用的函数,消息名字有些变化而已。
4:
头文件包含了#include <turtlesim/Pose.h>. ROS真是很喜欢用turtlesim这个package的内容来讲解了,具体里面包含的内容我也没去看过。不过相信大家在自己学习ROS官网的教程时都使用过这个package,打开过一个窗口看到两个可爱的小乌龟了。
我们的poseCallback函数的形参是这样的(const turtlesim::PoseConstPtr& msg),对比我们写过很多的callback函数的形参比如接收flaot类型消息的(const std_msgs::Float64::ConstPtr& msg)
我们就能猜到#include <turtlesim/Pose.h>是包含了消息类型Pose,这个Pose专门用来定义小乌龟的位置的。(定义ConstPtr时前面居然没有作用域符号::,我也吃了一惊,不过试了一下确实有没有都可以Em…)。
5: std::string turtle_name 用来接收后面要写的launch文件传递进来乌龟的名字。如何使用launch文件传递参数参考使用roslaunch那一节。
6: void poseCallback(const turtlesim::PoseConstPtr& msg), 这个回调函数对应的subscriber我们在main函数中可以看到。ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);也就是说它subscribe的topic是turtle_name+”/pose”.一会儿我们运行程序我们就能看到小乌龟,小乌龟的位置会时刻发布出来,这个subscriber就会接收
回调函数的内容,一二行我们已经讲过,第一行用了static定义br,主要是避免重复定义。就像定义publisher我们肯定也只定义一次。遇到这种情况其实我们最好把回调函数写在类中,把br定义为类成员(见在类中publish和subscribr那一讲)。
回调函数的第三行开始给我们要发布的坐标转换赋值。关于transformStamped包含哪些成员,怎么使用,赋值等,和我们之前在第三讲过的poseStamped类似。
transformStamped.header.stamp = ros::Time::now();坐标之间的关系可能变化,那么自然我们在定义坐标之间的关系时,自然要给它赋值在什么时刻,坐标之间的关系如何
transformStamped.header.frame_id = "world"transformStamped.child_frame_id = turtle_name;两行,既然涉及到两个坐标之间的关系,我们肯定需要知道两个坐标系的名字,所以world那一行定义了parent坐标系的名字,turtle_name那一行定义了child坐标系的名字。
那么具体两个坐标之间的关系是怎么样的呢?自然就涉及到给坐标系之间的rotation和translation赋值。我们说回调函数接收到的是turtle的位置和方向,那么msg中的位置和方向我们就需要赋值transformStamped。即下面几行。

 

  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
...
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

 

中间省略的两行

 

tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);

 

主要是换个方式表达rotation。后面我们可以看到由于乌龟是二维运动,所以msg涉及到的只有x,y和theta. theta即三维坐标中的yaw角度,这就是为什么利用q.setRPY(0, 0, msg->theta);定义了quaternion具体的内容。之后把tf的quaternon的值赋值给我们要发布的消息类型transformStamped所包含的quaternion的值就可以了。
当transformed的内容都设置好了后br.sendTransform(transformStamped);发布坐标转换。

5:主函数中,ros::NodeHandle private_node("~")和我们平时定义node的方式不是很一样。平时我们定义node都是直接ros::NodeHandle nh。这里多了一个参数。它表示此时这个nodehandle是读取局部参数的。具体可见这篇文章https://blog.csdn.net/shijinqiao/article/details/50450844。总体作用和使用方式和普通node没有大的区别。:
6:main函数的if else语句就是要从launch文件中读取turtle_name那个参数。
再之后定义获取乌龟pose的subscriber。为什么有两个nodehandle呢,我们在launch文件中解释。
把这个文件添加到CMakeLists.txt编译。
之后我们在learn_rviz_tf目录下添加一个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="learn_rviz_tf" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />
    <node pkg="learn_rviz_tf" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />

  </launch>

 

注意这儿和原tutorial的launch文件有些小出入。原launch文件第三个和第四个node的后面pkg的名字是learning_tf2,因为他们创建了一个叫learningtf2的package来做tf的tutorial,而我们的现在代码是在learning_rviz_tf这个pakcage里,所以package名字改一下,不是大问题。
写好roslauch编译好文件后我们先来运行一下程序看看什么效果。

 

roslaunch learn_rviz_tf start_demo.launch

 

看到下图一个小乌龟出来了。

机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系插图(3)
重新点击跑roslaunch的终端,你的小乌龟会移动。打开另一个terminal,在其中输入

rosrun tf tf_echo /world /turtle1

 

这时候你会看到类似于下面的内容

 

机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系插图(4)
show_tf.png

 

每一个

 

At time ...
- Transolation ..
- Rotation ...

 

是一组tf,显示的是在某个时间点,world坐标系和turtle坐标系之间的关系。当你移动小乌龟时,translation和rotation会发生改变。
下面解释launch文件中的内容
1:

<node pkg="turtlesim" type="turtlesim_node" name="sim"/>

 

这个节点是显示乌龟这个GUI的,不用深究。

2:

<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

 

这是为了使你的的键盘能控制乌龟的移动。(从key那个名字就可以看出)不用深究。
3:
param两行的参数目前在我们程序中没有使用。

4:

<node ... args = `/turtle1`...>

 

即运行刚刚写的程序
5:
下面一个node暂时没有使用。官网关于tf2的tutorial的另一个例子会使用。
好像并没有解释什么…
最后回到程序中关于praivate_node的问题,为什么多用一个nodeHandle?其实我们在launch文件中可以发现

 

    <node pkg="learn_rviz_tf" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />
    <node pkg="learn_rviz_tf" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />

 

这代表这两个node其实使用的是一个可执行文件,即turtle_tf2_broadcast,为了区分他们,给了他们不同的名字turtle1_tf2_broadcasterturtle2_tf2_broadcaster,即同样的执行文件不同的**节点
**名。同时这两个节点的传入args不同。
当好几个节点都来自于一个可执行文件时,我们难免遇到同一个问题。如果他们都需要传入某个名叫A的参数,但是A的值需要不一样,该怎么办呢?这时候我们通常要把要读取的参数分开写到节点内。如下

 

<param name="A" value="11" />
  <node name="you" pkg="ABC" type="abc" output="screen">
    <param name="A" value="10" />
  </node>
  <node name="me" pkg="ABC" type="abc" output="screen">
    <param name="A" value="9" />
  </node>

 

在<node>…</node>内部定义的参数称为局部参数,必须定义一个私有nodehandle, 即ros::NodeHandle abc("~")并使用代码abc.getParam(…)才能读取
例如上面为了让节点you和me分别读取值为10的A和值为9的A,我们需要在程序内定义带参数的”~”的nodeHandle。表示读取自己内部的参数,同时不要读取值为11的全局参数A。
但其实上面的launch文件没有重复名字的参数(args不算),理论上可以不用局部的nodehandle。我把(“~”)去掉了也一切正常。不过大家以后要在一个launch文件中使用相同名字但值不同的参数时,一定要考虑这个问题。
所以总的来说,我们原来的程序只定义一个node并且不添加参数(“~”)也没有问题,由于没用args为turtle2的节点,所以把args = “turtle2″那一个节点删掉了也没有问题,本来可以大大做简化,但是考虑到很多同学还是按照官网的例子学习,所以就冗杂地解释了一番。
在程序运行时,我们在使用rviz来可视化tf。打开一个新的terminal,在其中输入

 

rosrun rviz rivz

 

上一章节我们为了可视化pose通过ADD按钮添加了marker,现在类似,rviz打开后点击左下方的ADD按钮
,在显示出的对话框中选择TF,由于我们发布的TF是关于world和turtle1的,我们希望world坐标系是固定的,所以在rviz中把Fixed Frame改成world,这时候你刚才roslaunch跑的程序所发布的坐标系之间的转换就正在被rviz接收了。你会看到类似下图的东西

机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系插图(5)

rviz很清晰地显示了两个坐标系之间的联系。在你跑roslaunch的terminal中移动小乌龟,你会看到turtle1这个坐标系也在移动。
同样类似于你可以用rqt_graph命令来观察publisher和subscriber的关系,你可以在terminal中用下面的命令观察坐标系之间的关系

rosrun rqt_tf_tree rqt_tf_tree

 

输入该命令后会出现类似于下面的图像

机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系插图(6)

这表示world是母坐标系,turtle1是子坐标系。

tf2多个坐标系追踪

讲完了基础例子,我们就可以来实现我第一张动图moving_robot.gif。在那个动图中,我们有三个坐标系,camera和gps相对静止,和我们的机器人(方块marker)一块儿相对于世界坐标系world移动。官网的例子很不错,不过turtlesim之类的东西内部是咋样的我们毕竟不是那么了解,还有局部nodeHandle,几个例子里还使用了service之类,虽然都不是复杂东西,但和在一起总会有人不清楚其中的部分。我还是倾向于大家能懂tutorial的例子的每一行代码,代码的内容我们在之前都有所接触。
我们在learn_rviz_tf/src中创建一个叫moving_coordinate_system.cpp的文件。写入下面内容

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <tf2/LinearMath/Quaternion.h>

class MovingObject{
public:
    MovingObject(ros::NodeHandle& nh);
    void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void set_marker_fixed_property();
private:
    ros::Publisher pub_object_;
    visualization_msgs::Marker mk_;
    tf2_ros::TransformBroadcaster br_;
};

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

    ros::NodeHandle nh;

    MovingObject mo(nh);

    ros::Subscriber sub_gps = nh.subscribe("/chatter", 10, &MovingObject::PoseCallback, &mo);
  
    ros::spin();
}

MovingObject::MovingObject(ros::NodeHandle& nh){
    pub_object_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
    set_marker_fixed_property();
}

void MovingObject::PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
    ROS_INFO("get gps position, %f, %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "world";
    transformStamped.child_frame_id = "gps";
    transformStamped.transform.translation.x = msg->pose.position.x - 0.1;
    transformStamped.transform.translation.y = msg->pose.position.y;
    transformStamped.transform.translation.z = msg->pose.position.z;
    transformStamped.transform.rotation.w = msg->pose.orientation.w;
    transformStamped.transform.rotation.x = msg->pose.orientation.x;
    transformStamped.transform.rotation.y = msg->pose.orientation.y;
    transformStamped.transform.rotation.z = msg->pose.orientation.z;
    br_.sendTransform(transformStamped);

    mk_.header.stamp = ros::Time();
    mk_.pose = msg->pose;
    pub_object_.publish(mk_);
}

void MovingObject::set_marker_fixed_property(){
    mk_.ns = "my_namespace";
    mk_.id = 0;
    mk_.header.frame_id = "world";
    mk_.type = visualization_msgs::Marker::CUBE;

    mk_.scale.x = 0.5;
    mk_.scale.y = 0.5;
    mk_.scale.z = 0.5;

    mk_.color.a = 0.3; 
    mk_.color.r = 0.0;
    mk_.color.g = 1.0;
    mk_.color.b = 0.0;

    mk_.action = visualization_msgs::Marker::ADD;
}

 

头文件除了第一个基础例子的以外,增加了

#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>

 

我们依次讲解程序内容
1:我们在上一章可视化marker的时候写了一个subscriber文件,用来接收rosbag发出的机器人pose的信息,并转换为marker的pose使之能在rviz中显示出来。而我们现在的例子同样显示marker,marker的pose后面可以看到同样来自于rosbag发出的消息。所以包含了这两个上一章节使用过的头文件,一个用来使用PoseStamped消息,一个用来使用Marker消息。并不意外。
2:定义了MovingObject类。
成员函数:
类的构造函数传入了nodehandle,用来初始化一些性质并定义publisher的内容。
PoseCallback函数用来接收来自于rosbag的机器人位置的消息并转化为marker类型的消息发布出去。
set_marker_fixed_property()函数用来设置marker一些我们不想改变的性质,和上一章类似。
成员变量:
数据成员pub_object_用来发布marker的pose使rviz接收
mk_就是maker了
br_即用来发布坐标系之间的关系的。
3:主函数中sub_gps就是用来接收来自rosbag发布的poseStamped类型消息。消息在PoseCallback函数中处理。
4:主函数之后,是MovingObject类的构造函数,在构造函数中,我们首先给pub_object赋值,负责发布marker类型的消息,pub_object发布的内容会用来产生我们动图1中的小方块。构造函数调用了set_marker_fixed_property()函数,在上一讲我们用这个函数来设置一些我们不想要改变的marker性质。除了颜色和大小不想改变外,我们并不想像上次那样,每接收一个pose就产生一个新的marker,我们希望当前的marker接收到一个新的pose后就移动到那个位置,所以marker只能有一个。上一讲我们说过marker的id和namespace两个量共同定义一个marker,所以在set_marker_fixed_property中把这两个量设置为常亮就可以了。即

    mk_.ns = "my_namespace";
    mk_.id = 0;

 

5:最后是PoseCallback函数。
我们要发布坐标系之间的tf,和第一个例子一样,需要定义是哪两个坐标系,即frame_id和child_frame_id,分别是world和gps。需要定义transform的平移和旋转。pose和transform本就是一个东西,都需要定义平移和旋转,他们在transformStamped和PoseStamped的中的名字不一样。所以我们挨个把pose的position赋值给transform的translation,把pose的orinetation赋值给transform的rotation就可以了。
代码中有一行

mk_.pose = msg->pose;

 

我们直接把接收到的消息的pose直接赋值给了marker的pose。如果gps到world的transform也用接收到的消息的pose,那么marker的中心就会和gps坐标系的中心重合。所以代码中我把pose.position.x减了0.1,只是人为地把为了gps的坐标系原点和marker中心做点区别,不是非得加。
定义好了tranformStamped之后就可以用br_.sendTranform把它发布出去,定义好了marker的pose之后,由于它的其他性质我们已经在set_marker_fixed_property中定义,那么就可以用pub_object_把marker发布出去了。这两个东西同时发布出去,用的同一个pose(除了x方向有0.1的固定差别),如果我们像第一个例子那样编译并跑程序,设置好rviz,跑rosbag文件发布pose,理论上就可以在rviz当中看到gps这个坐标系随着我们的机器人(marker一起移动了)。
等一下!还有一个坐标系呢??动图里还有个camera坐标系,可是仔细看我们的代码里,没有任何关于camera的东西。原来当两个坐标系相对静止的时候我们,我们最好直接把他们之间的tf直接设置到launch文件里
首先我们把源文件写到CMakelists里进行编译。

add_executable(moving_coordinate_system src/moving_coordinate_system.cpp)
target_link_libraries(moving_coordinate_system ${catkin_LIBRARIES})

 

然后在launch文件夹里创建一个叫run_mcs.launch的launch文件。并把下面的内容写到launch文件中

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="0.3 0 0 0 0 0 1 gps camera" />
    <node type="moving_coordinate_system" pkg="learn_rviz_tf" name="moving_coordinate_system" output="screen">
    </node>
</launch>

 

可以看到<node pkg = "tf2_ros"...>那一行,我们使用了名叫tf2_ros这个pakcage里名叫static_transform_publisher这个可执行文件,节点名我们命名为link1_broadcaster,args即我们要传入的transform了,前三个数字是transform的平移,后四个是四元数x,y,z,w(如果只有三个数字默认roll pitch yaw)。gps camera表示前面那组平移旋转就是这两个坐标系的关系。接着如果我们运行我们刚刚编译好的moving_coordinate_system,就可以看到一个新的坐标系出现在rviz里了。
我们先打开rviz

rosrun rviz rviz

 

如果你保存了之前rviz的信息,现在你rviz可能直接能接收marker和tf的信息,但我们假设是空白的。现在由于我们的程序既要发布marker的消息,又要发布tf的消息,自然我们需要加两个接收器。如上一章一样,点击rviz中ADD按钮,选择Marker点击OK。再次点击ADD按钮,选择TF,点击OK。把Global Options下面的Fixed_Frame设置为world。这时候你的rviz界面应该是这样的(注意左边栏包含了TF和Marker)。

 

机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系插图(7)
下面我们运行roslaunch

roslaunch learn_tf_rviz run_mcs.launch

 

这时候我们注意到rviz中有些变化,如下图

 

机器人操作系统ROS从入门到放弃(九):使用tf追踪不同坐标系插图(8)

可以看到,rviz左边的Global Statue有刚刚的黄色警告变成了红色错误,TF也有个地方变成黄色警告。点击拥有红色错误标识的Fixed Frame,它旁边写着Fixed Frame [world] does not exist。是world frame还没有定义。
这是因为我们的程序中world是定义在PoseCallback函数里的,也就是需要接收到消息后,world才有定义,如果没有接收到消息,那么world 就一直没有定义。所以我们只要让程序开始接收消息,错误就会消失。下面跑我们的rosbag。这里的rosbag本质上和我们在rosbag那一章创造的rosbag没有不同,使用的同样是第三讲那个程序,不过我把每两个pose之间的时间缩短了,这样我们的方块儿移动起来看起来就更流畅。新的rosbag的名字叫robot_pose.bag,你用我们之前的bag文件跑也是可以的,只是看起来没那么流畅。

rosbag play robot_pose.bag

 

这行命令一执行,rviz里的错误会消失,你也就可以看到类似于我们本讲第一个动图里的东西了。

总结

关于静态坐标系之间的关系,当然也可以在程序中设置,不过直接写到launch文件中是优先推荐的。关于tf还有很多其他内容,在官网中有写。
tf2/Tutorials – ROS Wiki
我们没有讲到的,既然能写tf的发布程序,那么我们也能写tf的接收程序,这和publisher,subscriber一个道理。即官网里writing a tf listener那一讲。另外我们在设置transformStamped的时候总是设置了每一个tf对应的时间的,这意味着我们可以随时查看之前的tf,即learn about tf and time那一讲。我再讲一次就太冗余了,能看到这儿相信那些剩下的内容你也自己能看懂了。关于rviz和tf我们就说这么多,至少有个简单的认识,当你实际遇到更多的问题时,google一下。

发表评论

后才能评论