ROS学习笔记75（使用TF设置你的机器人）

1 转换配置

许多ROS包使用tf软件库发布机器人的变换树。在抽象级别，变换树根据不同坐标坐标系之间的平移和旋转来定义偏移。为了描述的更具体点，考虑一个简单机器人的例子，该机器人具有移动基座，其顶部安装有一个激光器。在提到机器人时，我们定义两个坐标系：一个对应于机器人底座的中心点，另一个对应于安装在底座顶部激光器的中心点。我们给它们起名，以便于参考。

我们称连接到移动基座的坐标系为：base_link（对于导航，该坐标系基点通常在机器人的旋转中心），我们称附加到激光器的坐标系为：base_laser。对于坐标系命名约定，可以参考REP 105。

假设我们从激光器得到了一些距离数据。换句话说，我们基于“base_laser”有一些数据。现在假设我们想要获取这些数据并使用它来帮助“”base_laser”避开世界坐标系上的障碍。为了成功地做到这一点，我们需要一种方法将我们从“base_laser”坐标系接收的激光扫描数据转换为基于“base_link”坐标系。实质上，我们需要定义“base_laser”和“base_link”坐标之间的关系。

在定义这种关系时，假设我们知道激光器安装在移动基座中心点前方10厘米处和上方20厘米处。这给了我们一个平移偏移量，它将“base_link”坐标系与“base_laser”坐标系关联起来。Specifically, we know that to get data from the "base_link" frame to the "base_laser" frame we must apply a translation of (x: 0.1m, y: 0.0m, z: 0.2m), and to get data from the "base_laser" frame to the "base_link" frame we must apply the opposite translation (x: -0.1m, y: 0.0m, z: -0.20m).

我们可以选择自己管理这种关系，这意味着在必要时存储和应用这种转换在适当的时候，但随着坐标系数量的增加，这变得非常痛苦。幸运的是，我们不必自己做这项工作。Instead we'll define the relationship between "base_link" and "base_laser" once using tf and let it manage the transformation between the two coordinate frames for us.

要使用tf定义和存储“base_link”和“base_laser”坐标系之间的关系，我们需要将它们添加到transform tree中。从概念上讲，变换树中的每个节点对应于坐标系，并且每个边对应于需要应用于从当前节点移动到其子节点的变换。Tf uses a tree structure to guarantee that there is only a single traversal that links any two coordinate frames together, and assumes that all edges in the tree are directed from parent to child nodes.

To create a transform tree for our simple example, we'll create two nodes, one for the "base_link" coordinate frame and one for the "base_laser" coordinate frame。要在它们之间创建边，我们首先需要确定哪个节点将成为父节点，哪个节点将成为子节点。请记住，这种区别很重要，因为tf假设所有变换都从父变换到子变换。Let's choose the "base_link" coordinate frame as the parent because as other pieces/sensors are added to the robot, it will make the most sense for them to relate to the "base_laser" frame by traversing through the "base_link" frame. 这意味着与连接“base_link”和“base_laser”的边相关联的变换应该是（x：0.1m，y：0.0m，z：0.2m）。设置此变换树后，将在“base_laser”坐标系中接收的激光扫描数据转换到“base_link”坐标系就像调用tf库一样简单。我们的机器人可以使用此信息来推断“base_link”坐标系中的数据，并计划安全的绕开其环境中的障碍物。

2 编写程序

希望上面的例子有助于理解概念层面的问题。现在，我们采用transform tree并使用代码创建它。对于此示例，假设您熟悉ROS，因此如果不熟悉任何术语或概念，请务必查看ROS文档。

假设我们具有上述高级任务，即在“base_laser”坐标系中获取点并将它们转换为“base_link”帧。我们需要做的第一件事是创建一个节点，负责在我们的系统中发布变换。接下来，我们必须创建一个节点来监听通过ROS发布的转换数据并将其应用于转换点。我们首先创建一个源代码包，然后我们给它一个简单的名称，如“robot_setup_tf”我们将依赖于roscpp，tf和geometry_msgs。

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

请注意，您必须在您有权限的情况下运行上面的命令（例如，您可能为之前的教程创建的〜/ ros）。

lternative in fuerte, groovy and hydro：navigation_tutorials堆栈中有一个标准的robot_setup_tf_tutorial包。

按以下方式安装（％YOUR_ROS_DISTRO％可以是{ fuerte，groovy }等）：

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

3 广播变换

现在我们已经得到了我们的包，我们需要创建一个节点来完成通过ROS 广播base_laser → base_link转换的工作。

在刚刚创建的robot_setup_tf包中，启动您喜欢的编辑器并将以下代码粘贴到src / tf_broadcaster.cpp文件中。

#include 
#include 

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包提供了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和z偏离机器人基座20cm。

第三，我们需要将变换发布为时间戳，我们只需加盖它即可ros :: Time :: now（）。

第四，我们需要传递我们正在创建的链接的父节点的名称，在本例中为“base_link”。

第五，我们需要传递我们正在创建的链接的子节点的名称，在本例中为“base_laser”。

4 使用变换

上面，我们创建了一个节点，通过ROS 发布base_laser → base_link转换。现在，我们将编写一个节点，该节点将使用该变换在“base_laser”坐标系中取一个点并将其转换为“base_link”坐标系中的一个点。再一次，我们首先将下面的代码粘贴到一个文件中，然后进行更详细的解释。在robot_setup_tf包中，创建一个名为src / tf_listener.cpp的文件并粘贴以下内容：

#include 
#include 
#include 

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 :: TransformListener所需的tf/transform_listener.h头文件。

一个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。

The "Stamped" on the end of the message name just means that it includes a header, allowing us to associate both a timestamp and a frame_id with the message. We'll set the stamp field of the laser_point message to be ros::Time() which is a special time value that allows us to ask the TransformListener for the latest available transform.至于标题的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”），我们要转换的点，以及存储转换后结果的点。So, after the call to transformPoint(), base_point holds the same information as laser_point did before only now in the "base_link" frame.

  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没有运行），则我们调用transformPoint（）时，TransformListener会抛出异常。

为了确保我们优雅地处理这个问题，我们将捕获异常并为用户打印出错误。

boost:ref()是一个类包装器，为了引用类。。。

5 编译

现在我们已经编写了我们的小例子，我们需要构建它。

打开由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 %TOP_DIR_YOUR_CATKIN_WS%
$ catkin_make

6 运行程序

好的...所以我们都建好了。让我们试一试看看ROS实际上发生了什么。对于本节，您将需要打开三个终端。

在第一个终端中，运行核心。

roscore

在第二个终端，我们将运行我们的tf_broadcaster

rosrun robot_setup_tf tf_broadcaster

现在我们将使用第三个终端来运行我们的tf_listener，将我们的模拟点从“base_laser”坐标系转换为“base_link”坐标系。

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

恭喜，您刚刚为二位激光器编写了一个成功的变换广播器。

下一步是将我们用于此示例的PointStamped替换为来自ROS的传感器流。

幸运的是，您可以在此处找到有关该过程的该部分的文档。