ROS探索总结-22.设置机器人的tf变换

ROS1/一代机器人系统 少儿编程 772浏览 0评论
ROS探索总结

本文转载自古月居,原作者古月,原文链接:https://www.guyuehome.com/355。

设置机器人的tf变换

1、什么是tf变换

ROS中的很多软件包都需要机器人发布tf变换树,那么什么是tf变换树呢?抽象的来讲,一棵tf变换树定义了不同坐标系之间的平移与旋转变换关系。具体来说,我们假设有一个机器人,包括一个机器人移动平台和一个安装在平台之上的激光雷达,以这个机器人为例,定义两个坐标系,一个坐标系以机器人移动平台的中心为原点,称为base_link参考系,另一个坐标系以激光雷达的中心为原点,称为base_laser参考系。
ROS探索总结-22.设置机器人的tf变换

假设在机器人运行过程中,激光雷达可以采集到距离前方障碍物的数据,这些数据当然是以激光雷达为原点的测量值,换句话说,也就是base_laser参考系下的测量值。现在,如果我们想使用这些数据帮助机器人完成避障功能,当然,由于激光雷达在机器人之上,直接使用这些数据不会产生太大的问题,但是激光雷达并不在机器人的中心之上,在极度要求较高的系统中,会始终存在一个雷达与机器人中心的偏差值。这个时候,如果我们采用一种坐标变换,将及激光数据从base_laser参考系变换到base_link参考下,问题不就解决了么。这里我们就需要定义这两个坐标系之间的变换关系。

为了定义这个变换关系,假设我们已知激光雷达安装的位置在机器人的中心点上方20cm,前方10cm处。这就根据这些数据,就足以定义这两个参考系之间的变换关系:当我们获取激光数据后,采用(x: 0.1m, y: 0.0m, z: 0.2m)的坐标变换,就可以将数据从base_laser参考系变换到base_link参考系了。当然,如果要方向变化,采用(x: -0.1m, y: 0.0m, z: -0.20m)的变换即可。

从上边的示例看来,参考系之间的坐标变换好像并不复杂,但是在复杂的系统中,存在的参考系可能远远大于两个,如果我们都使用这种手动的方式进行变换,估计很快你就会被繁杂的坐标关系搞蒙了。ROS提供的tf变换就是为解决这个问题而生的,tf功能包提供了存储、计算不同数据在不同参考系之间变换的功能,我们只需要告诉tf树这些参考系之间的变换公式即可,这颗tf树就可以树的数据结构,管理我们所需要的参考系变换。

ROS探索总结-22.设置机器人的tf变换

还是以上边的示例为基础,为了定义和存储base_link和base_laser两个参考系之间的关系,我们需要将他们添加到tf树中。从树的概念上来讲,tf树中的每个节点都对应一个参考系,而节点之间的边对应于参考系之间的变换关系。tf就是使用这样的树结构,保证每两个参考系之间只有一种遍历方式,而且所有变换关系,都是母节点到子节点的变换。

为了定义上边示例中的参考系,我们需要定义两个节点,一个对应于base_link参考系,一个对应于base_laser参考系。为了创建两个节点之间的边,我们首先需要决定哪一个节点作为母节点,哪一个节点作为子节点,这一点在tf树中是非常重要的。这里我们选择base_link作为母节点,这样会方便后边为机器人添加更多的传感器作为子节点。所以,根据之前的分析,从base_link节点到base_laser节点的变换关系为(x: 0.1m, y: 0.0m, z: 0.2m)。设置完毕后,我们就可以通过调用tf库,直接完成base_laser参考系到base_link参考系的数据坐标变换了。

2、代码流程

通过上边的分析,应该可以从理论上帮助你理解什么是tf,以及tf的功能了。在实际应用中,我们需要使用代码来完成这些理论,下边我们就来看看如何使用代码来调用tf的变换。

代码的总体思路分为两个部分:

(1)编写一个节点,广播两个参考系之间的tf变换关系
(2)编写另外一个节点,订阅tf树,然后从tf树中遍历到两个参考系之间的变换公式,然后通过公式计算数据的变换。

我们先来完成第一步。首先我们来创建一个功能包,用于后边程序的放置,这里需要依赖roscpp、tf、geometry_msgs。

$ cd %TOP_DIR_YOUR_CATKIN_WS%/src
$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

3、编写广播节点

进入功能包,创建src/tf_broadcaster.cpp文件,来完成广播节点的代码:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){

  ros::init(argc, argv, "robot_tf_publisher");

  ros::NodeHandle n;

 

  ros::Rate r(100);

 

  tf::TransformBroadcaster broadcaster;

 

  while(n.ok()){

    broadcaster.sendTransform(

      tf::StampedTransform(

        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),

        ros::Time::now(),"base_link", "base_laser"));

    r.sleep();

  }

}

逐行分析如上代码:

#include <tf/transform_broadcaster.h>

因为后边会使用到tf::TransformBroadcaster类的实例,来完成tf树的广播,所以需要先包含相关的头文件。

tf::TransformBroadcaster broadcaster;

创建一个tf::TransformBroadcaster类的实例,用来广播 base_link → base_laser的变换关系。

   broadcaster.sendTransform(
      tf::StampedTransform(

        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),

        ros::Time::now(),"base_link", "base_laser"));

这部分是代码的关键所在。通过TransformBroadcaster 类来发布变换关系的接口,需要五个参数。首先是两个参考系之间的旋转变换,通过btQuaternion四元数来存储旋转变换的参数,因为我们用到的两个参考系没有发生旋转变换,所以倾斜角、滚动角、偏航角都是0。第二个参数是坐标的位移变换,我们用到的两个参考系在X轴和Z轴发生了位置,根据位移值填入到btVector3 向量中。第三个参数是时间戳,直接太难过ROS的API完成。第四个参数是母节点存储的参考系,即base_link,最后一个参数是子节点存储的参考系,即base_laser。

4、编写订阅节点

上一节讲解了如何使用代码编写一个广播tf变换的节点,这一节,我们编写一个订阅tf广播的节点,并且使用tf树中base_link到base_laser的变换关系,完成数据的坐标变换。在robot_setup_tf功能包中创建src/tf_listener.cpp文件,代码如下:

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>

#include <tf/transform_listener.h>

 

void transformPoint(const tf::TransformListener& listener){

  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame

  geometry_msgs::PointStamped laser_point;

  laser_point.header.frame_id = "base_laser";

 

  //we'll just use the most recent transform available for our simple example

  laser_point.header.stamp = ros::Time();

 

  //just an arbitrary point in space

  laser_point.point.x = 1.0;

  laser_point.point.y = 0.2;

  laser_point.point.z = 0.0;

 

  try{

    geometry_msgs::PointStamped base_point;

    listener.transformPoint("base_link", laser_point, base_point);

 

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",

        laser_point.point.x, laser_point.point.y, laser_point.point.z,

        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());

  }

  catch(tf::TransformException& ex){

    ROS_ERROR("Received an exception trying to transform a point from "base_laser" to "base_link": %s", ex.what());

  }

}

 

int main(int argc, char** argv){

  ros::init(argc, argv, "robot_tf_listener");

  ros::NodeHandle n;

 

  tf::TransformListener listener(ros::Duration(10));

 

  //we'll transform a point once every second

  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

 

  ros::spin();

 

}

进行详细的分析:

#include <tf/transform_listener.h>

在后边的代码中,我们会使用到tf::TransformListener对象,该对象会自动订阅ROS中的tf消息,并且管理所有的变换关系数据。所以需要先包含相关的头文件。

void transformPoint(const tf::TransformListener& listener){

我们创建一个回调函数,每次收到tf消息时,都会自动调用该函数,上一节我们设置了发布tf消息的频率是1Hz,所以回调函数执行的频率也是1Hz。在回调函数中,我们需要完成数据从base_laser到base_link参考系的坐标变换。

//we'll create a point in the base_laser frame that we'd like to transform to the base_link frame

  geometry_msgs::PointStamped laser_point;

  laser_point.header.frame_id = "base_laser";

 

  //we'll just use the most recent transform available for our simple example

  laser_point.header.stamp = ros::Time();

 

  //just an arbitrary point in space

  laser_point.point.x = 1.0;

  laser_point.point.y = 0.2;

  laser_point.point.z = 0.0;

我们创建了一个geometry_msgs::PointStamped类型的虚拟点,该点的坐标为(1.0,0.2,0.0)。该类型包含标准的header消息结构,这样,我们可以就可以在消息中加入发布数据的时间戳和参考系的id。

try{

    geometry_msgs::PointStamped base_point;

    listener.transformPoint("base_link", laser_point, base_point);

 

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",

        laser_point.point.x, laser_point.point.y, laser_point.point.z,

        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());

  }

这里是代码的关键位置。我们已经在base_laser参考系下虚拟了一个数据点,那么怎样将该点的数据转换到base_base参考系下呢?使用TransformListener 对象中的transformPoint()函数即可,该函数包含三个参数:第一个参数是需要转换到的参考系id,当然是base_link了;第二个参数是需要转换的原始数据;第三个参数用来存储转换完成的数据。该函数执行完毕后,base_point就是我们转换完成的点坐标了!

catch(tf::TransformException& ex){

    ROS_ERROR("Received an exception trying to transform a point from "base_laser" to "base_link": %s", ex.what());

  }

为了保证代码的稳定性,我们也需要应对出错处理,例如当tf并没有发布需要的变换关系时,在执行transformPoint时就会出现错误。

5、编译代码

到此为止,我们已经完成了代码编写的全部流程,接下来就是编译代码了!打开功能包的CMakeLists.txt文件,加入相应的编译选项:

add_executable(tf_broadcaster src/tf_broadcaster.cpp)

add_executable(tf_listener src/tf_listener.cpp)

target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})

target_link_libraries(tf_listener ${catkin_LIBRARIES})

然后保存文件,经行编译:

$ cd %TOP_DIR_YOUR_CATKIN_WS%
$ catkin_make

6、运行代码

终于来到最激动人心的一步,见证奇迹的时刻就要来临!
打开一个终端,运行roscore:

roscore

再打开一个终端,运行tf广播tf_broadcaster:

rosrun robot_setup_tf tf_broadcaster

还要再打开一个终端,运行tf监听tf_listener,将我们虚拟的点坐标,从base_laser转换到base_link参考系下:

rosrun robot_setup_tf tf_listener

如果一切正常,你应该已经可以在第三个终端中看到如下的数据了:

[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19

[ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19

[ INFO] 1248138530.200821000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138530.19

[ INFO] 1248138531.200835000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138531.19

[ INFO] 1248138532.200849000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138532.19

从上边的信息中,我们可以清晰的看到,我们虚拟的坐标点,成功转换到了base_link参考系下,坐标位置变换成了(1.10, 0.20, 0.20)。

到此为止,我们已经完成了例程的所有讲解,在你需要完整的真实系统中,请根据需求将PointStamped数据类型修改成你所使用传感器发布的实际消息对应类型。

参考链接:

http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF

  

您必须 登录 才能发表评论!