使用tf来配置你的机器人
1. 坐标变换的配置
许多ROS包都需要订阅利用tf库发布的该机器人的变换树。抽象来说，变换树定义了不同的坐标帧之间转换和旋转的偏移量。具体来说，考虑一个简单的机器人，这个机器人有一个移动基座以及一个安装在其上的雷达，我们来定义两个坐标帧：一个是基座的中心，一个是雷达的中心。将这两个坐标帧分别命名为“base_link”（对navigation来说，将base_link放在机器人的旋转中心点是很重要的）和“base_laser”。
接下来假设我们已经有了一些从雷达处获取的关于雷达中心点到其他位置的距离信息，也就是说，我们有了base_laser坐标帧的数据。如果我们需要利用这些数据来让移动基座进行避障的话，我们就需要将base_laser帧的数据变换为base_link帧的数据。本质上也就是需要定义一个base_laser和base_link之间的变换关系。
为了定义这个变换关系，假设我们已经知道了雷达安装比基座中心点前10cm以及高20cm的地方。这也就给了我们base_link 和base_laser之间的转换偏移量。具体来说，如果我们需要将数据从base_laser帧变换到base_link帧的话，我们必须要将坐标加上(x: 0.1m, y: 0.0m, z: 0.2m)，而要从base_link帧变换到base_laser帧的话，就需要将坐标加上(x: -0.1m, y: 0.0m, z: -0.2m)。
看上去似乎我们完全可以自己来进行坐标变换，只要把合适的变化关系存起来，在需要变换的时候再应用上就OK了，但当坐标帧的数量很大时这项工作就不那么轻松了。这时tf就派上用场了，我们不需要自己进行坐标变换，只需要使用tf定义一次base_link和base_laser之间的关系，然后让tf来为我们处理坐标之间的变换就行了。
要使用tf来定义并存储base_link和base_laser之间的变换关系，我们需要将该变换关系添加一个变换树中。从概念上来说，变换树中的每个节点都对应了一个坐标帧，每条边都对应了从当前节点变换到子节点所需要的变换关系。tf使用了树的结构来保证任意两个坐标帧之间的变换都是单向的，并假设所有的边都是直接从父节点连接到子节点的。
为了给我们的例子创建一个变换树，我们将定义两个节点，一个关于base_link的，一个关于base_laser的。为了创建这两个节点之间的边，我们首先需要决定哪个节点将作为父节点，哪个节点作为子节点。这是非常重要的，因为tf假设了所有变换都是从父节点变换到子节点。这里我们选择base_link作为父节点，因为其他的传感器可能还会添加到机器人中。这也就意味着，连接base_link到base_laser之间的边应该是(x: 0.1m, y: 0.0m, z: 0.2m)。建立起这个变换树之后，从雷达处获得的base_laser帧变换到base_link就很容易了，只需要调用tf就OK了。这样我们的机器人也就能利用利用该信息来安全地避障了。
2. 编码
上面这个例子应该能帮助我们从概念上来理解tf的作用。接下来，我们需要在代码上实现上面那个变换树。在继续这个例子前，你应该对ROS比较熟悉了，如果碰到不熟悉的概念就去查ROS Documentation。
将base_laser变换到base_link帧的第一步是，创建一个负责在我们的系统中发布变换关系的节点。接下来，我们需要创建一个节点订阅该变换关系并进行变换。首先，创建一个包来存放我们这个例子的所有源码，将其命名为”robot_setup_tf”并依赖roscpp, tf和geometry_msgs包。
$ cd %TOP_DIR_YOUR_CATKIN_WS%/src
$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs或者在fuerte, groovy和hidro中：有一个navigation_tutorials stack中有一个标准的robot_setupt_tf_tutorial包，安装这个包就可以了：
$ sudo apt-get install ros-%YOUR_ROS_DISTRO%-navigation-tutorials3. 广播一个变换
创建这个包之后，我们需要创建一个用于广播base_laser -> base_link的变换的节点。将下面这段代码copy到刚才创建的包的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，我们需要include 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对象发送变换需要5个参数。
- 旋转变换，由btQuaternion指定的坐标帧之间的旋转变换。这里我们不需要进行变换，将pitch，roll和yaw设为0就可以了；
- 坐标变换，由btVector3指定的变换关系，从上文的分析我们知道从我们想设置的变换关系为(0.1, 0.0, 0.2)；
- 时间戳，我们需要给发送的变换附上一个时间戳，这里直接用ros::Time::now()附上当前时间就可以了；
- 变换树中的父节点名，这里就是base_link了；
- 变换树中的子节点名，这里就是base_laser。
4. 使用变换
上文中我们已经创建了一个发布base_link -> base_laser变换的节点了。接下来我们将编写一个节点使用这个变换并将一个base_laser帧变换为base_link帧。将下面这段代码copy到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>这里我们include了tf/transform_listener.h头文件，因为我们需要创建tf::TransformListener对象，该对象可以自动订阅发布的变换信息topic，并管理接受的所有变换数据。
void transformPoint(const tf::TransformListener& listener){这里我们创建了一个函数，给定一个TransformListener，该函数能将base_laser转换为base_link，该函数也将作为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’表明该消息包含了一个header，可以存储消息的timestamp和frame_id。这里我们将timestamp设为ros::Time()，这是一个特殊的时间值，可以允许我们想TransformListener请求最新的可用变换。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()方法，该方法有三个参数：
- 变换后的frame名，这里就是”base_link”；
- 需要变换的点；
- 以及变换后的点的存储位置。
调用之后base_point中就存储了laser_point变换后的数据了。
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可能会抛出异常。因此我们需要捕捉该异常。
5. build代码
打开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})保存，build这个包。
$ cd %TOP_DIR_YOUR_CATKIN_WS%
$ catkin_make6. 运行代码
构建之后，打开terminal，运行一个core
roscore再打开一个terminal运行tf_broadcaster节点
rosrun robot_setup_tf tf_broadcaster打开第三个terminal运行tf_listener
rosrun tobot_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.19OK，到这里你已经基本成功了，接下来你就需要将例子中的PointStamped替换为ROS中接受的实际的传感器流数据了。关于这一部分，你可以阅读这份文档。
扫一扫
429
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
