ros2 从零开始26 编写静态广播C前言背景发布静态变换对于定义机器人基座与其传感器或固定部件之间的关系非常有用。例如在激光扫描仪中心定义的坐标系中理解激光扫描测量数据最为直观。这是一个独立的教程涵盖静态变换的基础知识共分为两部分。第一部分我们将编写代码向 tf2 发布静态变换。第二部分我们将解释如何使用 tf2_ros 中的命令行工具 static_transform_publisher。在接下来的两个教程中我们将编写代码重现《tf2 简介》教程中的演示示例。之后后续教程将重点介绍如何使用更高级的 tf2 功能扩展该演示。tf2、tf2_ros、ros2的概念见上一章节。一、四元数基础概念Quaternion1. 什么是四元数四元数是一种用于表示三维空间旋转的数学工具由一个实部 (w)和三个虚部 (x, y, z)组成q w xi yj zk2. 为什么使用四元数避免万向节锁欧拉角在特定角度如 pitch±90°时会出现奇异点平滑插值可以方便地进行球面线性插值SLERP计算效率旋转组合只需四元数乘法比矩阵乘法快内存占用小只需存储 4 个浮点数二、6D姿势基础概念1. 消息定义# geometry_msgs/TwistVector3 linear Vector3 angular# geometry_msgs/Vector3float64 x float64 y float64 z2. 物理含义linear线速度m/sx前进/后退速度前进为正y左右平移速度左为正z上下移动速度上为正angular角速度rad/sx滚转角速度rolly俯仰角速度pitchz偏航角速度yaw本章实际是利用tf2变换计算把6D位置信息中的角度angularroll、pitch、yaw转换为四元数Quaternion (w、x、y、z)实践1.创建一个包进入工作区的src目录用如下指令创建我们的包ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies geometry_msgs rclcpp tf2 tf2_ros turtlesim -- learning_tf2_cpprootbc2bf85b2e4a:~/ros2_ws/src# ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies geometry_msgs rclcpp tf2 tf2_ros turtlesim -- learning_tf2_cppgoing to create a new package package name: learning_tf2_cpp destination directory: /root/ros2_ws/src package format:3version:0.0.0 description: TODO: Package description maintainer:[root ********]licenses:[Apache-2.0]build type: ament_cmake dependencies:[geometry_msgs,rclcpp,tf2,tf2_ros,turtlesim]creating folder ./learning_tf2_cpp creating ./learning_tf2_cpp/package.xml creatingsourceand include folder creating folder ./learning_tf2_cpp/src creating folder ./learning_tf2_cpp/include/learning_tf2_cpp creating ./learning_tf2_cpp/CMakeLists.txt新包名字是learning_tf2_cpp,除了基础依赖geometry_msgsrclcpp之外还依赖于tf2、tf2_ros、turtlesim。2.编写静态广播C在learning_tf2_cpp/src目录下创建源文件static_turtle_tf2_broadcaster.cpp, 其内容如下#include memory #include geometry_msgs/msg/transform_stamped.hpp #include rclcpp/rclcpp.hpp #include tf2/LinearMath/Quaternion.h #include tf2_ros/static_transform_broadcaster.h class StaticFramePublisher : public rclcpp::Node { public: explicit StaticFramePublisher(char * transformation[]) : Node(static_turtle_tf2_broadcaster) { tf_static_broadcaster_ std::make_sharedtf2_ros::StaticTransformBroadcaster(this); // Publish static transforms once at startup this-make_transforms(transformation); } private: void make_transforms(char * transformation[]) { geometry_msgs::msg::TransformStamped t; t.header.stamp this-get_clock()-now(); t.header.frame_id world; t.child_frame_id transformation[1]; t.transform.translation.x atof(transformation[2]); t.transform.translation.y atof(transformation[3]); t.transform.translation.z atof(transformation[4]); tf2::Quaternion q; q.setRPY( atof(transformation[5]), atof(transformation[6]), atof(transformation[7])); t.transform.rotation.x q.x(); t.transform.rotation.y q.y(); t.transform.rotation.z q.z(); t.transform.rotation.w q.w(); tf_static_broadcaster_-sendTransform(t); } std::shared_ptrtf2_ros::StaticTransformBroadcaster tf_static_broadcaster_; }; int main(int argc, char * argv[]) { auto logger rclcpp::get_logger(logger); // Obtain parameters from command line arguments if (argc ! 8) { RCLCPP_INFO( logger, Invalid number of parameters\nusage: $ ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw); return 1; } // As the parent frame of the transform is world, it is // necessary to check that the frame name passed is different if (strcmp(argv[1], world) 0) { RCLCPP_INFO(logger, Your static turtle name cannot be world); return 1; } // Pass parameters and initialize node rclcpp::init(argc, argv); rclcpp::spin(std::make_sharedStaticFramePublisher(argv)); rclcpp::shutdown(); return 0; }2.1 代码解析现在我们来看一下与发布静态姿势到 tf2 相关的代码。 开头行geometry_msgs/msg/transform_stamped.hpp包含所需的头文件包含访问消息类型TransformStamped。#include geometry_msgs/msg/transform_stamped.hpp之后包含了rclcpp以便使用其类别rclcpp::Node。#include rclcpp/rclcpp.hpptf2::Quaternion是四元数的一类提供方便的函数用于将欧拉角转换为四元数反之亦然。 我们还包括tf2_ros/static_transform_broadcaster.h使用使静态变换的发布StaticTransformBroadcaster变得简单。#include tf2/LinearMath/Quaternion.h #include tf2_ros/static_transform_broadcaster.hStaticFramePublisher类构造子初始化节点名字为static_turtle_tf2_broadcaster。 然后tf_static_broadcaster_被创建时主动执行一次静态变换。tf_static_broadcaster_ std::make_sharedtf2_ros::StaticTransformBroadcaster(this); this-make_transforms(transformation);这里我们创建一个对象TransformStamped t这个对象将是我们填充后发送的消息。 在传递实际变换值之前我们需要给它相应的元数据。我们需要给发布的变换一个时间戳这里我们用当前时间this-get_clock()-now()我们需要设置我们创建链接的父帧名称在这里是world最后我们需要设置所创建链接子框架的名称在这里是transformation[1]geometry_msgs::msg::TransformStamped t; t.header.stamp this-get_clock()-now(); t.header.frame_id world; t.child_frame_id transformation[1];这里我们填充龟的6D姿势平移和旋转。t.transform.translation.x atof(transformation[2]); t.transform.translation.y atof(transformation[3]); t.transform.translation.z atof(transformation[4]); tf2::Quaternion q; q.setRPY( atof(transformation[5]), atof(transformation[6]), atof(transformation[7])); t.transform.rotation.x q.x(); t.transform.rotation.y q.y(); t.transform.rotation.z q.z(); t.transform.rotation.w q.w();最后我们用函数sendTransform()广播静态变换。tf_static_broadcaster_-sendTransform(t);2.3 更新package.xml返回到目录learning_tf2_cpp文件CMakeLists.txt和package.xml在这一层用文件编辑器打开package.xml如之前提到的修改如下内容:descriptionLearning tf2 with rclcpp/description maintainer emailyouemail.comYour Name/maintainer licenseApache License 2.0/license填写成实际的值然后保存2.4 更新CMakeLists.txt在CMakeLists.txt添加配置可执行程序static_turtle_tf2_broadcaster, 如下add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp) ament_target_dependencies( static_turtle_tf2_broadcaster geometry_msgs rclcpp tf2 tf2_ros )最后添加安装配置install(TARGETS…)install(TARGETS static_turtle_tf2_broadcaster DESTINATION lib/${PROJECT_NAME})3 编译和运行进入工作区用colcon build等待编译完成。rootbc2bf85b2e4a:/# cd ~/ros2_ws rootbc2bf85b2e4a:~/ros2_ws# colcon build --packages-select learning_tf2_cpp Starting learning_tf2_cpp Finished learning_tf2_cpp [5.52s] Summary: 1 package finished [5.87s]编译完成后我们需要安装后才能运行使用source install/setup.sh打开一个终端输入如下命令运行rootbc2bf85b2e4a:~/ros2_ws# source install/setup.sh rootbc2bf85b2e4a:~/ros2_ws# ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0这条命令发布一个位置信息 x0, y 0, z 1离地1米高处。我们现在可以通过查看主题tf_static来确认静态变换是否已发布。 如果一切正常你应该会看到一个静态变换$ros2topicecho/tf_static transforms: - header: stamp: sec:1622908754nanosec:208515730frame_id: world child_frame_id: mystaticturtle transform: translation: x:0.0y:0.0z:1.0rotation: x:0.0y:0.0z:0.0w:1.04 静态变换的正确发布方式本教程旨在展示如何使用StaticTransformBroadcaster静态变换来发布。 在你的实际开发过程中你不应该自己写代码应该用专用工具tf2_ros来完成。tf2_ros提供了一个名为static_transform_publisher的可执行文件可以作为命令行工具使用也可以作为节点添加到你的启动文件中。以下命令向 tf2 发布一个静态坐标变换使世界坐标系world与静态海龟坐标系mystaticturtle之间在 z 轴上产生 1 米的偏移且无旋转。在 ROS 2 中滚转/俯仰/偏航roll/pitch/yaw分别指绕 x/y/z 轴的旋转。ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 1 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id mystaticturtle以下命令向 tf2 发布相同的静态坐标变换但使用四元数表示旋转。ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 1 --qx 0 --qy 0 --qz 0 --qw 1 --frame-id world --child-frame-id mystaticturtlestatic_transform_publisher 既可作为命令行工具手动使用也可在启动文件中用于设置静态变换。例如4.1 XMLlaunchnodepkgtf2_rosexecstatic_transform_publisherargs--x 0 --y 0 --z 1 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id mystaticturtle//launch4.2 YAMLlaunch:-node:pkg:tf2_rosexec:static_transform_publisherargs:--x 0 --y 0 --z 1 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id mystaticturtle4.3 PythonfromlaunchimportLaunchDescriptionfromlaunch_ros.actionsimportNodedefgenerate_launch_description():returnLaunchDescription([Node(packagetf2_ros,executablestatic_transform_publisher,arguments[--x,0,--y,0,--z,1,--yaw,0,--pitch,0,--roll,0,--frame-id,world,--child-frame-id,mystaticturtle]),])总结在本教程中您学习了静态变换如何用于定义坐标系之间的静态关系例如 mystaticturtle 相对于世界坐标系world的关系。此外您还了解了静态变换如何通过将数据关联到公共坐标系来帮助理解传感器数据例如激光扫描仪的数据。最后您编写了自己的节点来向 tf2 发布静态变换并学习了如何使用 static_transform_publisher 可执行文件和启动文件来发布所需的静态变换。