一个机械臂,每一个关节都需要一个坐标系,一个无人小车不同的传感器一般都需要自己的坐标系。必不可少的,他们都需要世界坐标系。 有些坐标系是移动的,比如在一个车子上的传感器的自己的坐标系,随着车子的移动而移动;有的坐标系是静止的,比如世界坐标系。那些固定在机器人身上的传感器,他们之间的位置是固定的,有了一者的位置,我们应该就能知道另一者的位置。 追踪不同的坐标系,看起来是个很简单的活,但是实现起来挺复杂的,各个坐标系之间需要良好的及时的通讯,而ROS最牛叉的就是用来通讯嘛。 在这一讲,我们会使用ROS的tf包,去解决不同坐标系追踪的问题。我们首先使用官网的第一个例子入门,然后再用我自己编写的例子更深入的了解。我自己的例子要实现的功能是:假设有一个在移动的机器人,它上面有两个能定位的传感器(比如GPS和相机,相机定位通过SLAM实现)。相机和GPS都有自己的坐标系,这两坐标系要一直附着在我们的机器人上移动即位置相对机器人固定,但是相对世界坐标系移动。结果会在rviz上显示出来。你们将会看到如下内容:
catkin_create_pkg learn_rviz_tf roscpp rospy std_msgs geometry_msgs visualization_msgs tf
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;
};
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对象可以调用的一切函数。其中就有
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的过程中你会获取到更多的经验。
另外很重要的是,在链接中我们可以看到下面的内容
这意味着,利用tf定义的四元数是重载了计算符号的,即你如果有如下定义
tf::Quaternion q_AB //A坐标到B坐标的的旋转变换
tf::Quaternion q_BC //B坐标到C坐标的旋转变换
q_AB * q_BC
<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);//发布坐标转换
#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);
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>
learning_tf2
,因为他们创建了一个叫learningtf2的package来做tf的tutorial,而我们的现在代码是在learning_rviz_tf这个pakcage里,所以package名字改一下,不是大问题。
写好roslauch编译好文件后我们先来运行一下程序看看什么效果。
roslaunch learn_rviz_tf start_demo.launch
rosrun tf tf_echo /world /turtle1
At time ...
- Transolation ..
- Rotation ...
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<node ... args = `/turtle1`...>
<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" />
turtle_tf2_broadcast
,为了区分他们,给了他们不同的名字turtle1_tf2_broadcaster
,turtle2_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>
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
TF
,由于我们发布的TF是关于world和turtle1的,我们希望world
坐标系是固定的,所以在rviz中把Fixed Frame
改成world
,这时候你刚才roslaunch跑的程序所发布的坐标系之间的转换就正在被rviz接收了。你会看到类似下图的东西
rviz很清晰地显示了两个坐标系之间的联系。在你跑roslaunch的terminal中移动小乌龟,你会看到turtle1这个坐标系也在移动。 同样类似于你可以用rqt_graph命令来观察publisher和subscriber的关系,你可以在terminal中用下面的命令观察坐标系之间的关系
rosrun rqt_tf_tree rqt_tf_tree
这表示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>
mk_.ns = "my_namespace";
mk_.id = 0;
mk_.pose = msg->pose;
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})
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
Marker
点击OK。再次点击ADD按钮,选择TF
,点击OK。把Global Options
下面的Fixed_Frame设置为world
。这时候你的rviz界面应该是这样的(注意左边栏包含了TF和Marker)。
roslaunch learn_tf_rviz run_mcs.launch
可以看到,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
总结
关于静态坐标系之间的关系,当然也可以在程序中设置,不过直接写到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一下。
评论(0)
您还未登录,请登录后发表或查看评论