ROS与C++入门教程-navigation-实现TF变换

ROS与C++入门教程-navigation-实现TF变换

说明：

• 介绍如何设置机器人开始使用tf的指南

变换配置

• 许多ROS包需要使用tf软件库发布机器人的变换树。

• 在抽象层面上，变换树根据不同坐标系之间的平移和旋转定义偏移。

• 为了更具体，考虑一个简单的机器人的示例，该机器人具有移动基座，其上安装有单个激光器。

• 在参考机器人时，我们定义两个坐标系：一个对应于机器人基座的中心点，一个对应于安装在基座顶部的激光器的中心点。

• 让我们给他们的名字，方便参考。

• 我们将调用附加到移动基站坐标系叫“base_link”（用于导航，重要的是将其放置在机器人的旋转中心）

• 将调用附加到激光器坐标系叫“base_laser”。

• 有关框架命名约定，请参见REP 105

• 在这一点上，我们假设我们有一些来自激光器的数据，其形式为距激光器中心点的距离。

• 换句话说，我们在“base_laser”坐标系中有一些数据。

• 现在假设我们想要获取这些数据并使用它来帮助移动基地避免世界上的障碍。

• 为了成功地做到这一点，我们需要一种方法将我们从“base_laser”坐标系到“base_link”坐标系的激光扫描。

• 实质上，我们需要定义“base_laser”和“base_link”坐标系之间的关系。

• 图示：
  

• 在定义这种关系时，假设我们知道激光器安装在移动基座中心点上方20cm处和右方10cm处。

• 这为我们提供了一个将“base_link”坐标系与“base_laser”坐标系相关联的平移偏移。

• 具体来说，我们知道要从“base_link”坐标系到“base_laser”坐标系的数据，我们必须应用（x：0.1m，y：0.0m，z：0.2m）的变换，并从“ base_laser“坐标系到”base_link“坐标系，我们必须应用相反的转换（x：-0.1m，y：0.0m，z：-0.20m）

• 我们可以选择自己管理这种关系，意味着在必要时在帧之间存储和应用适当的变换，但是随着坐标帧的数量增加，这变成了真正的痛苦。幸运的是，我们不必自己做这项工作。相反，我们将使用tf定义“base_link”和“base_laser”之间的关系，并让它管理我们的两个坐标框架之间的变换。

• 要使用tf定义和存储“base_link”和“base_laser”坐标系之间的关系，我们需要将它们添加到变换树。

• 在概念上，变换树中的每个节点对应于坐标系，并且每个边缘对应于需要应用于从当前节点移动到其子节点的变换。

• Tf使用树结构来保证只有一个遍历将任何两个坐标系链接在一起，并且假设树中的所有方向都从父节点到子节点。

• 图示：
  

• 要为我们的简单示例创建一个变换树，我们将创建两个节点，一个用于“base_link”坐标系，一个用于“base_laser”坐标系。

• 为了创建它们之间的方向，我们首先需要决定哪个节点将是父节点，哪个节点是子节点。

• 记住，这种区别很重要，因为tf假定所有的变换都从父对象移动到子对象。

• 让我们选择“base_link”坐标系作为父项，因为当其他零件/传感器添加到机器人时，通过遍历“base_link”框架，最有意义的是它们与“base_laser”坐标系相关。

• 这意味着与连接“base_link”和“base_laser”的方向相关联的变换应该是（x：0.1m，y：0.0m，z：0.2m）。

• 使用这个变换树设置，将在“base_laser”坐标系中接收到的激光扫描转换为“base_link”坐标系就像调用tf库一样简单。

• 我们的机器人可以使用这些信息来推测“base_link”坐标系中的激光扫描，并安全地规划其环境中的障碍物。

编写代码

• 希望上面的例子有助于在概念层面上理解tf。

• 现在，我们必须采取变换树并用代码创建它。

• 对于这个例子，假设熟悉ROS，所以如果任何术语或概念不熟悉，请确保查看ROS文档。

• 假设我们有上面描述的在“base_laser”坐标系中获取点并将它们转换为“base_link”坐标系的高级任务。

• 我们需要做的第一件事是创建一个负责在我们的系统中发布转换的节点。

• 接下来，我们必须创建一个节点来监听通过ROS发布的变换数据，并应用它来变换一个点。

• 我们将首先创建一个源代码包，我们将给它一个简单的名称

• 如“robot_setup_tf”我们将依赖于roscpp，tf和geometry_msgs。

$ cd ~/catkin_ws/src
$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

• 或者在fuerte，groovy和hydro版：在navigation_tutorials包中有一个标准robot_setup_tf_tutorial包。
• 您可以通过以下方式安装（%YOUR_ROS_DISTRO%可以是{ fuerte，groovy }等）：

$ sudo apt-get install ros-%YOUR_ROS_DISTRO%-navigation-tutorials 

广播变换

• 现在我们已经得到了我们的包，我们需要创建将通过ROS广播base_laser → base_link转换的工作的节点。
• 在刚刚创建的robot_setup_tf包中，启动您喜欢的编辑器，并将以下代码粘贴到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();
  }
}

• 现在，让我们来看看更详细地发布base_link → base_laser转换相关的代码。

代码解释：

• 代码：

#include <tf/transform_broadcaster.h>

• 解释：

  • tf包提供了一个tf::TransformBroadcaster的实现，以帮助使发布变换的任务更容易。
  • 要使用TransformBroadcaster，我们需要包括tf/transform_broadcaster.h头文件。

• 代码：

tf::TransformBroadcaster broadcaster;

• 解释：

  • 在这里，我们创建一个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指定，用于需要在两个坐标系之间发生的任何旋转。在这种情况下，我们希望应用无旋转，因此，我们在一个发送btQuaternion从俯仰，滚动和偏航值等于零构成。
    • 其次，btVector3任何我们想申请转换。然而，我们想要应用平移，因此我们创建一个btVector3，其对应于激光器的x偏移10cm和距离机器人基座20cm的z偏移。
    • 第三，我们需要给正在发布的变换发送时间戳，我们将使用ros::Time::now()。
    • 第四，我们需要传递我们正在创建的链接的父节点的名称，在这种情况下为“base_link”。
    • 第五，我们需要传递我们正在创建的链接的子节点的名称，在本例中为“base_laser”。

使用变换

• 上面，我们创建了一个在ROS 上发布base_laser → base_link转换的节点。
• 现在，我们将编写一个将使用该变换的节点在“base_laser”坐标系中取一个点，并将其转换为“base_link”坐标系中的点。
• 再次，我们将首先将下面的代码粘贴到文件中，并跟进更详细的解释。
• 在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/transform_listener.h头文件，我们需要创建一个tf::TransformListener。
  • 一个TransformListener对象自动订阅了ROS变换消息话题和管理所有的进入的转换数据。

• 代码：

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

• 解释：

  • 我们将创建一个函数，给定TransformListener，在“base_laser”坐标系中取一个点，并将其转换为“base_link”坐标系。
  • 这个函数将作为在我们的程序的main()中创建的ros::Timer的回调，并且每秒都会触发。

• 代码：

  //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。
  • 消息名称末尾的“Stamped”只是意味着它包含一个头，允许我们将时间戳和frame_id与消息相关联。
  • 我们将laser_point消息的stamp字段设置为 ros::Time()，它是一个特殊的时间值，允许我们向TransformListener请求最新的可用变换。
  • 对于header的frame_id字段，我们将其设置为“base_laser”，因为我们在“base_laser”坐标系中创建一个点。
  • 最后，我们将为点设置一些数据如x：1.0，y：0.2和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());
  }

• 解释：

  • 现在，我们已经在“base_laser”坐标系中的点，我们想将其转换为“base_link”框架。
  • 为此，我们将使用TransformListener对象，并使用三个参数调用transformPoint()：我们想要将点转换为的坐标系的名称（在我们的例子中为“base_link”），我们正在转换的点，存储变换点。
  • 因此，在调用transformPoint()之后，base_point保存与laser_point相同的信息，之前仅在“base_link”坐标系中。

• 代码：

 catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }

• 解释：
  • 如果由于某种原因，base_laser → base_link变换是不可用（也许tf_broadcaster未运行），则TransformListener当我们调用可能会引发异常transformPoint()。
  • 为了确保我们能够正常处理，我们将捕获异常并为用户打印一个错误。

构建代码

• 现在我们写了我们的小例子，我们需要构建它。
• 打开由roscreate-pkg自动生成的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 ~/catkin_ws/
$ catkin_make

运行代码

• 新终端，执行roscore

$ roscore

• 新终端，执行tf_broadcaster

rosrun robot_setup_tf tf_broadcaster

• 我们运行tf_listener将我们的模拟点从“base_laser”坐标系转换为“base_link”坐标系。
• 新终端， 执行tf_listener

rosrun robot_setup_tf tf_listener

• 您应该看到以下输出，显示每秒一次从“base_laser”坐标系转换为“base_link”坐标系的点
• 效果：

[ 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

• 恭喜，你刚刚为平面激光器写了一个成功的变换广播器。