ROS - tf（三）+https://developer.aliyun.com/article/1585199
7.2 编写代码
希望上面的示例能帮助我们从概念上理解 tf。现在，我们需要用代码来创建转换树。在本例中，我们假定对 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）。
在 fuerte、groovy 和 hydro 中的替代方法：在 navigation_tutorials 堆栈中有一个标准的 robot_setup_tf_tutorial 包。您可以按照以下方法安装（%YOUR_ROS_DISTRO% 可以是 { fuerte, groovy } 等）：
sudo apt-get install ros-%YOUR_ROS_DISTRO%-navigation-tutorials
7.3 广播变换
现在我们已经有了自己的软件包，需要创建一个节点，在 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，对应于激光器与机器人基座之间 10 厘米的 x 偏移量和 20 厘米的 z 偏移量。第三，我们需要给正在发布的变换加一个时间戳，我们只需用 ros::Time::now() 给它盖个戳。第四，我们需要传入所创建刚体的父节点名称，本例中为 "base_link"。第五，我们需要传递所创建刚体的子节点的名称，本例中为 "base_laser"。
7.4 使用变换
上面，我们创建了一个节点，通过 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::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;
在这里，我们将以几何_msgs::PointStamped 的形式创建点。信息名称末尾的 "Stamped"（加盖）仅表示它包含一个标头，允许我们将时间戳和帧 ID 与信息关联起来。我们将把 laser_point 信息的 stamp 字段设置为 ros::Time()，这是一个特殊的时间值，允许我们向 TransformListener 询问最新的可用变换。至于报文头的 frame_id 字段，我们将其设置为 "base_laser"，因为我们要在 "base_laser "坐标系中创建一个点。最后，我们将为 point.... 设置一些数据，取值为 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 没有运行），那么当我们调用 transformPoint() 时，TransformListener 可能会抛出异常。为确保从容应对，我们将捕获异常并打印出错误信息提供给用户。
7.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
7.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 传输的传感器流。幸运的是，你可以在这里找到关于这部分过程的文档。
八、在机器人上使用机器人状态发布器 （robot state publisher）
当您使用的机器人有许多相关坐标系时，将它们全部发布到 tf 就成了一项相当艰巨的任务。机器人状态发布器就是一个能帮您完成这项工作的工具。
机器人状态发布器可帮助您向 tf 变换库广播机器人的状态。机器人状态发布器内部有一个机器人运动学模型；因此，在给定机器人关节位置的情况下，机器人状态发布器可以计算并发布机器人每个环节的三维姿态。
您可以将机器人状态发布器作为独立的 ROS 节点或库来使用：
8.1作为 ROS 节点运行
8.1.1 机器人状态发布器（robot_state_publisher）
运行机器人状态发布器最简单的方法就是将其作为一个节点。对于普通用户来说，建议使用这种方式。运行机器人状态发布器需要两样东西：
- 参数服务器上加载的 urdf xml 机器人描述。
- 以 sensor_msgs/JointState 形式发布关节位置的源。
请阅读以下章节，了解如何配置机器人状态发布器的参数和主题。
8.1.1.1 订阅主题
joint_states (sensor_msgs/JointState)
关节位置信息
8.1.1.2 参数
机器人描述（urdf 地图）
urdf xml 机器人描述。可通过 `urdf_model::initParam` 访问
tf_prefix (string)
设置 tf 前缀，用于按名称空间发布变换。详情请参阅 tf_prefix。
publish_frequency (double)
状态发布器的发布频率，默认为 50Hz。
8.1.2 启动文件示例
设置好 XML 机器人描述和关节位置信息源后，只需创建一个类似的启动文件即可：
<launch> <!-- Load the urdf into the parameter server. --> <param name="my_robot_description" textfile="$(find mypackage)/urdf/robotmodel.xml"/> <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" > <remap from="robot_description" to="my_robot_description" /> <remap from="joint_states" to="different_joint_states" /> </node> </launch>
ROS - tf（五）+https://developer.aliyun.com/article/1585203
