ROS2：Humble 教程

序言

机器人操作系统(ROS：Robot Operating System)是一组用于构建机器人应用程序的软件库和工具。从驱动程序和最先进的算法到强大的开发人员工具，ROS为一个机器人项目提供了所需的开源工具。ROS是用于编写机器人软件程序的一种具有高度灵活性的软件架构。

ROS的原型源自斯坦福大学的STanford Artificial Intelligence Robot (STAIR) 和 Personal Robotics (PR)项目。ROS最初设计的目标机器人是PR2，这款机器人搭载了当时最先进的移动计算平台，而且网络性能优异，不需要考虑实时性方面的问题，主要应用于科研领域。如今ROS应用的机器人领域越来越广：轮式机器人、人形机器人、工业机械手、室外机器人（如无人驾驶汽车）、无人飞行器、救援机器人等等，美国NASA甚至考虑使用ROS开发火星探测器，越来越多的机器人开始从科研领域走向人们的日常生活。

随着ROS的发展，ROS1已不能满足开发人员的需求，于是ROS官方重新设计制作了ROS2。ROS2是在ROS1的基础上设计开发的第二代机器人操作系统，可以帮助开发人员简化机器人开发任务。鉴于国内尚无完整的ROS2系统化教程，笔者决定撰写本教程，主要用于自身学习与交流。此教程主要参考自官方文档，目的在于让读者初步了解ROS2的设计理念与使用流程。

读者在参考本教程之前，应具备Linux基础、Python基础、以及一些计算机知识。ROS2很多概念在开始可能难以理解，但学习本来就是从无到有的过程，本教程原则是简洁优雅，尽量使用了通俗易懂的方式进行讲述。ROS2本身是一个工具，作为使用者，我们更关心的是如何学会使用ROS2搭建自己的项目，初学者不必过分纠结底层概念。

本教程只适用于想要学习ROS2的初学者，并不涵盖关于ROS2的高级用法。笔者水平有限，若有不妥之处望请指正！联系邮箱：[email protected]

作者：刘思培

日期：2023.1.1

目录

序言目录前言提醒结构致谢建议版权声明许可声明第一部分 基础篇1 安装1.1 安装ros21.1.1 区域设置1.1.2 添加源1.1.3 安装1.1.3 设置环境变量1.2 安装海龟1.3 安装VS Code1.4 其它1.4.1 卸载1.4.2 推荐1.4.3 提醒2 节点2.1 节点概念 2.2 节点特点2.3 节点指令2.3.1 ros2 node list2.3.2 ros2 node info node_name3 话题3.1 话题概念3.2 话题特点3.3 话题指令3.3.1 ros2 topic list 3.3.2 ros2 topic list -t3.3.3 ros2 topic info topic_name 3.3.4 ros2 topic echo topic_name 3.3.5 ros2 topic hz topic_name3.3.6 ros2 topic bw topic_name3.3.7 ros2 topic pub topic_name msg_type msg_data args4 服务4.1 服务概念4.2 服务特点4.3 服务指令4.3.1 ros2 service list 4.3.2 ros2 service list -t4.3.3 ros2 service type service_name4.3.4 ros2 service find type_name4.3.5 ros2 service call service_name service_type service_data args5 动作5.1 动作概念5.2 动作特点5.3 动作指令5.3.1 ros2 action list5.3.2 ros2 action list -t5.3.3 ros2 action info action_name5.3.4 ros2 action send_goal action_name action_type action_data6 通信接口6.1 通信接口概念6.2 通信接口的消息类型6.2.1 话题的消息类型6.2.2 服务的消息类型6.2.3 动作的消息类型6.2.4 嵌套数据类型6.3 通信接口特点6.4 通信接口指令6.4.1 ros2 interface list6.4.2 ros2 interface show interface_name6.4.3 ros2 interface package package_name7 参数7.1 参数概念7.2 参数特点7.3 参数指令7.3.1 ros2 param list7.3.2 ros2 param get node_name parameter_name7.3.3 ros2 param set node_name parameter_name value7.3.4 ros2 param dump node_name7.3.5 ros2 param load node_name parameter_file 8 工作空间与功能包8.1 工作空间8.1.1 工作空间概念8.1.2 工作空间组成8.2 功能包8.2.1 功能包概念8.2.2 功能包类别8.2.2.1 Cmake功能包8.2.2.2 Python功能包8.3 工作空间、功能包与节点的关系8.3.1 工作空间与功能包8.3.2 功能包与节点9 命名规范9.1 工作空间命名9.2 功能包命名9.3 节点命名9.4 通信接口命名9.5 消息类型命名9.6 基本数据类型9.7 变量命名9.8 常量命名9.9 参数命名第二部分 开发篇10 facer设计10.1 背景介绍10.2 功能要求10.3 模块划分10.4 功能包设计10.5 节点身份设计10.6 通信接口设计11 创建工作空间12 创建通信接口12.1 创建通信接口功能包12.2 创建消息类型文件12.2.1 话题消息类型文件12.2.2 服务消息类型文件12.3.3 动作消息类型文件12.3.4 配置编译选项与添加依赖12.3.4.1 配置编译选项12.3.4.2 添加依赖12.3 查看通信接口列表13 创建节点13.1 创建节点功能包13.2 创建节点13.2.1 创建可执行文件13.2.2 编程接口初始化13.2.3 创建节点并初始化13.2.4 销毁节点并关闭接口13.3.5 添加依赖与入口点13.3.5.1 添加依赖13.3.5.2 添加入口点13.3 创建capture_image节点13.4 创建identify_image节点13.5 创建visual_interaction节点13.6 查看节点列表14 实现节点任务14.1 创建frame_image话题14.1.1 创建frame_image话题的发布者14.1.2 创建frame_image话题的订阅者14.1.3 添加依赖14.2 创建identify_result服务14.2.1 创建identify_result服务的服务端14.2.2 创建identify_result服务的客户端14.2.3 添加依赖14.2.4 查看服务列表14.3 创建camera_state动作14.3.1 创建camera_state动作的服务端14.3.2 创建camera_state动作的客户端14.3.3 添加依赖14.3.4 查看动作列表15 使用参数15.1 创建参数15.2 添加参数描述15.3 修改参数15.3.1 终端指令修改15.3.2 启动文件修改第三部分 工具篇16 编译功能包工具colcon16.1 背景16.2 前提16.3 基础16.3.1 创建工作空间16.3.2 添加源文件16.3.3 设置底层16.3.4 编译功能包16.3.5 运行测试16.3.6 设置环境变量16.3.7 尝试演示16.4 创建功能包16.5 设置colcon_cd16.6 设置colcon命令补全16.7 技巧17 添加依赖工具rosdep17.1 rosdep概念17.2 rosdep密钥17.3 rosdep工作逻辑17.4 填写rosdep密匙17.5 收录软件库17.6 使用方法18 模块化可视化工具rqt18.1 介绍18.2 安装与启动18.3 rqt_graph18.4 rqt_console19 诊断问题工具ros2doctor19.1 背景19.2 前提19.3 示例19.3.1 检查设置19.3.2 检查系统19.3.3 获取报告19.4 总结20 记录与回放工具ros2bag20.1 从命令行记录包20.1.1 背景20.1.2 前提20.1.3 任务20.1.3.1 设置20.1.3.2 选择话题20.1.3.3 记录话题20.1.3.4 记录多个话题20.1.3.5 包文件信息20.1.3.6 回放话题数据20.2 从节点记录包20.2.1 背景20.2.2 前提20.2.3 任务20.2.3.1 创建功能包20.2.3.2 更新package.xml与setup.py20.2.3.3 编写节点20.2.3.4 检查代码20.2.3.5 添加入口点20.2.3.6 编译与运行20.3 从节点记录合成数据20.3.1 编写节点20.3.2 检查代码20.3.3 添加入口点20.3.4 编译与运行20.4 从可执行文件记录合成数据20.4.1 编写可执行文件20.4.2 检查代码20.4.3 添加入口点20.4.4 编译与运行20.5 总结21 启动工具launch21.1 创建启动文件21.1.1 背景21.1.2 前提21.1.3 示例21.1.3.1 设置21.1.3.2 编写启动文件21.1.3.3 检查启动文件21.1.3.4 运行启动文件21.1.4 总结21.2 集成启动文件到功能包21.2.1 背景21.1.2 前提21.1.3 示例21.1.3.1 创建功能包21.1.3.2 创建存放启动文件的目录21.1.3.3 编写启动文件21.1.3.4 编译并运行启动文件文档21.3 使用取代21.3.1 背景21.3.2 前提21.3.3 示例21.3.3.1 创建与设置功能包21.3.3.2 父启动文件21.3.3.3 取代示例启动文件21.3.3.4 编译功能包21.3.4 运行示例21.3.5 修改启动参数文档21.3.6 总结21.4 使用事件处理器21.4.1 背景21.4.2 前提21.4.3 使用事件处理器21.4.3.1 事件处理器的示例启动文件21.4.4 编译功能包21.4.5 运行示例21.4.6 文档21.4.7 总结21.5 管理大型项目21.5.1 背景21.5.2 前提21.5.3 介绍21.5.4 编写启动文件21.5.4.1 顶层组织21.5.4.2 参数21.5.4.2.1 在启动文件设置参数21.5.4.2 加载YAML文件的参数21.5.4.3 在YAML文件中使用通配符21.5.4.3 命名空间21.5.4.4 复用节点21.5.4.5 优先参数21.5.4.6 重新映射21.5.4.7 配置文件21.5.4.8 环境变量21.5.5 运行启动文件21.5.5.1 更新setup.py21.5.5.2 编译与运行21.5.6 总结22 机器人坐标系管理工具tf222.1 tf2简介22.1.1 安装示例22.1.2 运行示例22.1.3 tf2的工具22.1.3.1 view_frames22.1.3.2 tf2_echo22.1.4 rvzi与tf222.2 编写静态广播22.2.1 背景22.2.2 前提22.2.3 示例22.2.3.1 创建功能包22.2.3.2 编写静态广播节点22.2.3.3 发布静态变换的正确方法22.2.4 总结22.3 编写广播22.4 背景22.5 前提22.6 示例22.6.1 编写广播节点22.6.2 检查代码22.6.3 添加入口点22.6.4 编写启动文件22.6.5 添加依赖22.6.6 更新setup.py22.6.7 编译22.6.8 运行22.7 总结22.4 编写监听器22.4.1 背景22.4.2 前提22.4.3 示例22.4.3.1 编写监听节点22.4.3.2 检查代码22.4.3.3 添加入口点22.4.3.4 更新启动文件22.4.3.5 编译22.4.3.6 运行22.4.4 总结22.5 添加坐标系22.5.1 背景22.5.2 tf2 tree22.5.3 示例22.5.3.1 编写固定坐标系广播器22.5.3.1.1 检查代码22.5.3.1.2 添加入口点22.5.3.1.3 编写启动文件22.5.3.1.4 编译22.5.3.1.5 运行22.5.3.2 编写动态坐标系广播器22.5.3.2.1 检查代码22.5.3.2.2 添加入口点22.5.3.2.3 编写启动文件22.5.3.2.4 编译22.5.3.2.5 运行22.5.4 总结22.6 使用时间22.6.1 背景22.6.2 示例22.6.2.1 更新监听器节点22.6.2.2 修复监听器节点22.6.3 总结22.7 时间旅行22.7.1 背景22.7.2 时间旅行22.7.3 lookup_transform()高级接口22.7.4 检查结果22.7.5 总结22.8 四元数基础知识22.8.1 背景22.8.2 前提22.8.3 四元数的组成22.8.4 ROS2中的四元数类型22.8.5 四元数运算22.8.5.1 在RPY中思考，然后转换为四元数22.8.5.2 应用四元数旋转22.8.5.3 反转四元数22.8.5.4 相对旋转数22.8.6 总结23 机器人建模方法URDF23.1 机器人的组成23.1.1 连杆Link的描述23.1.2 关节Joint描述23.2 构建机器人模型23.2.1 一个形状23.2.2 多个形状23.2.3 设置原点23.2.4 设置材料23.2.5 完成模型23.3 构建可移动机器人模型23.3.1 头部23.3.2 夹具23.3.3 夹具臂23.3.4 其它类型关节23.3.5 指定位姿23.3.6 下一步23.4 添加物理属性与碰撞属性23.4.1 碰撞23.4.2 物理属性23.4.3 惯性23.4.4 接触系数23.4.5 关节动力23.4.6 其他标签23.4.7 下一步23.5 使用Xacro简洁代码23.5.1 使用Xacro23.5.2 常量23.5.3 数学23.5.4 宏23.5.4.1 简单宏23.5.4.2 参数化宏23.5.5 实际使用23.5.5.1 腿部宏23.6 将URDF与robot_state_publisher一起使用23.6.1 背景23.6.2 前提23.6.3 任务23.6.3.1 创建功能包23.6.3.2 创建URDF文件23.6.3.3 发布状态23.6.3.4 创建启动文件23.6.3.5 编辑启动文件23.6.3.6 安装功能包23.6.3.7 查看结果23.6.4总结24 三维物理仿真平台Gazebo24.1 介绍24.1.1 安装24.1.2 导入模型库24.1.2.1 下载模型文件24.1.2.2 创建模型文件夹24.1.2.3 拷贝模型文件24.1.2.4 测试24.1.3 Gazebo界面24.1.3.1 场景24.1.3.2 面板24.1.3.3 工具栏24.1.3.4 菜单栏24.1.4 自定义模型24.1.4.1 urdf与sdf24.1.4.2 格式选择24.2 设置机器人仿真24.2.1 前提24.2.2 任务24.2.2.1 启动仿真24.2.2.2 集成ROS2与Gazebo24.2.2.3 可视化ROS 2雷达数据24.3 总结25 测试25.1 从命令行运行ROS2中的测试25.1.1 编译并运行测试25.1.2 检查测试结果25.2 用Python编写基本测试25.2.3 功能包设置25.2.3.1 setup.py25.2.3.2 测试文件和文件夹25.2.3.3 功能包目录层级结构示例：25.2.3.4 测试内容25.2.3.5 特殊命令附录

前言

提醒

在开始学习ROS2之前，笔者先带大家了解一下Ubuntu。想必读者都知道Linux是一套免费并且开放源代码的操作系统。严格来讲，Linux这个词本身只表示Linux内核，而Ubuntu操作系统则是使用Linux内核的操作系统之一。与很多计算机技术相同，ROS2的更新换代是非常迅速的，目前对ROS2兼容最好的系统就是Ubuntu操作系统，所以本教程所有内容是基于目前较新版的Ubuntu22.04 LTS操作系统，对应的ROS2版本是ROS2:Humble。这里也提醒读者要严格安装Ubuntu与ROS2相对应的版本，否则会出现难以解决的错误。

ROS2可使用多种编程语言进行机器人开发，由于笔者其它编程语言水平有限，本教程主要使用的是Python代码。本教程只需一台计算机，无需其它硬件外设。

结构

本教程分为三个部分，第一部分：基础篇；第二部分：开发篇；第三部分：工具篇。

第一部分，基础篇。在基础篇，会先讲解如何安装相关软件。对于初学者，安装并不是一件容易的事情！在安装时，读者会初步体验到如何使用Linux终端指令，除了安装ROS2以外，还需要安装海龟机器人与开发工具VS Code。安装好后，笔者将会使用ROS2提供的终端指令帮助读者理解ROS2的相关概念。机器人是多种功能的综合体，每一个功能包含多个任务，ROS2通过节点实现这些任务，任务之间相互交换信息则是通过称为话题、服务、动作的三种通信接口以及其它通信方式。除此以外，为了开发人员能够实时修改机器人数据，ROS2还有参数这一概念，读者将通过ROS2官方提供的海龟机器人学习这些相关概念。工作空间与功能包，则是ROS2项目与代码的容器。完成以上知识的学习后，会讲解这些专业术语的命名规范。

第二部分，开发篇。在基础篇了解到的ROS2相关概念，在此篇均有涉及。已对ROS相关概念有所了解，需要实际操作的读者，可跳过之前的内容直接阅读此篇。此篇是本教程最重要的一篇，笔者将根据自身学习与开发经验，向读者讲述如何使用ROS2搭配OpenCV开发一个简易的人脸识别机器人，会通过此案例简要总结开发一个ROS2项目应具备的流程。在学习此篇之前，读者最好对计算机视觉和机器学习软件库OpenCV有一定的了解，互联网学习途经很丰富，笔者不再赘述。由于笔者精力有限，在此篇没有逐行对代码进行剖析，但已将所有必要内容添加到了代码注释中。此篇的主要目的在于阐述使用ROS2开发项目的流程，根据此教程案例将其分为以下步骤：架构设计、创建工作空间、创建通信接口、创建节点、实现节点任务、使用参数。项目代码地址为：

https://gitee.com/sipeiliu/facer

第三部分，工具篇。ROS2的强大之处在于有许多可以帮助开发人员提高效率的工具与高级功能，例如有模块化管理工具、机器人坐标系管理工具、三维物理仿真平台、三维可视化显示平台等等，笔者会进行简要介绍。另外，ROS2的许多功能（例如tf2、DDS等）在本教程中并未提及或深入，详情还请参考官方文档。由于部分工具在笔者在工作过程中并未使用到，且此部分内容相对杂乱，此篇大部分内容是笔者根据自身理解对官方文档的原文翻译。其中大部分的专业术语还未形成统一的命名约定，翻译后再阅读会有理解障碍，难免晦涩难懂，所以建议读者尽量阅读官方文档的英文原本，可将此篇作为参考。

致谢

首先感谢张浩田先生在我学习ROS2过程中的提供的帮助，感谢魏鹏先生对本教程提出的宝贵建议。

特别感谢雷世成、肖博（排名不分先后）两位好友帮助我完成本教程的审核校对工作，以及在此期间对我提供的鼓励与支持。

最后感谢相关的优秀开发者们，免费将许多优秀文章发布到互联网，本教程参考并选取了这些文章的部分内容。

建议

由于笔者精力有限，本教程并不涵盖ROS2所有知识要点，在阅读完本教程后，笔者给大家提一些建议。由于ROS2很多资料来源于国外，良好的英文文献阅读能力至关重要，建议读者坚持培养阅读英文的良好习惯。此外，掌握好Linux和Python的知识对未来的ROS2项目开发有益无害。最后，计算机领域的技术日新月异，希望每位读者都能保持自己动手一探究竟的态度。共勉！

除了参考此教程，笔者也推荐这些学习途径：鱼香ROS的《动手学ROS2》、古月居的《ROS2入门21讲图文教程》、创客智造的《ROS2入门教程》、《ROS2官方文档》。其中《ROS2官方文档》虽然是英文，但强烈建议读者阅读，因为应该没有谁比ROS官方更了解ROS2吧！以上资料均可在互联网搜索获取，详情地址请查看附录。

版权声明

本书已申请版权，最终解释权归作者所有。本书参考了大量互联网资料，由于知识零散无法逐一联系到原作者，若有侵权请联系笔者立即删除。

许可声明

Copyright (C) 2023 by 刘思培。你可以在不收取任何费用，而且不修改任何内容的前提下自由分发这本书给任何人。但是本书的内容只允许完整原封不动地进行分发和传播。

第一部分 基础篇

1 安装

如果读者此前只接触过Windows操作系统，那么在安装篇读者会初次体验到Linux终端指令的魅力。由于许多优秀开发者的贡献与Linux开源的特点，许多软件的安装方法并不唯一。本篇主要介绍ROS2的安装，为了给读者呈现完整的ROS2安装细节，笔者只为大家展示官方文档描述的安装方式。

对于初次体验Ubuntu与ROS2的读者，建议使用虚拟机软件。VMware Workstation Player一款免费的桌面虚拟化软件，可在同一计算机上运行一个或多个操作系统。Ubuntu 22.04 LTS操作系统的镜像也可以在官网免费获取。互联网中有许多虚拟机安装操作系统的教程，笔者不再赘述。

VMware Workstation Player官网：

https://www.vmware.com/cn/products/workstation-player.html

Ubuntu官网：

https://ubuntu.com

1.1 安装ros2

由于国内网络环境原因，或者对于不希望在安装细节花太多时间的读者，可直接前往后文的“其它”小节，获取快速安装ROS2与相关软件的方式。

在Ubuntu操作系统下安装ros2，需要进行四个步骤。第一步，打开Ubuntu终端进行区域设置；第二步，添加源；第三步，执行安装指令。第四步，设置环境变量。下面，笔者对这四个步骤进行逐一介绍。

1.1.1 区域设置

区域设置，是特定于语言和地域的规则。不同系统、平台、与软件有不同的区域设置处理方式和不同的设置范围，区域设置的内容包括：数据格式、货币金额格式、小数点符号、千分位符号、度量衡单位、通货符号、日期写法、日历类型、文字排序、姓名格式、地址等等。例如，如果读者在Ubuntu系统中选择了美国英语区域设置，那么在 Ubuntu系统中，日期消息将以格式“月-日-年”显示，而货币符号将会使用“$”表示。

打开终端，输入以下指令显示当前系统的区域设置：

locale # 以键值对形式显示当前系统的区域设置

注释：打开终端快捷键 Ctrl+Alt+T。

LANG= # 默认设定
LANGUAGE= # 系统语言
LC_CTYPE= # 语言符号及其分类
LC_NUMERIC= # 数字
LC_TIME= # 时间显示格式，
LC_COLLATE= # 比较和排序习惯
LC_MONETARY= # 货币单位
LC_MESSAGES= # 信息，如提示信息、错误信息、状态信息、标题、标签、按钮和菜单等
LC_PAPER= # 默认纸张大小
LC_NAME= # 姓名书写方式
LC_ADDRESS= # 地址书写方式
LC_TELEPHONE= # 电话号码书写方式
LC_MEASUREMENT= # 度量衡表达方式
LC_IDENTIFICATION= # locale对自身包含信息的概述
LC_ALL= # 强制设定

其中键的含义笔者已经注释在了代码块中，值即为该健含义所使用的语言环境，这些键值对共同形成了系统的区域设置。语言环境的语法规则为：<语言>_<地区>.<字符集编码>，例如zh_CN.UTF-8中，zh表示中文，CN表示中国大陆，UTF-8表示编码方式。

下面笔者简单介绍一下字符集与编码方式。

字符集是一个系统支持的所有抽象字符的集合。通常以二维表的形式存在，二维表的内容和大小是由使用者的语言而定。如ASCII，GBxxx，Unicode等。

编码是指将人类语言中的字符转换为计算机可以处理的二进制序列的过程。UTF-8是针对Unicode的一种编码方式。它可以用来表示Unicode标准中的任何字符，而且其编码中的第一个字节仍与ASCII相容，使得原来处理ASCII字符的软件无须或只进行少部分修改后，便可继续使用。因此，它逐渐成为电子邮件、网页、其他存储或传送文字的应用中，优先采用的编码方式。

为了保证ROS2可以顺利安装与运行，需要进行区域设置。打开终端，输入以下指令：

sudo apt update && sudo apt install locales # 更新软件包列表并安装区域设置包

sudo locale-gen en_US en_US.UTF-8 # 生成en_US.UTF-8的区域设置包

sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 # 强制设定区域设置为en_US.UTF-8，默认设定区域设置为en_US.UTF-8

export LANG=en_US.UTF-8 # 设置环境变量LANG=en_US.UTF-8

注释：

• sudo是系统管理指令，是允许系统管理员让普通用户执行一些或者全部的root命令的一个工具，可以减少root用户的登录和管理时间，同时也提高了安全性。

• apt是软件包管理工具，可以安装、更新、删除和管理deb软件包。

• export是配置环境变量指令。

1.1.2 添加源

Ubuntu操作系统可使用终端指令直接安装软件，执行此命令时Ubuntu系统会在文件系统中寻找到/etc/apt/sources.list文件，或者在/etc/apt/sources.list.d/文件夹下的独立文件。这些文件称作源列表，源列表里存放着一条条称作源的地址信息，Ubuntu系统根据地址信息找到对应的源服务端上可供下载的软件，然后下载并安装。

总之在Ubuntu中，源是指可以获取软件的地址。安装ROS2之前添加源的目的是告诉Ubuntu系统在哪里下载ROS2。ROS2官方提供的添加源的终端操作步骤如下，打开终端，输入以下指令：

sudo apt install software-properties-common # 安装添加源的工具

sudo add-apt-repository universe # 添加universe源仓库

sudo apt update && sudo apt install curl gnupg lsb-release #更新软件包列表并安装curl,pnupg,lsb-release 

sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg # 授权GPG 密钥

echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null # 添加ros2 apt 存储库到源列表中

注释：

• curl是利用资源地址进行数据或者文件传输的工具。pnupg是加密文件工具。lsb-release是Linux 标准基础版本报告工具，可以帮助识别正在使用的 Linux 发行版以及它们对 Linux 标准基础的兼容性。

• echo指令用于向终端设备上输出字符串，ehco的重定向：echo " "> 文件路径,将双引号中的内容覆盖到对应的文件中。Linux tee命令用于读取标准输入的数据，并将其内容输出成文件。tee指令会从标准输入设备读取数据，将其内容输出到标准输出设备，同时保存成文件。

1.1.3 安装

以上步骤进行完后，就可以进行安装了。打开终端，输入以下指令：

sudo apt update # 更新软件包列表

sudo apt upgrade # 将软件包升级到最新版本

sudo apt install ros-humble-desktop # 安装ros2的humble桌面版本

sudo apt install ros-humble-ros-base # 安装ros2 humble基础的通信库、消息包、指令工具

ROS2安装的默认目录在/opt/ros/下，根据版本的名字进行区分。我们安装的是Humble版本的ROS2，所以安装目录在/opt/ros/humble下。

1.1.3 设置环境变量

为了让ROS2能够找到功能包，还需要设置环境变量。打开终端，输入以下指令：

echo " source /opt/ros/humble/setup.bash" >> ~/.bashrc # 设置始终执行该脚本文件,使ros2的软件包可在所有打开的新终端中使用

注释：

• shell是命令语言解释器，终端程序执行时会自动调用shell程序，在提示符下输入的每个命令都由shell先解释然后传给Linux内核。shell有多种，bash是Ubuntu系统默认的shell，而.bash文件就是bash的脚本文件。source指令用于执行setup.bash文件。

• echo "" >> 文件路径，将双引号中的内容追加到对应的文件中。~ 是指当前用户的home目录，.bashrc是home目录下的一个shell文件，用于储存用户的个性化设置。在bash每次启动时都会加载.bashrc文件中的内容，并根据内容定制当前bash的配置和环境。

1.2 安装海龟

海龟（Turtlesim）是ROS2官方为了帮助读者理解ROS2的相关概念，由ROS2官方开发并可使用终端指令安装并操控的简易机器人。海龟能够说明ROS2在最基本的层面上做了什么，让读者知道如何处理一个真实的机器人或机器人模拟。在本教程的概念篇，读者将通过操作海龟理解到ROS2的设计理念及相关术语。

安装海龟。打开终端，输入以下指令：

sudo apt update # 更新软件包列表

sudo apt install ros-humble-turtlesim # 安装海龟功能包

检查是否成功安装：

ros2 pkg executables turtlesim # 查看海龟功能包的可执行文件

如果出现了以下可执行文件，则说明安装成功：

turtlesim draw_square

turtlesim mimic

turtlesim turtle_teleop_key

turtlesim turtlesim_node

打开一个新终端，运行海龟：

ros2 run turtlesim turtlesim_node

此时显示器会弹出模拟器窗口，中间有一只随机的海龟。再打开一个新的终端，运行海龟控制器：

ros2 run turtlesim turtle_teleop_key

按照终端的提示，使用键盘上的箭头键来控制弹出窗口的海龟， 海龟将可以在屏幕上平移和旋转。到这里，已经成功安装了海龟机器人并对它实现了简单的操控。

注释：Ctrl+C中断进程，返回到终端界面。

1.3 安装VS Code

VS Code是微软公司开发的一款轻量级代码编辑器，它具有开源免费、轻量级、跨平台、可扩展等优势。它强大的插件功能使其十分适用于ROS2项目开发，当然读者也可以选择适合自己的开发工具。笔者推荐使用以下指令，可快速在Ubuntu安装VS Code。

互联网有大量在Ubuntu操作系统安装VS Code的方法，可以通过后文的一键安装指令安装，或者打开一个新的终端输入以下指令：

sudo apt install snap # 安装snap工具
sudo snap install --classic code # 安装VS Code

注释： snap是Linux的一种全新软件包管理工具，类似一个容器拥有一个应用程序所有的文件和库，各个应用程序之间完全独立。使用snap包解决了应用程序之间的依赖问题，使应用程序之间更容易管理。

点击Ubuntu桌面左下角的应用程序菜单按钮，可以找到VS Code软件图标，点击即可打开。为了更高效的开发ROS2，一些VS Code的扩展插件是必备或者推荐的，例如Python、ROS、Chinese(Simplified)等，直接在扩展搜索框就能搜索到。VS Code扩展商店提供了大量优秀的插件，读者可自行探索。

1.4 其它

1.4.1 卸载

如果在从二进制文件安装后需要卸载 ROS2 或切换到基于源代码的安装，打开终端，输入以下指令：

sudo apt remove ~nros-humble-* && sudo apt autoremovesudo rm /etc/apt/sources.list.d/ros2.list

sudo apt update

sudo apt autoremove

sudo apt upgrade

1.4.2 推荐

由于国内网络原因，这里笔者给大家推荐ROS2相关软件的一键安装指令：

wget http://fishros.com/install -O fishros && . fishros

1.4.3 提醒

ROS2自带了Python解释器，安装好ROS2后无需额外安装Python解释器。

ROS2的许多功能需要保持网络畅通，这也是国内开发者的一大痛点。

建议初学者学会使用虚拟机快照功能，以防误操作导致系统数据丢失，但此功能会消耗大量存储空间，需要谨慎使用。

建议读者学会使用分布式版本控制系统GIT，可以有效、高速地管理项目代码。

2 节点

2.1 节点概念

节点是基本的ros2单元，用于机器人系统中的模块化。学习过编程的读者应该知道，函数是把实现一个功能的代码集合在一起的代码块，它能实现某一具体的任务，并通过参数传递数据。在各种程序实际开发过程中，函数有利于代码重复利用与合作开发等，节点与函数有许多共同之处。

一个节点是一个利用ROS系统和其他节点通信的实体，是一个在ROS网络中的参与者。ROS2中的每个节点应负责单个模块目的，用于实现某一具体任务。一个完整的机器人系统由许多协同工作的节点组成。

在下图中，这些圆圈代表了某个机器人的部分节点，这些节点都有各自的任务，例如一个节点用于控制电机，一个节点用于控制测距仪等。节点不是独立存在的，节点之间需要相互沟通才能更好的工作。连线代表了沟通的管道，可以理解为生活中的网线。方框代表了节点间进行沟通的方式，例如在日常生活中，我们可以通过打电话进行沟通，那么沟通方式就是声音；也可以通过发短信进行沟通，那么沟通方式就是文字。小圆点和小椭圆代表了沟通的具体内容，例如我们可以打电话通过声音联系朋友，也可以发短信通过文字联系朋友。不同的沟通方式对应着不同的类型，所以图中使用了小圆点和小椭圆区分。

2.2 节点特点

节点用于执行某些具体的任务。将某一具体的功能交给某一具体的节点有利于机器人的模块化，也会大大提高节点的可复用性。节点之间除了非必要的数据传递，应保持良好的低耦合高内聚状态。

节点是可以独立运行的可执行文件。即使节点没有收到或发出数据，它也是可以独立运行的。开发者可随时运行某一个单独的节点，就像是打开它的电源，而能否正常工作，则取决于它是否获取到了有效的数据。

每一个节点都需要有唯一的命名。比如我们运行的两个节点/teleop_turtle、/turtlesim，它们都有各自唯一的名字，不然创造出的机器人就好比分不清左右手，这样就不合理了。

节点可以是分布式的。例如一个机器人的节点a位于计算机甲，节点b却位于计算机乙。当搭建协作项目和大型项目时，节点可分布式的特点能够发挥巨大的作用。

每个节点都可以通过主题、服务、动作等通信机制向其他节点发送和接收数据。话题、动作、服务就是节点之间进行沟通的方式。

2.3 节点指令

接下来，我们通过海龟机器人来了解与节点有关的终端指令。首先，根据前面的教程，打开一个新终端，运行海龟：

ros2 run turtlesim turtlesim_node # 运行海龟

再打开一个新的终端，运行海龟控制器：

ros2 run turtlesim turtle_teleop_key # 运行海龟控制器

2.3.1 ros2 node list

该指令用于查看节点列表，将显示所有正在运行的节点的名称。打开一个新的终端，输入以下指令：

ros2 node list # 查看节点列表

结果如下：

/teleop_turtle

/turtlesim

由此可知，当在终端输入指令ros2 run turtlesim turtlesim_node时，ROS2会运行turtlesim_node这个可执行文件，从而启动了一个名为/turtlesim的节点。同理，当在终端输入指令ros2 run turtlesim turtle_teleop_key时，ROS2会运行turtle_teleop_key这个可执行文件，从而启动了一个名为/teleop_turtle的节点。

2.3.2 ros2 node info node_name

节点启动后，便可以使用终端指令查看节点详细信息。打开一个新的终端，输入以下指令：

ros2 node info /turtlesim # 查看/turtlesim节点的详细信息，将显示该节点的身份

可以看到以下输出结果：

Subscribers:
...
Publishers:
...
Service Servers:
...
Service Clients:
...
Action Servers:
...
Action Clients:
...

Subscribers显示了节点订阅的话题，Subscribers显示了节点发布的话题。Service Servers显示了节点是哪些服务的服务端，Service Servers显示了节点是哪些服务的客户端。Action Servers显示了节点是哪些动作的服务端，Action Clients显示了节点是哪些动作的客户端。例如在 /turtlesim节点的Subscribers中：

/turtlesim
	Subscribers:
		/parameter_events: rcl_interfaces/msg/ParameterEvent
...

说明/turtlesim节点是话题/parameter_events的订阅者，且话题/parameter_events的类型为rcl_interfaces/msg/ParameterEvent。

3 话题

3.1 话题概念

ROS2将复杂的机器人分解成许多模块化节点，节点实现了机器人各种各样的功能，但节点并不是孤立的，节点之间会进行数据传递，话题就是节点之间传递数据的方式之一，话题是发布/订阅模式。

节点可以发布任意数量的不同话题，并且可以同时订阅任意数量的不同话题。话题就像是一个信息交流站，这个情报站点只负责传达某一种具体类型的信息，所有知道这个信息交流站的节点，都能向它推送或订阅数据。总之，节点的身份具有多样性，在明确话题名称和数据类型的前提下，可以是任意话题的发布者，也可以是任意话题的订阅者。发布者和订阅者使用的话题名称和消息类型必须匹配才能允许它们进行通信。

下图中，圆圈代表了节点。方框代表了通信机制采用话题，节点内的椭圆代表了节点的身份。连线代表了通信管道。连线上的椭圆代表了通过话题传递的具体信息。在下图中，发布该话题的节点有两个，订阅该话题的节点也有两个。

3.2 话题特点

话题是发布/订阅模型。话题的发布者不会将消息直接发送给特定的订阅者，无需了解哪些订阅者（如果有的话）可能存在。同样的，订阅者可以表达对一个或多个话题的兴趣，只接收感兴趣的话题，无需了解哪些发布者（如果有的话）存在。

节点可通过话题实现多对多通信。多个节点之间相互的通信可以同时进行，且通信之间互不干扰，一个具体的话题就是一个具体的独立数据交换点。由于话题的这个特点，当两个节点在同时间发布一个话题的时候，就需要注意优先级的问题了。

话题的通信方式属于异步通信。某个节点发布一个话题后，发布节点的话题并不知道订阅的节点什么时候能够接收到。因此，话题适合一些周期发布的数据。

3.3 话题指令

接下来，我们通过海龟机器人来了解与话题有关的终端指令。首先，根据前面的教程，打开一个新终端，运行海龟：

ros2 run turtlesim turtlesim_node # 运行海龟

再打开一个新的终端，运行海龟控制器：

ros2 run turtlesim turtle_teleop_key # 运行海龟控制器

3.3.1 ros2 topic list

该指令用于查看话题列表，将显示所有正在运行的节点发布的话题。打开一个新的终端，输入以下指令：

ros2 topic list # 查看话题列表

结果如下：

/parameter_events
/rosout
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

3.3.2 ros2 topic list -t

该指令用于查看话题列表，将显示所有正在运行的节点发布的话题。但是末尾的-t参数，会在话题名称后的中括号中显示话题类型。打开一个新的终端，输入以下指令：

ros2 topic list -t # 查看话题列表，附加话题类型

结果如下：

/parameter_events [rcl_interfaces/msg/ParameterEvent]
/rosout [rcl_interfaces/msg/Log]
/turtle1/cmd_vel [geometry_msgs/msg/Twist]
/turtle1/color_sensor [turtlesim/msg/Color]
/turtle1/pose [turtlesim/msg/Pose]

3.3.3 ros2 topic info topic_name

该指令用于查看某个话题的信息，将显示话题的消息类型与发布者、订阅者数量。打开一个新的终端，输入以下指令：

ros2 topic info /turtle1/pose # 查看话题信息

结果如下：

Type: turtlesim/msg/Pose
Publisher count: 3
Subscription count: 0

说明/turtle1/pose话题的消息类型是turtlesim/msg/Pose，发布该话题的节点有3个，订阅该话题的节点有0个。

3.3.4 ros2 topic echo topic_name

该指令用于查看某个话题传送的具体数据。打开一个新的终端，输入以下指令：

ros2 topic echo /turtle1/pose # 查看话题数据

结果如下：

x: ...
y: ...
theta: ...
linear_velocity: ...
sngular_vvelocity: ...
---
x: ...
y: ...
theta: ...
linear_velocity: ...
angular_velocity: ...
---
...

可以看到，终端在实时输出海龟在模拟窗口中的位置坐标、旋转角度、线速度、角速度。从而也可以判断出话题/turtle1/pose传送的具体数据是海龟的位姿。

3.3.5 ros2 topic hz topic_name

该指令用于查看某个话题的发布频率。打开一个新的终端，输入以下指令：

ros2 topic hz /turtle1/pose # 查看话题发布频率

结果如下：

average rate: ...
		min: ... max: ... std dev: ... window: ...
average rate: ...
		min: ... max: ... std dev: ... window: ...
...

3.3.6 ros2 topic bw topic_name

该指令用于查看某个话题的传输带宽。打开一个新的终端，输入以下指令：

ros2 topic bw /turtle1/pose # 查看话题传输带宽

结果如下：

WARNING:topic [/turtle1/pose] does not appear to be published yet
Subscribed to [/turtle1/pose]
... KB/s from ... messages
		Message size mean: ...KB min: ...KB max: ...KB
... KB/s from ... messages
		Message size mean: ...KB min: ...KB max: ...KB
...		

3.3.7 ros2 topic pub topic_name msg_type msg_data args

该指令用于发布话题消息。打开一个新的终端，输入以下指令：

ros2 topic pub --once /turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}"  # 发布话题消息,--once是一个可选参数，表示发布一条消息然后退出，若要以频率n HZ发布则使用--rate n

结果如下：

publisher: beginning loop
publishing #1: geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=2.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=1.8))

在前面的教程中，我们使用了键盘控制海龟的行为，可以看到除此以外，还可以通过终端指令的方式控制海龟。

4 服务

4.1 服务概念

服务是节点的另一种通信机制，基于服务端/客户端模型 。我们日常浏览网页就是服务端/客户端模型。服务是你问我答的同步通信，客户端在需要某些数据的时候，针对某个具体的服务，发送请求信息，服务端收到请求之后，就会进行处理并返回应答信息。作为服务端的节点只能有一个，作为客户端的节点可以任意数量。

假设有两个节点A，B。A节点的身份是服务端，B节点的身份是客户端，B节点可以向A节点请求数据，A节点收到B节点的数据请求后，会将数据回答给B节点。

下图中，圆圈代表了节点。方框代表了通信机制采用服务，节点内的椭圆代表了节点的身份。连线代表了通信管道。连线上的小圆点代表了通过话题传递的具体信息。在下图中，作为服务客户端的节点有两个。

4.2 服务特点

服务是客户端/服务端模型。客户端/服务端模型是一种分布式应用程序，用于在资源或服务的提供者和服务请求者之间划分任务或工作负载。客户端在需要某些数据的时候，针对某个具体的服务，发送请求信息，服务端收到请求之后，就会进行处理并返回应答信息。例如浏览网页时，电脑浏览器就是客户端，向网站服务端发送请求，服务端收到之后返回需要展现的数据。

节点可通过服务实现一对多通信。作为服务客户端的节点可以有任意个，但作为服务服务端的节点只能有一个。一对多通信保证了数据的唯一性，每个做为服务客户端的节点接收到的数据是相同的。

服务的通信方式属于同步通信。同步通信的特点是响应快，所以相比话题，在服务通信中，作为客户端的节点可以通过接收到的应答信息，迅速判断作为服务端的节点状态。

4.3 服务指令

接下来，我们通过海龟机器人来了解与服务有关的终端指令。首先，根据前面的教程，打开一个新终端，运行海龟：

ros2 run turtlesim turtlesim_node # 运行海龟

再打开一个新的终端，运行海龟控制器：

ros2 run turtlesim turtle_teleop_key # 运行海龟控制器

4.3.1 ros2 service list

该指令用于查看服务列表。打开一个新的终端，输入以下指令：

ros2 service list # 查看服务列表

结果如下：

/clear
/kill
/reset
/spawn
/teleop_turtle/describe_parameters
/teleop_turtle/get_parameter_types
/teleop_turtle/get_parameters
/teleop_turtle/list_parameters
/teleop_turtle/set_parameters
/teleop_turtle/set_parameters_atomically
/turtle1/set_pen
/turtle1/teleport_absolute
/turtle1/teleport_relative
/turtlesim/describe_parameters
/turtlesim/get_parameter_types
/turtlesim/get_parameters
/turtlesim/list_parameters
/turtlesim/set_parameters
/turtlesim/set_parameters_atomically

4.3.2 ros2 service list -t

该指令用于查看服务列表，附加服务类型。打开一个新的终端，输入以下指令：

ros2 service list -t # 查看服务列表

结果如下：

/clear [std_srvs/srv/Empty]
/kill [turtlesim/srv/Kill]
/reset [std_srvs/srv/Empty]
/spawn [turtlesim/srv/Spawn]
...
/turtle1/set_pen [turtlesim/srv/SetPen]
/turtle1/teleport_absolute [turtlesim/srv/TeleportAbsolute]
/turtle1/teleport_relative [turtlesim/srv/TeleportRelative]
...

4.3.3 ros2 service type service_name

该指令用于查看服务消息类型。打开一个新的终端，输入以下指令：

ros2 service type /spawn # 查看服务类型

结果如下:

turtlesim/srv/Spawn

说明/spawn服务的消息类型是turtlesim/srv/Spawn。

4.3.4 ros2 service find type_name

该指令用于查找使用某一具体服务消息类型的所有服务。打开一个新的终端，输入以下指令：

ros2 service find turtlesim/srv/Spawn # 查找特定消息类型的所有服务

结果如下：

/spawn

说明只有/spawn这个服务使用了turtlesim/srv/Spawn这种消息类型。

4.3.5 ros2 service call service_name service_type service_data args

该指令用于发送服务请求。打开一个新的终端，输入以下指令：

ros2 service call /spawn turtlesim/srv/Spawn "{x: 2, y: 2, theta: 0.2, name: ''}"

结果如下：

requester: making request: turtlesim.srv.Spawn_Request(x=2.0, y=2.0, theta=0.2, name='')

response:
turtlesim.srv.Spawn_Response(name='turtle2')

可以看到，在窗口中生成了一只新的海龟，至此我们也逐渐明白服务/spawn的作用是在指定位置复制一只新的海龟，并返回新海龟名字。

5 动作

5.1 动作概念

动作是 ROS 2 中的通信机制之一，用于对机器人某一完整行为的流程进行管理。动作是一种应用层的通信机制，适用于长时间运行的任务。它们由三部分组成：目标，反馈和结果。动作使用客户端/服务端模型，类似于发布者/订阅者模型，因为其底层就是基于一个话题和两个服务来实现的。作为动作客户端的节点将目标发送到动作服务端节点，作为动作服务端的节点确认目标并进行实时反馈，最后返回结果。

动作基于主题和服务。它的功能类似于服务，只是动作可以取消。动作与服务的区别在于，动作提供稳定的反馈，而服务返回单个响应。

下图中，圆圈代表了节点。方框代表了通信机制采用动作，节点内的方框代表了节点的身份。连线代表了通信管道。连线上的小圆点代表了通过动作传递的具体信息。

5.2 动作特点

动作采用客户端/服务端模型，动作是一种应用层的通信机制，其底层就是基于话题和服务来实现的。当客户端发送运动目标时，使用的是服务的请求调用，服务端也会反馈一个应答，表示收到命令。动作的反馈过程，其实就是一个话题的周期发布，服务端是发布者，客户端是订阅者。从上图中可以看到，一个动作实际上是由两个服务和一个话题封装形成的。

一对多通信，和服务一样，动作通信中的客户端可以有多个，大家都可以发送动作请求，但是服务端只能有一个。

动作属于同步通信，既然有反馈，那动作也是一种同步通信机制。

5.3 动作指令

接下来，我们通过海龟机器人来了解与动作有关的终端指令。首先，根据前面的教程，打开一个新终端，运行海龟：

ros2 run turtlesim turtlesim_node # 运行海龟

再打开一个新的终端，运行海龟控制器：

ros2 run turtlesim turtle_teleop_key # 运行海龟控制器

5.3.1 ros2 action list

该指令用于查看动作列表。打开一个新的终端，输入以下指令：

ros2 action list # 查看动作列表

结果如下：

/turtle1/rotate_absolute

5.3.2 ros2 action list -t

该指令用于查看动作列表，附加动作类型在中括号内。打开一个新的终端，输入以下指令：

ros2 action list -t

结果如下：

/turtle1/rotate_absolute [turtlesim/action/RotateAbsolute]

5.3.3 ros2 action info action_name

该指令用于查看动作信息。打开一个新的终端，输入以下指令：

ros2 action info /turtle1/rotate_absolute

结果如下：

Action: /turtle1/rotate_absolute
Action clients: 1
    /teleop_turtle
Action servers: 1
    /turtlesim

5.3.4 ros2 action send_goal action_name action_type action_data

该指令用于发送动作请求。打开一个新的终端，输入以下指令：

ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: -1.57}" --feedback # 参数--feedback用于查看此目标的反馈

结果如下：

Sending goal:
   theta: -1.57

Goal accepted with ID: e6092c831f994afda92f0086f220da27

Feedback:
  remaining: -1.5700000524520874

Feedback:
  remaining: -1.5540000200271606
  
…

Feedback:
  remaining: -0.03400003910064697

Feedback:
  remaining: -0.018000006675720215

Result:
  delta: 1.5520000457763672

Goal finished with status: SUCCEEDED

至此我们也逐渐明白动作/turtle1/rotate_absolute的作用是请求海龟旋转一定的角度，并实时反馈完成的进度，最后返回的结果是海龟的绝对角度。

6 通信接口

6.1 通信接口概念

在前面教程中，读者已经了解到了话题、服务、动作的概念，这三者统称为ROS2的通信接口。通信接口就像是我们生活中的沟通方式，通过每一种通信接口的消息类型，各种节点才能有机的联系到一起。

6.2 通信接口的消息类型

在前文中，我们通过对海龟机器人的终端指令了解到了这三种通信接口的存在，每一种通信接口都有各自的消息类型，以文件的形式保存与调用。消息类型类似于编程语言的结构体，是承载具体数据的容器，且一种消息类型可以被多个通信接口使用。为了提高ROS2使用者的开发效率，ROS2提供了许多标准消息类型，这些标准消息类型的文件可以在ROS2安装路径中的ros/humble/share文件夹中找到。

也可以根据自己的需求，创建自定义消息类型文件。

6.2.1 话题的消息类型

打开/opt/ros/humble/share/turtlesim/msg目录，可以看到有两个后缀为.msg的文件，分别为Color.msg、Pose.msg。其中Pose.msg就是一个话题类的消息类型文件，Pose就称为话题turtle1/pose的消息类型。打开该文件，查看Pose消息类型的内容如下：

float32 x
float32 y
float32 theta

float32 linear_velocity
float32 angular_velocity

可以看到，话题的消息类型就像编程语言的结构体一样。先指定变量的数据类型，再指定变量的名称，最后多个变量组成这个消息类型。也就是说，turtle1/pose这个话题推送的是海龟的位姿信息，由横坐标x、纵坐标y、转角theta、线速度linear_velocitylinear_velocity、角速度angular_velocity构成，这五条数据都是32位的浮点型，它们的值是可以变化的。

话题类的消息类型文件后缀为.msg。

6.2.2 服务的消息类型

打开/opt/ros/humble/share/turtlesim/srv目录，可以看到有一个名为Spawn.srv的文件。其中Spawn.srv就是一个服务类的消息类型文件，Spawn就称为服务/spawn的消息类型。打开该文件，查看其消息类型的内容如下：

float32 x
float32 y
float32 theta
string name # Optional.  A unique name will be created and returned if this is empty
---
string name

在服务一节中的最后，中我们已经了解到/spawn服务的作用是接受生产新海龟请求并反馈新海龟名字。x、y、theta、name是作为客户端的节点通过服务这种通信机制发送给作为服务端节点的请求数据，作用是指定新海龟的坐标、转角、名称；name是服务端返回给客户端的反馈数据，作用是提示客户端已生成了一只新海龟。

与话题消息类型不同的是，服务的消息类型有请求和反馈两种数据。可以看到在服务的消息类型中，请求与反馈数据之间使用"---"分隔开，请求的数据在"---"上方定义，反馈的数据在"---"下方定义。

服务类的消息类型文件后缀为.srv。

注释：在ROS2的消息类型中，使用与Python相同的注释方法，即#号与"""号。

6.2.3 动作的消息类型

打开/opt/ros/humble/share/turtlesim/action目录，可以看到有一个名为RotateAbsolute.action的文件。RotateAbsolute.action是一个动作类的消息类型文件，RotateAbsolute就是动作/turtle1/rotate_absolute的消息类型。打开该文件，查看其消息类型的内容如下：

# The desired heading in radians
float32 theta
---
# The angular displacement in radians to the starting position
float32 delta
---
# The remaining rotation in radians
float32 remaining

在动作一节中的最后，我们已经知道了/turtle1/rotate_absolute的作用。其中theta是请求的目标，具体为请求海龟旋转的角度；delta是返回的结果，具体为海龟最后的绝对角度；remaining是实时的反馈，具体为还需要旋转的角度。

与服务不同的是，动作有目标、反馈、结果三种数据。可以看到在其消息类型中，请求、反馈、目标数据之间使用"---"分隔开，请求的数据在第一个"---"上方定义，反馈的数据在两个"---"之间定义，返回的结果数据在第二个"---"下方定义。

动作类的消息类型文件后缀为.action。

6.2.4 嵌套数据类型

嵌套数据类型是指在一个消息类型文件中包含了其它消息类型文件，相当于C语言言中在结构体嵌套其它结构体。嵌套数据类型有利于避免重定义，提高复用性。例如现在需要在一个名为B的服务类消息类型文件中使用名为B的话题类消息类型文件，则需要创建两个文件并填写以下内容：

• A.msg

uint32 a
uint8 b

• B.srv

# 请求
A c
---
# 响应
bool status

6.3 通信接口特点

通信接口的特点取决于其使用的消息类型文件，消息类型文件是可嵌套的，每一种消息类型的特点如下：

话题消息类型文件的后缀为.msg，由于是单向传输，只需要定义单一的数据。

服务消息类型文件的后缀为.srv，包含请求和返回两部分，通过文件内的“---”区分。

动作消息类型文件的后缀为.action，包含请求、反馈、结果三部分，通过文件内的的“---”区分。

6.4 通信接口指令

接下来，我们通过海龟机器人来了解与通信接口有关的终端指令。首先，根据前面的教程，打开一个新终端，运行海龟：

ros2 run turtlesim turtlesim_node # 运行海龟

再打开一个新的终端，运行海龟控制器：

ros2 run turtlesim turtle_teleop_key # 运行海龟控制器

6.4.1 ros2 interface list

该指令用于查看通信接口列表。打开一个新的终端，输入以下指令：

ros2 interface list # 查看通信接口列表

结果如下：

Messages:
action_mags/msg/GoalInfo
...
Services:
action_msgs/srv/CancelGoal
...
Actions:
action_tutorials_interfaces/action/Fibonacci
...

运行后会出现一长串的通信接口，可见数据交互的重要性等同于人类的神经网络。

6.4.2 ros2 interface show interface_name

该指令用于查看某个通信接口的消息类型。打开一个新的终端，输入以下指令：

ros2 interface show action_mags/msg/GoalInfo # 查看接口消息类型

结果如下：

# Goal ID
unique_identifier_msga/UUID goal_id
		#
		uint8[16] uuid
		
# Time when the goal was accepted
builtin_interfaces/Tome stamp
		int32 sec
		uint32 nanosec

6.4.3 ros2 interface package package_name

该指令用于查看某个功能包中的所有通信接口。打开一个新的终端，输入以下指令：

ros2 interface package turtlesim # 查看功能包中的通信接口

结果如下：

turtlesim/srv/SetPen
turtlesim/msg/Color
turtlesim/srv/Spawn
turtlesim/srv/Kill
turtlesim/msg/Pose
turtlesim/srv/TeleportRelative
turtlesim/srv/TeleportAbsolute
turtlesim/action/RotateAbsolute

7 参数

7.1 参数概念

参数类似编程语言中的全局变量，是ROS2机器人系统中的全局字典。通过参数可以在多个节点中共享数据，它与话题、服务、动作这三种通信接口共同构成了ROS2的通信机制。

ROS2参数与ROS2节点相关联，参数可用于在系统运行过程中从外部配置节点。参数的生存期与节点的生存期相关（尽管节点可以实现某种持久性以在重启后重新加载这些值）。参数使用节点名称、节点命名空间、参数名称和参数命名空间寻址，其中参数命名空间是可选的。

每个参数都包含一个键和一个值，其中键是一个字符串，值可以是以下类型之一：bool，int，double，string，byte []，bool []，int []，double [] 或string[]。

7.2 参数特点

参数是ROS2的全局字典。在ROS系统中，参数是以全局字典的形态存在的，什么叫字典？就像真实的字典一样，由名称和数值组成，也叫做键和值，合成键值。或者我们也可以理解为，就像编程中的参数一样，有一个参数名 ，然后跟一个等号，后边就是参数值了，在使用的时候，访问这个参数名即可。

参数可动态监控。在ROS2中，参数的特性非常丰富，比如某一个节点共享了一个参数，其他节点都可以访问，如果某一个节点对参数进行了修改，其他节点也有办法立刻知道，从而获取最新的数值。这在参数的高级编程中，大家都可能会用到。

7.3 参数指令

7.3.1 ros2 param list

该指令用于查看参数列表。打开一个新的终端，输入以下指令：

ros2 param list # 查看参数列表

结果如下：

/teleop_turtle:
  qos_overrides./parameter_events.publisher.depth
  qos_overrides./parameter_events.publisher.durability
  qos_overrides./parameter_events.publisher.history
  qos_overrides./parameter_events.publisher.reliability
  scale_angular
  scale_linear
  use_sim_time
/turtlesim:
  background_b
  background_g
  background_r
  qos_overrides./parameter_events.publisher.depth
  qos_overrides./parameter_events.publisher.durability
  qos_overrides./parameter_events.publisher.history
  qos_overrides./parameter_events.publisher.reliability
  use_sim_time

7.3.2 ros2 param get node_name parameter_name

该指令用于查询某个参数的数据类型和当前值。打开一个新的终端，输入以下指令：

ros2 param get /turtlesim background_g # 查看参数的类型和值

结果如下：

Integer value is: 86

7.3.3 ros2 param set node_name parameter_name value

该指令用于修改某个参数的值。打开一个新的终端，输入以下指令：

ros2 param set /turtlesim background_g 150 # 修改参数值

结果如下：

Set parameter successful

说明修改成功。

7.3.4 ros2 param dump node_name

该指令用于将某个节点的参数保存到参数文件中。默认情况下，该命令打印为标准输出 （stdout），但您也可以将参数值重定向到文件中以保存以供以后使用。要将 参数的当前配置保存到文件中，打开一个新的终端，输入以下指令：

ros2 param dump /turtlesim > turtlesim.yaml # 保存参数到文件turtlesim.yaml

您将在运行 shell 的工作目录中找到一个新文件。如果打开此文件，您将看到以下内容：

/turtlesim:
  ros__parameters:
    background_b: 255
    background_g: 150
    background_r: 69
    qos_overrides:
      /parameter_events:
        publisher:
          depth: 1000
          durability: volatile
          history: keep_last
          reliability: reliable
    use_sim_time: false

7.3.5 ros2 param load node_name parameter_file

该指令用于重新加载parameter_flie文件中的所有参数值，赋值给节点。打开一个新的终端，输入以下指令：

ros2 param load /turtlesim turtlesim.yaml # 加载文件参数值

结果如下：

Set parameter background_b successful
Set parameter background_g successful
Set parameter background_r successful
Set parameter qos_overrides./parameter_events.publisher.depth failed: parameter 'qos_overrides./parameter_events.publisher.depth' cannot be set because it is read-only
Set parameter qos_overrides./parameter_events.publisher.durability failed: parameter 'qos_overrides./parameter_events.publisher.durability' cannot be set because it is read-only
Set parameter qos_overrides./parameter_events.publisher.history failed: parameter 'qos_overrides./parameter_events.publisher.history' cannot be set because it is read-only
Set parameter qos_overrides./parameter_events.publisher.reliability failed: parameter 'qos_overrides./parameter_events.publisher.reliability' cannot be set because it is read-only
Set parameter use_sim_time successful

8 工作空间与功能包

8.1 工作空间

8.1.1 工作空间概念

工作空间就是存放ROS2项目的文件夹。工作空间的命名是任意的，习惯命名为name_ws。为了隔离外界以形成独立的开发环境，与更好的管理项目，所有与ROS2项目相关的文件，都应该放置在工作空间中。

8.1.2 工作空间组成

完整的工作空间主要包含以下四个子文件夹：

• src：代码空间，用于存放功能包。即未来编写的代码、脚本，都需要放置到这里。src就是source code （源代码）的简写。

• build：编译空间，保存编译过程中产生的中间文件。 对于每个功能包，将创建一个子文件夹。

• install：安装空间，放置编译得到的可执行文件和脚本。是每个功能包将安装到的位置， 默认情况下，每个包功能都将安装到单独的子目录中。

• log：日志空间，编译和运行过程中，保存各种警告、错误、信息等日志。包含有关每次调用构建工具colcon 的各种日志记录信息。

其中，build、install、log这三个文件夹在编译后都会自动生成，如何编译在后文会讲解，现在只有src文件夹是我们最关心的，因为它需要我们自己创建。src文件夹是放置源代码的文件夹，里面用来存放我们创建的功能包，进而在功能包中编写代码。

8.2 功能包

8.2.1 功能包概念

工作空间包含有功能包，功能包可以被视为ROS2代码的容器。如果希望能够共享或者更好的管理代码，那么需要将代码组织在一个功能包中。每个功能包都位于各自的文件夹中，这些文件夹放置在工作空间的src文件夹中，可以简单的把功能包理解为src文件下的独立文件夹。

一个工作空间的src文件夹中可以包含多个功能包，一个功能包可以包含多个可执行文件。自定义通信接口、节点等功能，都是在功能包中编写代码实现的。

ROS2的功能包有两种，Cmake功能包和Python 功能包。可以在一个工作空间中包含不同（CMake、Python 等）生成类型的包，但是不能有嵌套包。

8.2.2 功能包类别

8.2.2.1 Cmake功能包

在讲解Cmake功能包之前，笔者先简述一下Cmake工具。CMake是一个跨平台的构建工具，可以用简单的语句来描述所有平台的安装(编译过程)。能够输出各种各样的makefile或者project文件。Cmake 并不直接建构出最终的软件，而是产生其他工具（如Makefile ）的脚本，然后再依这个工具的构建方式使用。以CMake编写的源文件以CMakeLists.txt命名或以.cmake为扩展名。

简而言之，Cmake通过调用CMakeLists.txt直接生成Makefile。一个工程中的源文件不计其数，其按类型、功能、模块分别放在若干个目录中，Makefile定义了一系列的规则来指定哪些文件需要先编译，哪些文件需要后编译，哪些文件需要重新编译，甚至于进行更复杂的功能操作。Makefile文件中的命令有一定规范，一旦该文件编写好以后，在Linux命令行中执行一条make命令即可自动编译整个工程。

Cmake功能包主要包含以下文件夹以及文件。

• src：源代码目录。我们自己创建的C++（后缀名为.cpp）文件就放置在此文件夹中。

• package.xml：该文件包含与此功能包相关的元信息。我们可以打开该文件，在里面填写功能包的说明和许可证声明等信息。

• CMakeLists.txt：描述如何在功能包中生成代码的文件。ROS2的编译工具colcon就是通过CMakeLists.txt编译功能包的。

在 ROS 2 中创建新Cmake功能包的指令为：

cd ~/ros2_ws/src # 进入src目录

ros2 pkg create --build-type ament_cmake  # 创建Cmake功能包，为功能包名称

8.2.2.2 Python功能包

在讲解Python功能包之前，笔者先简述一下Setuptools工具。Setuptools是Python自带的用来构建包的工具，构建出来的wheel(.whl)可供其他人安装与导入。Setuptools使开发者构建和发布Python包更加容易，包的使用者无需安装Setuptools就可以使用该包。setup.py是模块分发与安装的指导文件，在里面导入Setuptools工具编写完成后，执行命令即可自动编译整个工程。

Python功能包主要包含以下文件夹以及文件：

• package_name：与此功能包同名的文件夹，使ROS2工具能够查找到此功能包。包含有init.py文件。我们自己创建的Python文件（后缀名为.py）就放置在此文件夹中。

• package.xml：该文件包含与此功能包相关的元信息。我们可以打开该文件，在里面填写功能包的说明和许可证声明等信息。

• setup.cfg：该文件使ROS2运行指令能够查找到此功能包的可执行文件。

• setup.py：该文件包含有关如何安装此功能包的说明，会自动读取setup.cfg中的设置。该文件包含与package.xml文件中相同的功能包说明和许可证声明等信息，因此也需要设置这些字段在两个文件中完全匹配。ROS2的编译工具colcon就是通过setup.py编译Python功能包的。

在 ROS 2 中创建新Python功能包的指令为：

cd ~/ros2_ws/src # 进入src目录

ros2 pkg create --build-type ament_python package_name # 创建Cmake功能包，package_name为功能包名称

8.3 工作空间、功能包与节点的关系

8.3.1 工作空间与功能包

从前文可知，单个工作空间可以包含任意数量的功能包，每个功能包都位于各自的文件夹中。 还可以在一个工作空间中包含不同生成类型（CMake、Python 等）的功能包，但是不能有嵌套功能包。最佳做法是在工作空间的src文件夹下创建功能包。 这样可以保持工作空间的顶层“干净”。单个工作空间与功能包的目录结构应具有如下的层级关系：

ros2_ws
	src
		package_c
			include
			src
			CMakeLists.txt
			package.xml
 	  	package_python
 	  		package_python
 	  		resource
 	  		test
 	  		package.xml
 	  		setup.cfg
      		setup.py
      		resource
      			package_python
  		...

8.3.2 功能包与节点

一个功能包可以包含多个可执行文件，这些可执行文件可以是创建节点的文件，也可以是有其它用途的脚本文件。一个功能包可以包含多个节点，我们应该只将为达成某一共同目的而创建的多个节点放置在一个功能包。如果将节点比作细胞，那么功能包就是由这些细胞组成的器官。

不将所有节点都放在一个功能包的原因有两个，一是因为ROS2的功能包可以打包后供他人直接单独安装，当一个功能包中的某些功能需要修改时，不会影响其它功能包的正常使用，有利于模块化与分发部署。二是因为功能包中的节点数量是任意的，当机器人某个功能的细节需要优化或完善时，只需要进入到实现此功能的功能包中添加新的节点，有利于扩展。

以Python功能包为例，创建节点的Python文件就放置在与功能包同名的子文件夹中。单个功能包与可执行文件的目录结构应具有如下的层级关系：

ros2_ws
	src
 	  	package_python
 	  		package_python
 	  			node_1.py
 	  			node_2.py
 	  			...
 	  			script_1.py
 	  			script_2.py
 	  			...
 	  		resource
 	  		test
 	  		package.xml
 	  		setup.cfg
      		setup.py
      		resource
      			package_python

9 命名规范

9.1 工作空间命名

工作空间的命名采用小写字母加下划线的格式，应该一目了然这是一个ROS2的工作空间。工作空间的命名通常与项目名相关，例如在创建篇我们要创建一个名为facer的机器人项目，那么工作空间最好命名为facer_ws，ws是workspace的缩写。如果想不到合适的命名方式，不妨命名为ros2_ws或dev_ws：

ros2_ws

9.2 功能包命名

功能包的命名同样采用小写字母加下划线的格式，应该突出功能包的功能。例如facer机器人负责输入模块功能的功能包，命名为：

facer_input

9.3 节点命名

节点的命名同样采用小写字母加下划线的格式，应该突出节点的任务是什么。例如例如facer机器人负责输入模块功能的功能包中，有一个节点的任务是采集图像，那么将它命名为：

capture_image

9.4 通信接口命名

话题、服务、动作等通信接口的命名采用小写字母加斜杠“/”的格式，例如facer机器人中有一个话题传递的数据与图像相关，那么就可以将话题命名为:

facer/image

9.5 消息类型命名

消息类型的命名采用驼峰命名法，单词首字母大写。例如facer机器人有一个服务通信接口传递的数据是图像检测结果，那么此服务使用的消息类型就可以命名为：

DetectResult

9.6 基本数据类型

通信接口的消息类型文件是由基本的数据类型组成的，每种数据类型都可以用来定义数组。ROS2支持的基本数据类型如下表所示：

9.7 变量命名

消息类型文件中的变量命名方式与C语言类似，先声明变量数据类型，再声明变量名称，最后可以给变量赋予默认值。变量名称必须以小写字母开始，同时以下划线作为单词的分割符。不能以下划线结束，也不允许有两个连续的下划线。以下是几个示例：

数据类型 变量名称 变量默认值（可无）
uint8 x 42
int16 y -2000
string full_name "John Doe"
int32[] samples [-200, -100, 0, 100, 200]

9.8 常量命名

常量名必须是大写，常量通过等号进行赋值。同样需要先声明数据类型，再声明变量名称，最后给常量赋值。以下是几个示例：

数据类型 常量名称=常量值
int32 X=123
int32 Y=-123
string FOO="foo"
string EXAMPLE='bar'

9.9 参数命名

参数需要在程序中定义，每个参数都包含一个键和一个值，其中键是一个字符串，值可以是以下类型之一：bool，int，double，string，byte []，bool []，int []，double [] 或string[]。当编程语言为Python时，示例如下：

self.declare_parameter('robot_name', 'mbot') # 创建一个参数，并设置参数的默认值

第二部分 开发篇

10 facer设计

10.1 背景介绍

RCL（ROS Client Library）是ROS2的客户端库，其实就是ROS2的一种API，提供了对ROS2话题、服务、参数、动作等接口。针对Python语言，ROS2提供了rclpy库来操作ROS2的节点；针对C++，ROS2提供了rclcpp库来操作ROS2的节点。所以后面我们使用Python来编写ROS2节点实现通讯等功能时，我们就会导入rclpy库。

OpenCV（开源计算机视觉）是英特尔于 1999 年推出的跨平台机器视觉库。专注于实时图像处理，并包括最新机器视觉算法的无专利实现。2008年，Willow Garage接管了支持，OpenCV现在带有C，C++，Python和Android等语言的编程接口。OpenCV是在BSD许可证下发布的，因此它可被用于学术项目和商业产品。OpenCV现在带有用于人脸识别的全新FaceRecognizer类，因此使用OpenCV可以轻松的实现人脸识别功能。

使用ROS2开发项目，经笔者总结需要进行的步骤有：第一步，创建工作空间；第二步，创建消息类型；第三步，创建节点；第四步，实现节点任务；第五步，编译功能包；第六步，运行节点。创建一个项目并不是一蹴而就的，在这个过程中我们会不断地进行调试修改，所以部分步骤是可以反复交叉进行的。除此以外，还将使用ROS2的参数在终端修改facer机器人。

10.2 功能要求

在之前的内容中，我们一直使用了海龟机器人来理解ROS2的相关概念。在理解到ROS2如何设计机器人的理念后，是时候自己动手创建一个属于自己的机器人了。在开发篇，笔者会教大家如何创建一个名为facer的机器人。为了简化流程，facer机器人的功能是简单且易于实现的，读者不必过分担心难度。

facer机器人的功能是可以实现简易的人脸识别。facer在采集到摄像头的视频流后，判断出镜头中是否出现了人脸，最后将判断结果输出。

10.3 模块划分

为了实现此功能，程序划分为三个模块：

输入模块：主要功能为采集图像。视频流是由许多帧图像组成的，该模块在采集到视频流后，需要将每一帧图像传送给处理模块。此外，为了让使用者更好的了解facer的工作细节，输入模块还将监测摄像头运行状态，实时发送给输出模块。

处理模块：主要功能为识别图像。接收来自输入模块的图像，通过与数据模型文件对比后，得出结论。此外，为了让我们在交互界面看到结论，处理模块还会将人脸识别结果传递给输出模块。

输出模块：主要功能为人机交互。接收来自于输入模块的摄像头运行状态信息，接收来自于处理模块的人脸识别结果。最后，在终端输出接收到摄像头运行状态信息与人脸识别结果。

模块功能与模块之间的关系如下图所示：

10.4 功能包设计

结合ros2的设计理念，一个机器人是许多功能的集合，功能包用于实现单个功能，功能包中的节点用于实现单个功能的单个任务。根据对facer机器人的模块划分，对应的我们将facer分为输入、处理、输出等三个功能包，每个功能包都含有一个节点。

名为facer_input的功能包，负责与输入模块相关的功能。facer_input功能包的主要任务是采集图像，因此包含了一个名为capture_image的节点。

名为facer_handle的功能包，负责与处理模块相关的功能。facer_handle功能包主要任务是识别图像，因此包含了一个名为identify_image的节点。

名为facer_output的功能包，负责与输出模块相关的内容。facer_output功能包主要任务是人机交互，因此包含了一个名为visual_interaction的节点。

功能包与节点之间的关系如下图所示：

10.5 节点身份设计

facer_input节点与facer_handle节点之间，需要传递的是图像数据。图像数据来源于我们的电脑摄像头，facer需要将视频流转换为一帧一帧的图像传递，这个过程是周期进行的。从前文可知，话题适用于周期发布的数据，所以我们使用话题这种通信接口传递图像，facer_input节点是图像话题的发布者，facer_handle是图像话题的订阅者。最后，我们将图像话题命名为frame_image。

facer_handle节点与facer_output节点之间，需要传递的是人脸识别结果。人脸识别是通过调用OpenCV人脸识别相关函数实现的，机器视觉跟人一样，识别是需要时间的，在人脸识别结果出来后才能传递。从前文可知，服务适用于按需请求的数据，服务端收到请求之后，就会进行处理并返回应答信息，所以我们使用服务这种通信接口传递人脸识别结果。最后，我们将服务命名为face_result。

facer_output节点与facer_input节点之间，需要传递的是摄像机状态。摄像机状态有打开与关闭两种，在facer机器人做出人脸识别结果之前应保持摄像机打开状态，并将状态实时输出，这是需要持续一段时间的事件。从前文可知，动作适用于运行需要耗时的任务，服务端收到请求之后，就会实时反馈任务状态，所以我们使用动作这种通信接口传递摄像机状态。最后，我们将动作命名为camera_status。

总结下来，节点的身份如下：

capture_image节点，它是frame_image话题的发布者，camera_state动作的服务端。

identify_image节点，它是frame_image话题的订阅者，face_result服务的客户端。

visual_interaction节点，它是camera_state动作的客户端，face_result服务的服务端。

整个facer机器人的节点身份如下图所示：

10.6 通信接口设计

frame_image话题，我们使用标准通信接口Image，它是图像相关数据的载体。

face_result服务，我们使用自定义通信接口FaceResult，它是人脸识别结果相关数据的载体。

camera_state动作，我们使用自定义通信接口CameraState，它是摄像机状态相关数据的载体。

至此，我们已经将整个facer机器人的架构描绘出来了。整个facer机器人的ROS2网络如下图所示：

在后面的内容，我们将正式使用ROS2实现facer机器人。

11 创建工作空间

首先，我们来创建facer的工作空间。从前文可知，工作空间就是一个存放ROS2工程的文件夹。创建工作空间的流程就是创建文件夹的流程，命名为与项目相关的名字facer_ws。通常我们将工作空间放在用户主目录下。创建好工作空间的文件夹后，应该顺带创建子文件夹src，其名称src是source的缩写，从名字可以得知此文件夹是我们放置源代码的位置。除了使用VS Code等编辑器，还可以使用终端指令直接创建多级文件夹。

打开终端，进入用户主目录创建一个名为facer_ws的工作空间，并在此工作空间目录下创建src文件夹。输入以下指令：

mkdir -p ~/facer_ws/src # 创建~/facer_ws/src多级目录

注释：~代表用户主目录，例如笔者~目录的绝对路径为/home/sipeiliu；mkdir指令用于创建目录，-p参数可使其创建多级目录。

facer的工作空间在创建好后应该有如下目录层级结构：

facer_ws
	src

12 创建通信接口

在ROS2中，节点使用话题、服务、动作等通信接口是以导入其消息类型文件的方式实现的，例如Python使用import语句。在创建话题、服务、动作之前，最好先将其消息类型文件准备好。ROS2官方虽然推荐开发者使用其提供的标准消息类型文件，但这些标准消息类型文件时常是不能完全满足需求的，所以通常都会在项目中创建自定义消息类型文件。

12.1 创建通信接口功能包

为了使ROS2能够找到导入的自定义消息类型文件，创建的自定义消息类型文件需要放置在一个Cmake功能包中，通常将此功能包称为通信接口功能包。请读者注意通信接口功能包是一个 CMake 功能包，目前没有办法使用纯 Python 功能包实现通信接口功能，CMake功能包中创建的自定义消息类型文件，可以在Python功能包中导入使用。ROS2提供了创建功能包的终端指令，接下来让我们创建一个用于存放自定义消息类型文件的Cmake功能包。打开终端，输入以下指令进入facer_ws/src文件夹：

cd ~/facer_ws/src # 进入facer_ws/src目录

进入facer_ws/src文件夹后，输入以下指令创建用于存放消息类型的通信接口功能包：

ros2 pkg create --build-type ament_cmake facer_interfaces # 创建Cmake功能包facer_interfaces

注释：pkg表示功能包相关的功能，create表示创建功能包，--build-type表示新创建功能包的语言环境，ament_cmake是指创建Cmake功能包，语言环境为C++，最后跟上新建功能包的名字facer_interfaces。

12.2 创建消息类型文件

在通信接口功能包的目录下，新建三个分别名为msg、srv、action文件夹。msg、srv、action的含义分别为message、service、action，由此可知这三个文件夹分别用于存放话题类通信接口的消息类型文件(后缀为.msg)、服务类通信接口的消息类型文件(后缀为.srv)、动作类通信接口的消息类型文件(后缀为.action)。

打开终端，输入以下指令进入通信接口功能包目录：

cd ros2_ws/src/facer_interfaces # 进入通信接口功能包目录

输入以下指令创建msg文件夹：

mkdir msg # 创建msg文件夹

输入以下指令创建srv文件夹：

mkdir srv # 创建srv文件夹

输入以下指令创建action文件夹：

mkdir action # 创建action文件夹

或者直接输入以下指令：

mkdir msg srv action # 创建msg、srv、action文件夹

在将功能包和文件夹创建好后，终于可以创建自定义的消息类型文件了。facer机器人有三个通信接口：名为frame_image的话题，用于传递一帧一帧的图像；名为face_result的服务，用于传递人脸识别结果；名为camera_state的动作，用于传递摄像机状态。每一种通信接口都有各自的消息类型文件，接下来逐一讲解并创建。

12.2.1 话题消息类型文件

frame_image话题是负责采集图像的capture_image节点与负责识别图像的identify_image节点之间的通信接口，用于传递一帧一帧的图像。frame_image话题的消息类型文件，使用标准消息类型文件Image.msg，可直接导入使用。虽然本案例中话题的消息类型无需自己创建，但是在前文中我们依然创建了存放自话题类自定义消息类型文件的文件夹msg，因为有一天如果想要修改机器人的功能，可能会需要创建话题类消息类型文件。Image.msg可以在ROS2安装路径中的ros/humble/share/sensor_msgs/msg文件夹中找到,文件打开如下：

# This message contains an uncompressed image

# (0, 0) is at top-left corner of image

std_msgs/Header header # Header timestamp should be acquisition time of image

                             # Header frame_id should be optical frame of camera

                             # origin of frame should be optical center of cameara

                             # +x should point to the right in the image

                             # +y should point down in the image

                             # +z should point into to plane of the image

                             # If the frame_id here and the frame_id of the CameraInfo

                             # message associated with the image conflict

                             # the behavior is undefined

uint32 height                # image height, that is, number of rows
uint32 width                 # image width, that is, number of columns

# The legal values for encoding are in file src/image_encodings.cpp

# If you want to standardize a new string format, join

# [email protected] and send an email proposing a new encoding.

string encoding       # Encoding of pixels -- channel meaning, ordering, size

                      # taken from the list of strings in include/sensor_msgs/image_encodings.hpp

uint8 is_bigendian    # is this data bigendian?
uint32 step           # Full row length in bytes
uint8[] data          # actual matrix data, size is (step * rows)

12.2.2 服务消息类型文件

face_result服务是负责识别图像的identify_image节点与负责人机交互的visual_interaction节点之间的通信接口，用于传递人脸识别结果。face_result服务的消息类型文件，使用自定义消息类型文件FaceResult.srv。打开终端，输入以下指令，进入通信接口功能包的srv文件夹：

cd ~/facer_ws/src/facer_interfaces/srv # 进入srv文件夹

输入以下指令，创建自定义消息类型文件FaceResult.srv：

touch FaceResult.srv

此时文件内容是空的，使用编辑器打开文件填入如下内容：

bool apply_result # 申请结果
---
string get_result # 获取结果

“---”上方的布尔类型的apply_result变量，是服务的请求数据。当其值为True时，服务端identify_image节点会接受客户端visual_interaction节点的申请并传递人脸识别结果。具体如何用代码实现会在后文讲解。

“---”下方的字符串类型的get_result变量，是服务的反馈数据。服务端identify_image节点将人脸检测结果存放在get_result变量中返回给客户端visual_interaction节点。具体如何用代码实现会在后文讲解。

12.3.3 动作消息类型文件

camera_state动作是负责人机交互的visual_interaction节点与负责采集图像的capture_image节点之间的通信接口，用于传递摄像机状态。camera_state动作的消息类型文件，使用自定义消息类型文件CameraState.action。打开终端，输入以下指令，进入通信接口功能包的action文件夹：

cd ~/facer_ws/src/facer_interfaces/action # 进入srv文件夹

输入以下指令，创建自定义消息类型文件CameraState.action：

touch CameraState.action

此时文件内容是空的，使用编辑器打开文件填入如下内容：

bool start # 动作开始
---
string finish # 动作结束
---
string state # 反馈状态

第一个“---”上方的布尔类型的start变量，是动作的请求数据。当变量start值为True时，服务端capture_image节点会接受客户端visual_interaction节点的申请并执行动作。具体如何用代码实现会在后文讲解。

两个“---”之间的字符串类型的finish变量，是动作的结果数据。在动作执行完成后，服务端capture_image节点将通过finish变量，返回动作完成的提示信息给客户端visual_interaction节点。具体如何用代码实现会在后文讲解。

第二个“---”下方的字符串类型的state变量，是动作的周期反馈数据。动作开始执行时，服务端capture_image节点将按照一定的频率，通过state变量返回摄像机状态信息给客户端visual_interaction节点。具体如何用代码实现会在后文讲解。

12.3.4 配置编译选项与添加依赖

12.3.4.1 配置编译选项

自定义消息类型文件创建完成后，需要在功能包的CmakeLists.txt中配置编译选项，让编译器在编译过程中，根据接口定义，自动生成不同语言的代码。由于frame_image话题的消息类型文件，使用标准消息类型文件Image.msg，可直接导入使用，所以此案例中只需要配置FaceResult.srv与CameraState.action。

打开通信接口功能包facer_interfaces的CmakeLists.txt，填写以下内容：

cmake_minimum_required(VERSION 3.8)
project(facer_interfaces)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies

find_package(ament_cmake REQUIRED)

# uncomment the following section in order to fill in

# further dependencies manually.

# find_package( REQUIRED)

# “添加代码”
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/FaceResult.srv"
  "action/CameraState.action"
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)

  # the following line skips the linter which checks for copyrights

  # comment the line when a copyright and license is added to all source files

  set(ament_cmake_copyright_FOUND TRUE)

  # the following line skips cpplint (only works in a git repo)

  # comment the line when this package is in a git repo and when

  # a copyright and license is added to all source files

  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

另外，笔者提一下此案例中没有涉及到的特殊情况。假设需要使用并创建了一个名为DemoMsg.msg的消息类型文件，其中包含了以下内容：

geometry_msgs/Point center
float64 radius

可以看到此自定义消息类型使用了来自另一个通信接口功能包（在本例中为geometry_msgs/Point）的消息类型文件。则在CmakeLists.txt中应该填写：

cmake_minimum_required(VERSION 3.8)
project(facer_interfaces)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies

find_package(ament_cmake REQUIRED)

# uncomment the following section in order to fill in

# further dependencies manually.

# find_package( REQUIRED)

# “添加代码”
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/DemoMsg.msg"
  "srv/FaceResult.srv"
  "action/CameraState.action"
  DEPENDENCIES geometry_msgs # 添加上述消息类型所依赖的功能包，在本例中为DemoMsg.msg添加geometry_msgs，如果没有则不填写此行代码。
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)

  # the following line skips the linter which checks for copyrights

  # comment the line when a copyright and license is added to all source files

  set(ament_cmake_copyright_FOUND TRUE)

  # the following line skips cpplint (only works in a git repo)

  # comment the line when this package is in a git repo and when

  # a copyright and license is added to all source files

  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

12.3.4.2 添加依赖

因为通信接口依赖于rosidl_fault_generator来生成特定于语言的代码，所以需要声明对它的依赖。标记用于指定运行时或执行阶段依赖项，而rosidl_interface_packages是功能包所属的依赖项组的名称，使用标记声明。所以，通信接口功能包facer_interfaces的package.xml文件中也需要添加代码生成功能依赖。打开package.xml文件，输入以下内容：

geometry_msgs # 添加上述消息类型所依赖的功能包，在本例中为DemoMsg.msg添加geometry_msgs，如果没有则不填写此行代码。

rosidl_default_generators

rosidl_default_runtime

rosidl_interface_packages

12.3 查看通信接口列表

现在我们来简单学习一下如何在终端查看刚才自定义的通信接口列表，以此检测我们是否成功创建了通信接口。

打开终端，输入以下指令进入facer_ws工作空间：

cd ~/facer_ws # 进入工作空间

编译通信接口功能包facer_interfaces后，ROS2才可以发现存放在其中的通信接口。编译功能包的详细内容将在后文讲解，现在先输入以下指令编译功能包：

colcon build # 编译功能包

若在工作空间生成了build、install、log等三个文件夹且无报错信息，则功能包编译成功。接下来，还需要设置环境变量让ROS2发现facer_interfaces功能包，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

最后，终于可以输入以下指令查看通信接口列表了：

ros2 interface list # 查看通信接口列表

在终端输出的列表仔细查找，会看到我们自定义的通信接口如下：

Messages:
...
Services:
...
facer_interfaces/srv/FaceResult
...
Actions:
...
facer_interfaces/action/CameraState
...

现在，自定义通信接口功能包的所有内容都已就绪，目录层级结构如下：

facer_ws
	build
	install
	log
	src
		facer_interfaces
			CamkeLists.txt
			package.xml
			msg
			srv
				FaceResult.srv
			action
				CameraState

13 创建节点

在前面我们已经创建好了facer通信接口的消息类型，现在让我们来创建facer的三个节点。

13.1 创建节点功能包

因为Python的可执行文件（*.py）是放置在Python功能包的同名子文件夹中的，所以首先需要创建Python功能包。ROS2提供了创建Python功能包的终端指令，为了简化操作使逻辑清晰，我们先一口气把facer的三个功能包都创建好。

打开终端，输入以下指令，进入工作空间的src目录：

cd ~/facer_ws/src # 功能包放在src目录下

输入以下指令，创建facer_input功能包：

ros2 pkg create --build-type ament_python facer_input #创建功能包facer_input

输入以下指令，创建facer_handle功能包：

ros2 pkg create --build-type ament_python facer_handle #创建功能包facer_handle

输入以下指令，创建facer_output功能包：

ros2 pkg create --build-type ament_python facer_output #创建功能包facer_output

注释：pkg表示功能包相关的功能，create表示创建功能包，--build-type表示新创建功能包的语言环境，ament_python是指创建Python功能包，语言环境为Python，最后跟上新建功能包的名字。

之后打开每一个功能包的package.xml与setup.py，确保填写功能包描述、维护者信息、许可证等相关信息。

facer_input功能包：

  facer_input
  0.0.0
  采集图像
  sipeiliu
  Apache License 2.0

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

同样的，打开facer_input功能包的setup.py，维护者姓名、维护者邮箱、功能包描述与package.xml填写的一致。

from setuptools import setup

package_name = 'facer_handle'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='sipeiliu',
    maintainer_email='[email protected]',
    description='采集图像',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

facer_handle功能包：

  facer_handle
  0.0.0
  识别图像
  sipeiliu
  Apache License 2.0

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

同样的，打开facer_input功能包的setup.py，维护者姓名、维护者邮箱、功能包描述与package.xml填写的一致。

from setuptools import setup

package_name = 'facer_handle'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='sipeiliu',
    maintainer_email='[email protected]',
    description='识别图像',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

facer_output功能包：

  facer_output
  0.0.0
  人机交互
  sipeiliu
  Apache License 2.0

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

同样的，打开facer_input功能包的setup.py，维护者姓名、维护者邮箱、功能包描述与package.xml填写的一致。

from setuptools import setup

package_name = 'facer_output'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='sipeiliu',
    maintainer_email='[email protected]',
    description='人家交互',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

至此，目录的层级结构如下：

facer_ws
	build
	install
	log
	src
		facer_interfaces
			CamkeLists.txt
			package.xml
			msg
			srv
				FaceResult.srv
			action
				CameraState
		facer_input
			facer_input
			resource
			test
			package.xml
			setup.cfg
			setup.py
		facer_handle
			facer_handle
			resource
			test
			package.xml
			setup.cfg
			setup.py
		facer_output
			facer_output
			resource
			test
			package.xml
			setup.cfg
			setup.py

注释：其中Apache License是著名的非盈利开源组织Apache采用的协议。该协议和BSD类似，同样鼓励代码共享和尊重原作者的著作权，同样允许代码修改，再发布（作为开源或商业软件）。需要满足的条件也和BSD类似：需要给代码的用户一份Apache License。如果你修改了代码，需要在被修改的文件中说明。在延伸的代码中（修改和有源代码衍生的代码中）需要带有原来代码中的协议，商标，专利声明和其他原来作者规定需要包含的说明。如果再发布的产品中包含一个Notice文件，则在Notice文件中需要带有Apache License。你可以在Notice中增加自己的许可，但不可以表现为对Apache License构成更改。若读者今后根据自己项目性质想要修改为其它开源协议，或了解更多开源许可证信息，请浏览：

https://opensource.org/licenses

13.2 创建节点

功能包有了，是时候创建节点了。节点是通过调用ROS2客户端rclpy库函数实现的，分为创建可执行文件、编程接口初始化、创建节点并初始化、销毁节点并关闭接口、添加依赖与入口点等步骤。在创建facer的三个节点之前，先逐一讲解一下这些步骤。

13.2.1 创建可执行文件

通常我们在一个可执行文件中只创建一个节点，将可执行文件名命名为节点名。首先进入与功能包同名的子文件夹，打开终端输入以下指令，创建名为node_name的可执行文件：

touch node_name.py # 创建名为node_name的可执行文件

注释：touch指令用于创建任意格式的文件。

创建好的可执行文件是一个空文件，我们打开它进行编写代码，为了提高代码的可读性，可以在文件开头注释节点的相关信息。如下所示：

"""
@节点作者：略

@节点身份：略

@节点任务：略
"""

13.2.2 编程接口初始化

首先要导入ROS2的客户端库rclpy以及节点类，才能使用其提供的相关功能。

"""
@节点作者：略

@节点身份：略

@节点任务：略
"""

'''新增代码'''
# ros2库
import rclpy # ros2 python接口库
import rclpy.node import Node # ros2 节点类

然后在文件的末尾，添加以下代码。

"""
@节点作者：略

@节点身份：略

@节点任务：略
"""

# ros2库
import rclpy # ros2 python接口库
import rclpy.node import Node # ros2 节点类

'''新增代码'''
if __name__ == '__main__':
    main()

可能有部分读者并不清楚这两行代码的含义，下面详细解释一下。每个python可执行文件都有一个内置属性name，如果可执行文件以顶层程序文件执行，内置属性name的值为main，该两行代码可使main()函数执行；如果可执行文件被其它程序导入使用，name的值为模块名，该两行代码可使main()函数不会执行。在Python中这样的写法很常见，这样写的好处是使此可执行文件成为主程序。

main()函数是节点的主入口函数，接下来我们编写主入口函数并初始化ROS2的Python接口，由于没有参数传入，所以我们填写了args=None。

"""
@节点作者：略

@节点身份：略

@节点任务：略
"""

# ros2库
import rclpy # ros2 python接口库
import rclpy.node import Node # ros2 节点类

'''新增代码'''
def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化
    
# 使模块是顶层程序文件是能运行，是导入模块时不会运行   
if __name__ == '__main__':
    main()

13.2.3 创建节点并初始化

初始化ROS2的Python接口后，就可以使用其提供的功能创建节点了。我们使用面相对象的编程方法，先创建一个节点类，然后继承ROS2的Python接口提供的节点功能，节点名称node_name作为输入参数，代码如下：

"""
@节点作者：略

@节点身份：略

@节点任务：略
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 python接口库节点类 

class NodeClass(Node):
    '''新增代码'''
    def __init__(self):
        super().__init__('node_name') # ros2节点父类初始化,节点名称为node_name

def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

对象是类的实例，现在让我们创建一个节点对象：

"""
@节点作者：略

@节点身份：略

@节点任务：略
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 python接口库节点类 

class NodeClass(Node):
    def __init__(self):
        super().__init__('node_name') # ros2节点父类初始化,节点名称为node_name

def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化
    
    '''新增代码'''
	node = NodeClass() # 创建ros2节点对象并进行初始化
    
# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

现在创建的节点，它没有任何的任务，节点的任务往往是处理来自通信接口的数据，所以节点的任务实现将在后文单独讲解。

13.2.4 销毁节点并关闭接口

运行节点会占用计算机的资源，在节点停止运行后应该释放资源，所以还要在入口主函数添加以下代码，销毁节点并关闭ROS2的Python接口。

"""
@节点作者：略

@节点身份：略

@节点任务：略
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 python接口库节点类 

class NodeClass(Node):
    def __init__(self):
        super().__init__('node_name') # ros2节点父类初始化,节点名称为node_name

def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化
    
	node = NodeClass() # 创建ros2节点对象并进行初始化
    
    '''新增代码'''
    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口
    
# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

13.3.5 添加依赖与入口点

13.3.5.1 添加依赖

打开功能包的package.xml，添加创建节点依赖的库rclpy：

  demo
  0.0.0
  demo
  sipeiliu
  Apache License 2.0

rclpy

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

13.3.5.2 添加入口点

打开功能包的setup.py，在entry_points的'console_scripts'字段的括号内添加节点主入口函数位置：

from setuptools import setup

package_name = 'demo'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='sipeiliu',
    maintainer_email='[email protected]',
    description='demo',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'node_name_image = package_name.node_name:main',
        ],
    },
)

创建节点的步骤已经讲解完成，接下来我们来创建facer的三个节点。

13.3 创建capture_image节点

打开终端，输入以下指令进入~/ros2_ws/src/facer_input/facer_input文件夹：

cd ~/ros2_ws/src/facer_input/facer_input # 进入facer_input文件夹

输入以下指令，在该目录下创建名为capture_image.py的python文件：

touch capture_image.py # 创建python文件

编写代码

"""
@作者:[email protected]

@节点身份：frame_image话题的发布者，camera_stste动作的服务端

@节点任务：采集图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 

"""采集图像节点类"""
class CaptureImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('capture_image') # ros2节点父类初始化,节点名称为capture_image
        
        '''frame_image话题的发布者的相关代码'''
        # 暂略
        
        '''camera_stste动作的服务端的相关代码'''
        # 暂略
    
def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

    node = CaptureImage() # 创建ros2节点对象并进行初始化

    rclpy.spin(node) # 循环等待ros2退出

    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

添加依赖与入口点

打开facer_input功能包的package.xml，添加创建节点依赖的库rclpy：

  facer_input
  0.0.0
  采集图像
  sipeiliu
  Apache License 2.0
    
  rclpy

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

打开faer_input功能包的setup.py，在entry_points的'console_scripts'字段的括号内添加节点主入口函数位置。

from setuptools import setup

package_name = 'facer_input'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='sipeiliu',
    maintainer_email='[email protected]',
    description='采集图像',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'capture_image = facer_input.capture_image:main',
        ],
    },
)

13.4 创建identify_image节点

打开终端，输入以下指令进入~/ros2_ws/src/facer_handle/facer_handle文件夹：

cd ~/ros2_ws/src/facer_handle/facer_handle # 进入facer_input文件夹

输入以下指令，在该目录下创建名为identify_image.py的python文件：

touch identify_image.py # 创建python文件

编写代码

"""
@作者:[email protected]

@节点身份：frame_image话题的订阅者，face_result服务的服务端

@节点任务：识别图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 

"""识别图像节点类"""
class IdentifyImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('identify_image') # ros2节点父类初始化,节点名称为identify_image
        
        '''frame_image话题的订阅者的相关代码'''
        # 暂略
        
        '''face_result服务的服务端的相关代码'''
        # 暂略
    
def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

    node = IdentifyImage() # 创建ros2节点对象并进行初始化

    rclpy.spin(node) # 循环等待ros2退出

    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

添加依赖与入口点

打开facer_handle功能包的package.xml，添加创建节点依赖的库rclpy：

  facer_handle
  0.0.0
  识别图像
  sipeiliu
  Apache License 2.0
    
  rclpy

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

打开faer_handle功能包的setup.py，在entry_points的'console_scripts'字段的括号内添加节点主入口函数位置。

from setuptools import setup

package_name = 'facer_handle'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='sipeiliu',
    maintainer_email='[email protected]',
    description='识别图像',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'identify_image = facer_handle.identify_image:main',
        ],
    },
)

13.5 创建visual_interaction节点

打开终端，输入以下指令进入~/ros2_ws/src/facer_output/facer_output文件夹：

cd ~/ros2_ws/src/facer_output/facer_output # 进入facer_input文件夹

输入以下指令，在该目录下创建名为identify_image.py的python文件：

touch visual_interaction.py # 创建python文件

编写代码

"""
@作者:[email protected]

@节点身份：face_result服务的客户端，camera_state动作的客户端

@节点任务：人机交互
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 

"""人机交互节点类"""
class VisualInteraction(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('visual_interaction') # ros2节点父类初始化,节点名称为visual_interaction
        
        '''face_result服务的客户端的相关代码'''
        # 暂略
        
        '''camera_state动作的客户端的相关代码'''
        # 暂略
    
def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

    node = VisualInteraction() # 创建ros2节点对象并进行初始化

    rclpy.spin(node) # 循环等待ros2退出

    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

添加依赖与入口点

打开facer_output功能包的package.xml，添加创建节点依赖的库rclpy：

  facer_output
  0.0.0
  人机交互
  sipeiliu
  Apache License 2.0
    
  rclpy

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

打开faer_handle功能包的setup.py，在entry_points的'console_scripts'字段的括号内添加节点主入口函数位置。

from setuptools import setup

package_name = 'facer_output'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='sipeiliu',
    maintainer_email='[email protected]',
    description='人机交互',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'visual_interaction = facer_output.visual_interaction:main',
        ],
    },
)

13.6 查看节点列表

现在我们来简单学习一下如何在终端查看刚才创建好的节点，以此检测我们是否成功创建了节点。

打开终端，输入以下指令进入facer_ws工作空间：

cd ~/facer_ws # 进入工作空间

因为我们修改了功能包中的内容，所以需要重新编译，输入以下指令编译功能包：

colcon build # 编译功能包

现在，我们让三个节点都运行起来，只有运行中的节点才能被后面的查看列表指令发现。

设置环境变量才能让此终端发现节点功能包，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

输入以下指令运行facer_input功能包的capture_image节点：

ros2 run facer_input capture_image # 运行capture_image节点

运行capture_image节点后，这个终端就被capture_image节点占用了。所以我们需要打开一个新的终端，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

输入以下指令运行facer_handle功能包的identify_image节点：

ros2 run facer_handle identify_image # 运行identify_image节点

同样的，运行identify_image节点后，这个终端就被identify_image节点占用了。所以我们需要打开一个新的终端，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

输入以下指令运行facer_output功能包的visual_interaction节点：

ros2 run facer_output visual_interaction # 运行visual_interaction节点

最后，打开一个新的终端，终于可以输入以下指令查看节点列表了：

ros2 node list # 查看节点列表

在终端输出的列表会看到我们创建的节点如下：

/capture_image
/identify_image
/visual_interaction

至此，目录层级结构如下图所示：

facer_ws
	build
	install
	log
	src
		facer_interfaces
			CamkeLists.txt
			package.xml
			msg
			srv
				FaceResult.srv
			action
				CameraState
		facer_input
			facer_input
				__init__.py
				capture_image.py
			resource
			test
			package.xml
			setup.cfg
			setup.py
		facer_handle
			facer_handle
				__init__.py
				identify_image.py
			resource
			test
			package.xml
			setup.cfg
			setup.py
		facer_output
			facer_output
				__init__.py
				visual_interaction.py
			resource
			test
			package.xml
			setup.cfg
			setup.py

14 实现节点任务

我们的案例facer需要使用到计算机的摄像头，如果是在虚拟机中操作，需要进行以下设置：

1. 把虚拟机设置为兼容USB3.1。虚拟机-设置-硬件-USB控制器-USB兼容性

2. 在可移动设备中将摄像头连接至虚拟机。虚拟机-可移动设备-关于摄像头设备的选项

节点的任务通常是处理来自于通信接口的数据，在ROS2中数据是通过话题、服务、动作三种通信接口进行传递的。从通信接口角度，节点接收数据后处理数据，然后发布新的数据，创建一个通信接口实现一个节点任务，就是实现节点任务的流程。facer拥有三个通信接口：传递图像的frame_image话题、传递人脸识别结果的face_image服务、传递摄像机状态的camera_state动作，下面我们会依次创建并实现对应的节点任务。

实现节点任务后，还需要在package.xml文件中，声明对系统包的依赖。如果声明缺失或错误，可能并不影响本机编译和运行，但是当你发布到ROS社区，分享给他人使用的时候，其他使用者可能难以根据你的package.xml信息进行编译。所以我们最好将这些依赖项添加到package.xml文件中。

14.1 创建frame_image话题

14.1.1 创建frame_image话题的发布者

打开facer_input功能包中的可执行文件capture_image.py，添加以下内容：

"""
@作者：[email protected]

@节点身份：frame_image的话题发布者，camera_state动作的服务端

@节点任务：采集图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from cv_bridge import CvBridge # ros2与OpenCv图象转换类

# 其它库
import cv2 # OpenCv图像处理库
import time # 时间库


# 导入消息类型
from sensor_msgs.msg import Image # 图像消息类型

"""采集图像节点类"""
class CaptureImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('capture_image') # ros2节点父类初始化,节点名称为capture_image
        
        '''frame_image话题的发布者的相关代码'''
        self.cap = cv2.VideoCapture(0)# 创建视频采集对象
        self.cv_bridge = CvBridge() # 创建图象转换对象，该对象可以将OpenCv采集到的图像转换为ros2的图像消息类型
        # 创建话题发布者对象(消息类型、话题名、队列长度)
        self.publisher_ = self.create_publisher(
            Image,
            'frame_image',
            10)     
        timer_period = 0.1 # 确定发布周期，单位为秒      
        self.timer = self.create_timer(timer_period, self.timer_callback) # 创建定时器
    
        
        '''camera_state动作的服务端的相关代码'''
        # 暂略

    '''发布话题定时函数'''
    def timer_callback(self):
        ret, frame = self.cap.read() # 读取一帧图像
        if ret ==  True: # 返回值为True则摄像头已打开，采集图像成功     
            self.camera_state = '摄像头打开'

            ros2_image = self.cv_bridge.cv2_to_imgmsg(frame, 'bgr8') # 将OpenCv采集到的图像转换为ros2的图像消息类型
            self.publisher_.publish(ros2_image) # 发布图像
        else:
            self.camera_state = '摄像头关闭'
            
      
'''ros2 节点主入口main函数'''
def main(args=None):
    rclpy.init(args=args) # ros2 python接口初始化

    node = CaptureImage() # 创建ros2节点对象并进行初始化

    rclpy.spin(node) # 循环等待ros2退出

    node.destory_node() # 销毁节点对
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

14.1.2 创建frame_image话题的订阅者

打开facer_handle功能包中的可执行文件identify_image.py，添加以下内容：

"""
@作者:[email protected]

@节点身份:frame_image话题的订阅者,face_result服务的服务端

@节点任务:识别图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from cv_bridge import CvBridge # ros2与OpenCv图象转换类

# 其它库
import cv2 # OpenCv图像处理库


# 导入消息类型
from sensor_msgs.msg import Image # 图像消息类型

"""识别图像节点类"""
class IdentifyImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('identify_image') # ros2节点父类初始化,节点名称为identify_image
    
        '''frame_image话题订阅者的相关代码'''
        # 创建话题订阅者对象(消息类型、话题名、订阅者回调函数、队列长度)
        self.subscription = self.create_subscription(
            Image,
            'frame_image',
            self.listener_callback,
            10)
        self.cv_bridge = CvBridge() # 创建图象转换对象，该对象可以将ros2的图像消息类型转换为OpenCv采集到的图像
        # 加载人脸数据模型,创建人脸分类器对象，注意要填写绝对路径
        self.face_Classifier = cv2.CascadeClassifier('/home/sipeiliu/facer_ws/src/facer/haarcascade_frontalface_default.xml')
        self.result = '没有检测到人脸' # 默认不存在人脸

        '''face_result服务服务端的相关代码'''
        # 暂略

def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

    node = IdentifyImage() # 创建ros2节点对象并进行初始化
    
    rclpy.spin(node) # 循环等待ros2退出
    
    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

注释：数据模型文件使用haarcascade_frontalface_default.xml。可以在OpenCV安装目录中的\data\ haarcascades目录下找到。Haar特征分类器就是一个XML文件，该文件中会描述人脸各个部位的Haar特征值，包括鼻子、眼睛、嘴唇等等。数据模型文件也可以在下面的地址找到：

https://github.com/opencv/opencv.git

14.1.3 添加依赖

cv_bridge可以将ROS2的图像消息类型和OpenCv采集到的图像类型相互转换。为了实现对图像的处理，使用到了机器视觉库OpenCv，它是图像处理的依赖项。frame_image话题是capture_image与identify节点之间的通信接口，我们使用标准通信接口Image，Image.msg可以在ROS2安装路径中的ros/humble/share/sensor_msgs/msg文件夹中找到，它是话题的依赖项。

打开功能包facer_input的package.xml，我们将这些依赖项添加到package.xml文件中。添加以下内容：

  facer_input
  0.0.0
  采集图像
  sipeiliu
  Apache License 2.0
    
  rclpy
  cv_bridge
    
  python-opencv
   
  sensor_msgs
 

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

打开功能包facer_handle的package.xml，填写以下内容：

  facer_handle
  0.0.0
  识别图像
  sipeiliu
  Apache License 2.0
    
  rclpy
  cv_bridge
    
  python-opencv
   
  sensor_msgs
  
  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  


demo
sipeiliu
Apache License 2.0

14.1.4查看话题列表

现在我们来简单学习一下如何在终端查看刚才创建好的frame_image话题，以此检测我们是否成功创建frame_image话题。

打开终端，输入以下指令进入facer_ws工作空间：

cd ~/facer_ws # 进入工作空间

rosdep用于安装系统依赖，具体地说就是功能包的依赖，rosdep的详细内容将在工具篇讲解。输入以下指令，在工作空间的根目录facer_ws中运行rosdep以检查缺少的依赖项：

rosdep install -i --from-path src --rosdistro humble -y

因为我们修改了功能包中的内容，所以需要重新编译，输入以下指令编译功能包：

colcon build # 编译功能包

只有使用中的话题才能被后面的查看话题指令发现，所以我们先要运行capture_image节点和identify_image节点。

设置环境变量让此终端发现节点功能包，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

输入以下指令运行facer_input功能包的capture_image节点：

ros2 run facer_input capture_image # 运行capture_image节点

运行capture_image节点后，这个终端就被capture_image节点占用了。所以我们需要打开一个新的终端，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

输入以下指令运行facer_handle功能包的identify_image节点：

ros2 run facer_handle identify_image # 运行identify_image节点

最后，终于可以输入以下指令查看话题列表了：

ros2 topic list # 查看话题列表

在终端输出的列表会看到我们创建的话题如下：

/frame_image

14.2 创建identify_result服务

14.2.1 创建identify_result服务的服务端

打开facer_handle功能包中的可执行文件identify_image.py，添加以下内容：

"""
@作者:[email protected]

@节点身份:frame_image话题的订阅者,face_result服务的服务端

@节点任务:识别图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from cv_bridge import CvBridge # ros2与OpenCv图象转换类

# 其它库
import cv2 # OpenCv图像处理库


# 导入消息类型
from sensor_msgs.msg import Image # 图像消息类型
from facer_interfaces.srv import FaceResult # 自定义检测结果消息类型

"""识别图像节点类"""
class IdentifyImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('identify_image') # ros2节点父类初始化,节点名称为identify_image
    
        '''frame_image话题订阅者的相关代码'''
        # 创建话题订阅者对象(消息类型、话题名、订阅者回调函数、队列长度)
        self.subscription = self.create_subscription(
            Image,
            'frame_image',
            self.listener_callback,
            10)
        self.cv_bridge = CvBridge() # 创建图象转换对象，该对象可以将ros2的图像消息类型转换为OpenCv采集到的图像
        # 加载人脸数据模型,创建人脸分类器对象，注意要填写绝对路径
        self.face_Classifier = cv2.CascadeClassifier('/home/sipeiliu/facer_ws/src/facer/haarcascade_frontalface_default.xml')
        self.result = '没有检测到人脸' # 默认不存在人脸

        '''face_result服务服务端的相关代码'''
        # 创建服务服务端对象(消息类型、服务名、服务端回调函数)
        self.srv = self.create_service(FaceResult, 'face_result', self.face_result_callback)

    '''话题订阅者回调函数'''
    def listener_callback(self, ros2_image):       
        frame = self.cv_bridge.imgmsg_to_cv2(ros2_image, 'bgr8') # 将OpenCv采集到的图像转换为ros2的图像消息类型      
        # 将每一帧摄像头记录的数据带入OpenCv中，让人脸分类器判断人脸
        faces = self.face_Classifier.detectMultiScale(frame) # 返回一个元组列表，每个列表保存了人脸方框的四个坐标
        if len(faces) > 0:
            self.result = '检测到人脸' # 只要有一帧图像出现了人脸，就判断结果为存在人脸

    '''服务服务端回调函数'''
    def face_result_callback(self, request, reponse):
        if request.apply_result == True: # 判断请求真假
            reponse.get_result = self.result # 传递检测结果
            return reponse # 回答服务的请求


def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

    node = IdentifyImage() # 创建ros2节点对象并进行初始化
    
    rclpy.spin(node) # 循环等待ros2退出
    
    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

14.2.2 创建identify_result服务的客户端

打开facer_output功能包中的可执行文件visual_interaction.py，添加以下内容：

"""
@作者:[email protected]

@节点身份：face_result服务的客户端，camera_status动作的客户端

@节点任务：人机交互
"""

# ros2 库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 

# 导入消息类型
from facer_interfaces.srv import FaceResult # 自定义检测结果消息类型

"""人机交互节点类"""
class VisualInteraction(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('visual_interaction') # ros2节点父类初始化,节点名称为visual_interaction
        
    
        '''服务face_result客户端的相关代码'''
        # 创建服务客户端对象(消息类型、服务名)
        self.cli= self.create_client(FaceResult, 'face_result')
        # 每隔一秒检查服务端是否运行,若没有运行就等待
        while not self.cli.wait_for_service(timeout_sec=1.0):
            print('等待服务face_result的服务端运行...')
        # 创建服务消息类型对象
        self.req = FaceResult.Request()

        
        '''动作camera_state客户端的相关代码'''
        # 暂略

    '''发送服务请求函数'''
    def send_request(self, apply_result):
        self.req.apply_result = apply_result # 设置申请结果为真，让服务端判断为可以返回检测结果
        self.future = self.cli.call_async(self.req) # 异步方式发送服务请求
        rclpy.spin_until_future_complete(self, self.future) # 阻塞直到self.future完成
        return self.future.result() # 返回结果

def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

    node = VisualInteraction() # 创建ros2节点对象并进行初始化

    time.sleep(3) # 等待3秒后发送服务请求
    reponse = node.send_request(True) # 获取服务返回的检测结果
    print(reponse.get_result)
    
    future = node.send_goal(True) # 获取动作返回的结果

    rclpy.spin(node) # 循环等待ros2退出
    
    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

14.2.3 添加依赖

facer_handle功能包的identify_image节点与facer_output功能包的visual_interaction节点依赖facer_interfaces.srv的FaceResult。

打开功能包facer_handle的package.xml，我们将依赖项添加到package.xml文件中。添加以下内容：

  facer_handle
  0.0.0
  识别图像
  sipeiliu
  Apache License 2.0
    
  rclpy
  cv_bridge
    
  python-opencv
   
  sensor_msgs
  facer_interfaces.srv
  
  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  


demo
sipeiliu
Apache License 2.0

打开功能包facer_output的package.xml，我们将依赖项添加到package.xml文件中。添加以下内容：

  facer_output
  0.0.0
  人机交互
  liusipei
  Apache License 2.0

  rclpy

  facer_interfaces.srv

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

14.2.4 查看服务列表

现在我们来简单学习一下如何在终端查看刚才创建好的face_result服务，以此检测我们是否成功创建face_result服务。

打开终端，输入以下指令进入facer_ws工作空间：

cd ~/facer_ws # 进入工作空间

输入以下指令，在工作空间的根目录facer_ws中运行rosdep以检查缺少的依赖项：

rosdep install -i --from-path src --rosdistro humble -y

因为我们修改了功能包中的内容，所以需要重新编译，输入以下指令编译功能包：

colcon build # 编译功能包

只有使用中的服务才能被后面的查看服务指令发现，所以我们先要运行identify_image节点和visual_interaction节点。

设置环境变量让此终端发现节点功能包，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

输入以下指令运行facer_handle功能包的identify_image节点：

ros2 run facer_handle identify_image # 运行identify_image节点

运行identify_image节点后，这个终端就被identify_image节点占用了。所以我们需要打开一个新的终端，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

输入以下指令运行facer_output功能包的visual_interaction节点：

ros2 run facer_output visual_interaction # 运行visual_interaction节点

最后，终于可以输入以下指令查看服务列表了：

ros2 service list # 查看服务列表

在终端输出的列表会看到我们创建的服务如下：

/face_result

14.3 创建camera_state动作

14.3.1 创建camera_state动作的服务端

打开facer_imput功能包中的可执行文件capture_image.py，添加以下内容：

"""
@作者：[email protected]

@节点身份：frame_image的话题发布者，camera_state动作的服务端

@节点任务：采集图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from rclpy.action import ActionServer
from cv_bridge import CvBridge # ros2与OpenCv图象转换类

# 其它库
import cv2 # OpenCv图像处理库
import time # 时间库


# 导入消息类型
from sensor_msgs.msg import Image # 图像消息类型
from facer_interfaces.action import CameraState # 自定义摄像机状态消息类型

"""采集图像节点类"""
class CaptureImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('capture_image') # ros2节点父类初始化,节点名称为capture_image
        
        '''frame_image话题的发布者的相关代码'''
        self.cap = cv2.VideoCapture(0)# 创建视频采集对象
        self.cv_bridge = CvBridge() # 创建图象转换对象，该对象可以将OpenCv采集到的图像转换为ros2的图像消息类型
        # 创建话题发布者对象(消息类型、话题名、队列长度)
        self.publisher_ = self.create_publisher(
            Image,
            'frame_image',
            10)     
        timer_period = 0.1 # 确定发布周期，单位为秒      
        self.timer = self.create_timer(timer_period, self.timer_callback) # 创建定时器
    
        
        '''camera_state动作的服务端的相关代码'''
        # 创建动作服务端对象
        self._action_server = ActionServer(
            self,
            CameraState,
            'camera_state',
            self.execute_callback)


    '''发布话题定时函数'''
    def timer_callback(self):
        ret, frame = self.cap.read() # 读取一帧图像
        if ret ==  True: # 返回值为True则摄像头已打开，采集图像成功     
            self.camera_state = '摄像头打开'

            ros2_image = self.cv_bridge.cv2_to_imgmsg(frame, 'bgr8') # 将OpenCv采集到的图像转换为ros2的图像消息类型
            self.publisher_.publish(ros2_image) # 发布图像
        else:
            self.camera_state = '摄像头关闭'

    '''执行动作回调函数'''
    def execute_callback(self, goal_handle):
        self.count = 0
        feedback_msg = CameraState.Feedback() # 实例化动作的反馈
        while goal_handle.request.start == True and self.count < 10 : # 动作请求为真且反馈次数为10
            feedback_msg.state = self.camera_state # 传递摄像头状态
            goal_handle.publish_feedback(feedback_msg) # 发送周期反馈
            self.count = self.count + 1
            time.sleep(0.5) # 隔0.5秒就反馈一次

        goal_handle.succeed() # 表明目标成功，否则会警告目标状态没有设置

        result = CameraState.Result() # 实例化动作的结果
        result.finish = '动作完成' # 给动作结果的finish赋值，动作完成
        return result # 返回动作结果
      
'''ros2 节点主入口main函数'''
def main(args=None):
    rclpy.init(args=args) # ros2 python接口初始化

    node = CaptureImage() # 创建ros2节点对象并进行初始化

    rclpy.spin(node) # 循环等待ros2退出

    node.destory_node() # 销毁节点对
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

14.3.2 创建camera_state动作的客户端

打开facer_output功能包中的可执行文件visual_interaction.py，添加以下内容：

"""
@作者:[email protected]

@节点身份：face_result服务的客户端，camera_status动作的客户端

@节点任务：人机交互
"""

# ros2 库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from rclpy.action import ActionClient # 动作客户端库

# 其它库
import time # 时间库

# 导入消息类型
from facer_interfaces.srv import FaceResult # 自定义检测结果消息类型
from facer_interfaces.action import CameraState # 自定义摄像机状态消息类型

"""人机交互节点类"""
class VisualInteraction(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('visual_interaction') # ros2节点父类初始化,节点名称为visual_interaction
        
    
        '''服务face_result客户端的相关代码'''
        # 创建服务客户端对象(消息类型、服务名)
        self.cli= self.create_client(FaceResult, 'face_result')
        # 每隔一秒检查服务端是否运行,若没有运行就等待
        while not self.cli.wait_for_service(timeout_sec=1.0):
            print('等待服务face_result的服务端运行...')
        # 创建服务消息类型对象
        self.req = FaceResult.Request()

        
        '''动作camera_state客户端的相关代码'''
        self._action_client = ActionClient(self, CameraState, 'camera_state')

    '''发送服务请求函数'''
    def send_request(self, apply_result):
        self.req.apply_result = apply_result # 设置申请结果为真，让服务端判断为可以返回检测结果
        self.future = self.cli.call_async(self.req) # 异步方式发送服务请求
        rclpy.spin_until_future_complete(self, self.future) # 阻塞直到self.future完成
        return self.future.result() # 返回结果

    '''发送动作目标函数'''
    def send_goal(self, start):
        goal_msg = CameraState.Goal() # 实例化动作的目标
        goal_msg.start = start # 给实例化的动作目标赋值
        # self._action_client.wait_for_server() # 等待动作服务端启动
        while not self._action_client.wait_for_server(timeout_sec=1.0):
            print('等待动作camera_state的服务端运行...')
        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) # 返回动作目标的future
        self._send_goal_future.add_done_callback(self.goal_reponse_callback) # 通过动作目标的future给动作目标添加回调函数

    '''动作目标的回调函数'''
    def goal_reponse_callback(self, future):
        goal_handle = future.result() # 目标处理
        if goal_handle.accepted:
            print('目标请求成功')
        else:
            print('目标请求失败')

        self._get_result_future = goal_handle.get_result_async() # 使用目标处理的方法请求动作结果，当动作结果出来后会返回动作结果的future
        self._get_result_future.add_done_callback(self.get_result_callback) # 通过动作结果的future给动作结果添加回调函数

    '''动作结果的回调函数'''
    def get_result_callback(self, future):
        result = future.result().result
        print(result.finish)

    '''动作周期反馈函数'''
    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        print(feedback)

def main(args=None): # ros2 节点主入口main函数
    rclpy.init(args=args) # ros2 python接口初始化

    node = VisualInteraction() # 创建ros2节点对象并进行初始化

    time.sleep(3) # 等待3秒后发送服务请求
    reponse = node.send_request(True) # 获取服务返回的检测结果
    print(reponse.get_result)
    
    future = node.send_goal(True) # 获取动作返回的结果

    rclpy.spin(node) # 循环等待ros2退出
    
    node.destory_node() # 销毁节点对象
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

14.3.3 添加依赖

facer_output功能包的visual_interaction节点与facer_input功能包的capture_image节点依赖facer_interfaces.action的CameraState。

打开功能包facer_output的package.xml，我们将依赖项添加到package.xml文件中。添加以下内容：

  facer_output
  0.0.0
  人机交互
  liusipei
  Apache License 2.0

  rclpy

  facer_interfaces.srv
  facer_interfaces.action

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

打开功能包facer_input的package.xml，我们将这些依赖项添加到package.xml文件中。添加以下内容：

  facer_input
  0.0.0
  采集图像
  sipeiliu
  Apache License 2.0
    
  rclpy
  cv_bridge
    
  python-opencv
   
  sensor_msgs
  facer_interfaces.action
 

  ament_copyright
  ament_flake8
  ament_pep257
  python3-pytest

  
    ament_python
  

14.3.4 查看动作列表

打开终端，输入以下指令进入facer_ws工作空间：

cd ~/facer_ws # 进入工作空间

输入以下指令，在工作空间的根目录facer_ws中运行rosdep以检查缺少的依赖项：

rosdep install -i --from-path src --rosdistro humble -y

因为我们修改了功能包中的内容，所以需要重新编译，输入以下指令编译功能包：

colcon build # 编译功能包

只有使用中的动作才能被后面的查看动作指令发现，所以我们先要运行visual_interaction节点和capture_image节点。

设置环境变量让此终端发现节点功能包，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

输入以下指令运行facer_output功能包的visual_interaction节点：

ros2 run facer_output visual_interaction # 运行visual_interaction节点

运行visual_interaction节点后，这个终端就被visual_interaction节点占用了。所以我们需要打开一个新的终端，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

输入以下指令运行facer_input功能包的capture_image节点：

ros2 run facer_input capture_image # 运行capture_image节点

最后，终于可以输入以下指令查看动作列表了：

ros2 action list # 查看服务列表

在终端输出的列表会看到我们创建的动作如下：

/camera_state

15 使用参数

现在，我们来修改capture_image节点的话题发布周期timer_period为参数。之后便可以从终端或启动文件设置timer_period参数的值。

15.1 创建参数

打开capture_image.py，修改后的代码如下。参数类型是ROS2系统自动从默认值推断的，因此在本例中，它将被设置为双精度浮点型。接下来，计时器以0.1秒的周期初始化，这导致timer_callback函数每0.1秒执行一次。

"""
@作者：[email protected]

@节点身份：frame_image的话题发布者，camera_state动作的服务端

@节点任务：采集图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from rclpy.action import ActionServer # 动作客户端库
from cv_bridge import CvBridge # ros2与OpenCv图象转换类

# 其它库
import cv2 # OpenCv图像处理库
import time # 时间库


# 导入消息类型
from sensor_msgs.msg import Image # 图像消息类型
from facer_interfaces.action import CameraState # 自定义摄像机状态消息类型

"""采集图像节点类"""
class CaptureImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('capture_image') # ros2节点父类初始化,节点名称为capture_image
        
        '''frame_image话题的发布者的相关代码'''
        self.cap = cv2.VideoCapture(0)# 创建视频采集对象
        self.cv_bridge = CvBridge() # 创建图象转换对象，该对象可以将OpenCv采集到的图像转换为ros2的图像消息类型
        self.publisher_ = self.create_publisher(Image, 'frame_image', 10) # 创建话题发布者对象(消息类型、话题名、队列长度)
        
        '''参数代码'''
        # timer_period = 0.1 # 确定发布周期，单位为秒 
        self.declare_parameter('timer_period', 0.1) # 修改发布周期为参数 
        timer_period_param = self.get_parameter('timer_period').get_parameter_value().double_value  
        
        self.timer = self.create_timer(timer_period_param, self.timer_callback) # 创建定时器


        '''camera_state动作的服务端的相关代码'''
        self._action_server = ActionServer(self, CameraState, 'camera_state', self.execute_callback) # 创建动作服务端对象(消息类型、动作名、回调函数)


    '''发布话题定时函数'''
    def timer_callback(self):
        ret, frame = self.cap.read() # 读取一帧图像
        if ret ==  True: # 返回值为True则摄像头已打开，采集图像成功     
            self.camera_state = '摄像头打开'
            ros2_image = self.cv_bridge.cv2_to_imgmsg(frame, 'bgr8') # 将OpenCv采集到的图像转换为ros2的图像消息类型
            self.publisher_.publish(ros2_image) # 发布图像
        else:
            self.camera_state = '摄像头关闭'

    '''执行动作回调函数'''
    def execute_callback(self, goal_handle):
        feedback_count = 0 # 返回次数
        feedback_msg = CameraState.Feedback() # 实例化动作的反馈
        while goal_handle.request.start == True and feedback_count < 10 : # 动作请求为真且反馈次数为10
            feedback_msg.state = self.camera_state # 传递摄像头状态
            goal_handle.publish_feedback(feedback_msg) # 发送周期反馈
            feedback_count = feedback_count + 1 # 统计反馈次数
            time.sleep(0.5) # 隔0.5秒就反馈一次

        goal_handle.succeed() # 表明目标成功，否则会警告目标状态没有设置

        result = CameraState.Result() # 实例化动作的结果
        result.finish = '动作完成' # 给动作结果的finish赋值，动作完成
        return result # 返回动作结果
      
'''ros2 节点主入口main函数'''
def main(args=None):
    rclpy.init(args=args) # ros2 python接口初始化

    node = CaptureImage() # 创建ros2节点对象并进行初始化

    rclpy.spin(node) # 循环等待ros2退出

    node.destory_node() # 销毁节点对
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

15.2 添加参数描述

也可以为参数设置描述符。描述符允许您指定参数和参数约束的文本描述，例如将其设置为只读、指定范围等。要实现这一点，代码修改如下。

"""
@作者：[email protected]

@节点身份：frame_image的话题发布者，camera_state动作的服务端

@节点任务：采集图像
"""

# ros2库
import rclpy # ros2 python接口库
from rclpy.node import Node # ros2 节点类 
from rclpy.action import ActionServer # 动作客户端库
from cv_bridge import CvBridge # ros2与OpenCv图象转换类

# 其它库
import cv2 # OpenCv图像处理库
import time # 时间库


# 导入消息类型
from sensor_msgs.msg import Image # 图像消息类型
from facer_interfaces.action import CameraState # 自定义摄像机状态消息类型

"""采集图像节点类"""
class CaptureImage(Node):

    """构造函数"""
    def __init__(self):
        super().__init__('capture_image') # ros2节点父类初始化,节点名称为capture_image
        
        '''frame_image话题的发布者的相关代码'''
        self.cap = cv2.VideoCapture(0)# 创建视频采集对象
        self.cv_bridge = CvBridge() # 创建图象转换对象，该对象可以将OpenCv采集到的图像转换为ros2的图像消息类型
        self.publisher_ = self.create_publisher(Image, 'frame_image', 10) # 创建话题发布者对象(消息类型、话题名、队列长度)
        
        '''参数代码'''
        # timer_period = 0.1 # 确定发布周期，单位为秒 
        from rcl_interfaces.msg import ParameterDescriptor # 导入参数描述消息类型
        self.parameter_descriptor = ParameterDescriptor(description='this is topic publish period！') # 添加参数描述
        self.declare_parameter('timer_period', 0.1，parameter_descriptor) # 修改发布周期为参数 
        self.timer_period_param = self.get_parameter('timer_period').get_parameter_value().double_value # 获取参数值
        
        self.timer = self.create_timer(self.timer_period_param, self.timer_callback) # 创建定时器


        '''camera_state动作的服务端的相关代码'''
        self._action_server = ActionServer(self, CameraState, 'camera_state', self.execute_callback) # 创建动作服务端对象(消息类型、动作名、回调函数)


    '''发布话题定时函数'''
    def timer_callback(self):
        ret, frame = self.cap.read() # 读取一帧图像
        if ret ==  True: # 返回值为True则摄像头已打开，采集图像成功     
            self.camera_state = '摄像头打开'
            ros2_image = self.cv_bridge.cv2_to_imgmsg(frame, 'bgr8') # 将OpenCv采集到的图像转换为ros2的图像消息类型
            self.publisher_.publish(ros2_image) # 发布图像
        else:
            self.camera_state = '摄像头关闭'

    '''执行动作回调函数'''
    def execute_callback(self, goal_handle):
        feedback_count = 0 # 返回次数
        feedback_msg = CameraState.Feedback() # 实例化动作的反馈
        while goal_handle.request.start == True and feedback_count < 10 : # 动作请求为真且反馈次数为10
            feedback_msg.state = self.camera_state # 传递摄像头状态
            goal_handle.publish_feedback(feedback_msg) # 发送周期反馈
            feedback_count = feedback_count + 1 # 统计反馈次数
            time.sleep(0.5) # 隔0.5秒就反馈一次

        goal_handle.succeed() # 表明目标成功，否则会警告目标状态没有设置

        result = CameraState.Result() # 实例化动作的结果
        result.finish = '动作完成' # 给动作结果的finish赋值，动作完成
        return result # 返回动作结果
      
'''ros2 节点主入口main函数'''
def main(args=None):
    rclpy.init(args=args) # ros2 python接口初始化

    node = CaptureImage() # 创建ros2节点对象并进行初始化

    rclpy.spin(node) # 循环等待ros2退出

    node.destory_node() # 销毁节点对
    rclpy.shutdown() # 关闭ros2 python接口

# 使模块是顶层程序文件是能运行，是导入模块时不会运行
if __name__ == "__main__":
    main()

同样的，打开终端，输入以下指令进入facer_ws工作空间：

cd ~/facer_ws # 进入工作空间

输入以下指令，在工作空间的根目录facer_ws中运行rosdep以检查缺少的依赖项：

rosdep install -i --from-path src --rosdistro humble -y

因为我们修改了功能包中的内容，所以需要重新编译，输入以下指令编译功能包：

colcon build # 编译功能包

输入以下指令运行facer_input功能包的capture_image节点：

ros2 run facer_input capture_image # 运行capture_image节点

运行capture_image节点后，这个终端就被capture_image节点占用了。所以我们需要打开一个新的终端，输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

运行节点后，输入以下指令即可查看参数类型和描述：

ros2 param describe /capture_image timer_period

终端输出内容如下：

Parameter name: timer_period
	Type: double
	Description: this is topic publish period!
	Constraints:

15.3 修改参数

现在我们知道话题发布周期timer_period参数的默认值是0.1，现在我们学习从程序外部修改其值。有两种方法可以实现这一点。第一种方法是在控制台使用终端指令。第二种方法是通过启动文件修改。

15.3.1 终端指令修改

确保已经运行capture_image节点：

ros2 run facer_input capture_image

打开新终端，设置环境变量：

source ~/facer_Ws/install/setup.bash

输入以下指令查看参数列表：

ros2 param list

终端将会输出timer_period参数：

/capture_image:
	timer_period

现在我们将timer_period参数的值修改为1.0，只需在终端中输入以下指令：

ros2 param set /capture_image timer_period 1.0

终端将会提示设置成功：

Set parameter successful

由于传递图像的话题发布周期延长，如果此时运行所有节点，将会发现facer执行相对迟缓了。

15.3.2 启动文件修改

我们可以在启动文件中设置参数，但首先需要添加启动文件夹。打开终端，输入以下指令进入facer_input文件夹：

cd ~/facer_ws/src/facer_input

输入以下指令，创建名为launch的文件夹：

mkdir launch

输入以下指令，进入launch文件夹：

cd launch

输入以下指令，创建名为python_parameters_launch.py的文件：

touch python_parameters_launch.py

此为，也可以输入以下指令一步到位：

touch ~/facer_ws/src/facer_input/launch/python_parameters_launch.py

打开文件，填入以下内容：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='facer_input',
            executable='capture_image',
            name='capture_image',
            output='screen',
            emulate_tty=True,
            parameters=[
                {'timer_period': 1.0}
            ]
        )
    ])

这里可以看到，当我们启动节点capture_image时，我们将timer_period设置为1.0。通过添加下面的两行，我们可以确保在终端中打印输出。

output='screen',
emulate_tty=True,

现在打开facer_input功能包的setup.py文件。将import语句添加到文件的顶部，将另一条新语句添加到data_files参数，以包括所有启动文件：

'''新增代码'''
import os
from glob import glob

from setuptools import setup

package_name = 'facer_input'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        
        '''新增代码'''
        (os.path.join('share', package_name), glob('launch/*launch.[pxy][yma]*')),
        
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='sipeiliu',
    maintainer_email='[email protected]',
    description='采集图像',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'capture_image = facer_input.capture_image:main',
        ],
    },
)

最好在功能包的package.xml中添加对ros2launch包的依赖，这有助于确保在编译功能包后ros2启动命令可用，它还确保识别所有启动文件格式：

ros2launch

打开终端，输入以下指令进入facer_ws工作空间：

cd ~/facer_ws # 进入工作空间

因为我们修改了功能包中的内容，所以需要重新编译，输入以下指令编译功能包：

colcon build # 编译功能包

输入以下指令设置环境变量：

source install/setup.bash # 设置环境变量

现在使用我们刚刚创建的启动文件运行节点：

ros2 launch facer_input python_parameters_launch.py

可以发现capture_image节点启动，timer_period也被设置为1.0。

关于启动文件的更多用法将在工具篇讲解。

第三部分 工具篇

16 编译功能包工具colcon

16.1 背景

colcon是ROS编译工具catkin_make、catkin_make_isolated、catkin_tools和ament_tools的迭代品。有关colcon设计的更多信息，请参阅文档：

https://design.ros2.org/articles/build_tool.html

源代码可以在colcon GitHub organization中找到：

https://github.com/colcon

16.2 前提

安装colcon

sudo apt install python3-colcon-common-extensions

16.3 基础

现在我们结合colcon来回顾一下工作空间的内容。ROS2工作空间是具有特定结构的目录，通常有一个src子目录。在该子目录中是ROS2包的源代码所在的位置。通常，src目录开始时为空。colcon执行源代码构建，默认情况下，它将创建以下目录作为src目录同级。

build：编译目录是存储中间文件的位置。对于每个功能包，将创建一个子文件夹，例如调用CMake。

install：安装目录是每个功能包将安装到的位置。默认情况下，每个功能包将被安装到单独的子目录中。

log：日志目录包含有关每个colcon调用的各种日志信息。

16.3.1 创建工作空间

首先，创建一个目录（ros2_ws）以包含我们的工作空间：

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws

此时，工作空间含一个空目录src：

.
└── src

1 directory, 0 files

16.3.2 添加源文件

让我们将示例存储库克隆到工作空间的src目录中：

git clone https://github.com/ros2/examples src/examples -b humble

现在，工作空间应该有ROS2示例的源代码：

.
└── src
    └── examples
        ├── CONTRIBUTING.md
        ├── LICENSE
        ├── rclcpp
        ├── rclpy
        └── README.md

4 directories, 3 files

16.3.3 设置底层

重要的是，我们已经为现有ROS2安装提供了环境，这将为我们工作空间的示例包提供所需的编译依赖项。这是通过获取二进制安装或源安装提供的设置脚本来实现的，即另一个colcon工作空间。我们称此环境为底层。

我们的工作空间ros2_ws将是现有ROS2安装之上的覆盖层。通常，当您计划在少量功能包上进行迭代时，建议使用覆盖，而不是将所有功能包放在同一个工作空间中。

在安装时，我们已经设置了始终执行以下指令，即已经设置了底层，无需重复操作。

source /opt/ros/humble/setup.bash 

16.3.4 编译功能包

在工作区=空间的根目录中，运行colcon build。由于像ament_cmake这样的构建类型不支持devel空间的概念，并且需要安装包，因此colcon支持参数--symlink安装。这允许通过更改源代码空间中的文件（例如Python文件或其他未编译的资源）来更改已安装的文件，以加快迭代。

colcon build --symlink-install

编译完成后，我们应该看到编译、安装和日志目录：

.
├── build
├── install
├── log
└── src

4 directories, 0 files

16.3.5 运行测试

要对我们刚刚编译的功能包进行测试，请运行以下命令：

colcon test

16.3.6 设置环境变量

当colcon成功完成编译后，能运行的的功能包将位于安装目录install中。在使用任何已安装的可执行文件或库之前，需要将它们添加到路径和库路径中，我们称为设置环境变量。colcon将在安装目录install中生成setup.bash等文件，以帮助设置环境变量。这些文件将向路径和库路径添加所有必需的元素，并提供功能包导出的任何bash或shell命令。简而言之，功能包编译后，还需要在每个终端输入以下指令才能正常使用，即设置环境变量：

. install/setup.bash

注释：在这里.与source指令含义相同，意为执行。

16.3.7 尝试演示

设置环境变量后，我们可以运行由colcon编译的可执行文件。让我们从示例运行订阅者节点：

ros2 run examples_rclcpp_minimal_subscriber subscriber_member_function

在另一个终端中，让我们运行一个发布者节点（不要忘记设置环境变量）：

ros2 run examples_rclcpp_minimal_publisher publisher_member_function

您应该看到来自发布者和订阅者的消息，其中数字递增。

16.4 创建功能包

colcon使用的package.xml遵循REP 149中定义的xml规范（也支持format 2）。

colcon支持编译多种类型的功能包。建议的编译类型为ament_cmake和ament_python。还支持纯cmake功能包。

ament_python构建的一个示例是ament_index_python包，其中setup.py是编译的主要入口。

demo_nodes_cpp等功能使用ament_cmake编译类型，并使用cmake作为编译工具。

为了方便起见，您可以使用工具ros2 pkg create 创建基于模板的新功能包。

16.5 设置colcon_cd

命令colcon_cd允许您快速将shell的当前工作目录更改为功能包的目录。例如，colcon_cd some_ros_package将很快将您带到目录~/ros2_install/src/some_ros_paackage。

echo "source /usr/share/colcon_cd/function/colcon_cd.sh" >> ~/.bashrc
echo "export _colcon_cd_root=/opt/ros/humble/" >> ~/.bashrc

根据您安装colcon_cd的方式和工作空间的位置，上面的指令可能有所不同，请参阅文档了解更多详细信息：

https://colcon.readthedocs.io/en/released/user/installation.html#quick-directory-changes

要在Linux中撤消此操作，请找到系统的shell启动脚本并删除附加的源命令和导出命令。

16.6 设置colcon命令补全

如果安装了colcon argcomplete包，命令colcon支持bash和类bash shell的命令补全。

echo "source /usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash" >> ~/.bashrc

根据您安装colcon的方式和工作空间的位置，上面的说明可能有所不同，请参阅文档了解更多详细信息：

https://colcon.readthedocs.io/en/released/user/installation.html

要在Linux中撤消此操作，请找到系统的shell启动脚本并删除附加的源命令。

16.7 技巧

如果不想编译某个特定的功能包，请在目录中放置一个名为COLCON_IGNORE的空文件，该文件将不会被索引。

如果要避免在CMake包中配置和构建测试，可以传递：--cmakeargs-DBUILD_TESTING=0。

如果要对单个功能包进行编译，可输入以下指令：

colcon build --packages-select package_name # 编译package_name

此外，要查看colcon更多相关指令用法，可输入以下指令：

colcon --help

17 添加依赖工具rosdep

17.1 rosdep概念

在功能包中使用第三方 Python 包有三种方法：通过rosdep安装、通过包管理器安装、通过虚拟环境安装。其中最快方法是通过rosdep安装，rosdep是用于安装系统依赖项的命令行工具。rosdep可以帮助从源代码构建的软件安装系统依赖项，简化了在不同平台上安装系统依赖项的问题，且在多种平台/软件包管理器上受支持。

rosdep是ROS的依赖管理工具，可以与ROS包和外部库一起使用。rosdep用于识别和安装在编译或安装功能包时所需要的依赖项。它可以在以下情况下使用：

• 编译工作空间的功能包需要合适的依赖。

• 安装功能包（例如sudo apt Install-ros-humble-demo-nodes-cpp）之前检查执行所需的依赖。

• 它能够检查单个功能包或一个目录下所有功能包所需要的依赖（例如工作空间）。

17.2 rosdep密钥

一个功能包的package.xml文件包含了一系列的依赖项。此文件中的依赖项通常称为rosdep密钥。这些rosdep密钥在标记、、、和中表示，它们指定在什么情况下需要这些依赖项：

• ：出于混合目的，使用depend 。它包括构建、导出和执行时间依赖关系

• ：对于仅用于测试代码的依赖项，使用test_depend。

• ：对于仅用于构建代码的依赖项，使用build_depend。

• ：对于代码导出头所需的依赖项，使用build_export_depend。

• ：对于仅在运行代码时使用的依赖项，使用exec_depend。

17.3 rosdep工作逻辑

这些依赖项需要手动填充到package.xml文件中，并且应该是功能包所需的所有非内置库和包的详尽列表。rosdep会检查package.xml文件，并查找其中存储的rosdep密钥。这些密钥对照一个称为rosdistro的中心索引，以在各种包管理器中找到可安装的ROS包或软件库。最后一旦在rosdistro找到了这些依赖，rosdep就会安装它们。与rosdistro相关的文件可以在以下地址找到：

https://github.com/ros/rosdistro

17.4 填写rosdep密匙

现在我们来了解如何正确填写密匙名称到package.xml。对于ROS包（例如nav2_bt_navigator），直接填写包的名称。可以在rosdistro的

对于非ROS包的系统依赖项，我们需要找到特定软件库的密钥。通常，有两个感兴趣的文件：rosdep/base.yaml和rosdep/python.yaml。base.yaml包含大部分Linux软件包管理工具apt中可安装的依赖项。python.yaml包含大部分Python软件包管理工具pip中可安装的依赖项。要使用rosdep安装软件库，就在这两个文件中查找软件库对应的密匙名称，然后填写到功能包的package.xml中。总之，只要我们拥有了某个软件库的rosdep密匙，就可以使用rosdep安装软件库，就不再需要去使用这些软件包管理工具安装软件库了。例如，假设一个包依赖于doxygen。我们会在base.yaml查找到以下内容：

doxygen:
  arch: [doxygen]
  debian: [doxygen]
  fedora: [doxygen]
  freebsd: [doxygen]
  gentoo: [app-doc/doxygen]
  macports: [doxygen]
  nixos: [doxygen]
  openembedded: [doxygen@meta-oe]
  opensuse: [doxygen]
  rhel: [doxygen]
  ubuntu: [doxygen]

可以看到在Ubuntu操作系统中，它的rosdep密匙是doxygen。

17.5 收录软件库

此外，rosdistro支持开发者上传未收录的软件库，这对于需要打包部署的项目十分有用。如果要使用的软件库不在rosdistro中，可以自己添加它！对rosdistro的拉取请求通常会在一周内合并。 有关如何提供新的rosdep密钥的详细说明，请参阅此处：

https://github.com/ros/rosdistro/blob/master/CONTRIBUTING.md#rosdep-rules-contributions

如果出于某种原因，这些内容可能无法公开提供，可以分支rosdistro并维护备用索引以供使用。

17.6 使用方法

现在我们对rosdep有了一些了解，现在来学习如何使用它了。首先，如果这是第一次使用rosdep，则必须通过以下方式进行初始化：

sudo rosdep init

接下来更新本地缓存的rosdistro索引。偶尔更新rosdep以获取最新索引是个好主意：

rosdep update

最后，我们可以安装依赖项了。通常，这是在一个工作空间上运行的，以安装所有依赖项。如果工作空间具有包含源代码的目录src，则输入以下指令：

rosdep install --from-paths src -y --ignore-src

注释：--from-paths，指定要检查依赖项的功能包的路径。-y，表示对包管理器的所有提示默认为yes，以便在没有提示的情况下进行安装。--ignore-src，意味着即使存在rosdep密钥，也忽略已安装的依赖项。

更多其他可用的参数和选项，请使用以下指令查看：

rosdep -h

18 模块化可视化工具rqt

18.1 介绍

rqt是一个图形用户界面（GUI）框架，以插件的形式实现各种工具和界面。可以将全部现有的GUI工具作为rqt中的可停靠窗口运行！这些工具仍然可以以传统的独立方式运行，但是rqt使得在单个屏幕布局中管理所有各个窗口变得更加容易。

18.2 安装与启动

打开终端，输入以下指令安装rqt：

sudo apt install ros-humble-rqt* # 安装

输入以下指令打开rqt可视化界面：

rqt

菜单栏插件类型有：

Container     # 容器相关
Actions       # 动作相关, 比如查看动作类型
Configuration # 配置相关, 比如配置动态参数
Debugging     # 调试相关, 比如录制bag
Intorspection # 节点相关, 比如查看节点图
Logging       # 日志相关
Miscellaneours Tools # 杂项,比如shell管理
RobotTools    # 机器人相关,比如控制转向
Services      # 服务相关,比如调用服务
Topics        # 话题相关,比如发布话题
Visualization # 视觉相关,比如查看图像, TF, 曲线图等

18.3 rqt_graph

要可视化节点，以及它们之间的连接，打开终端输入以下指令：

rqt_graph 

18.4 rqt_console

是一个用于在ROS 2中查看日志消息的GUI工具。节点使用日志以各种方式输出有关事件和状态的信息，通常日志显示在终端中，使用rqt_console可以随时收集这些日志消息。rqt_console可以更有序高效的方式查看、过滤、保存、甚至重新加载保存的日志文件，以便在不同的时间进行查看。

打开终端，输入以下指令打开rqt_console：

ros2 run rqt_console rqt_console

页面应该弹出rqt_console的窗口。运行节点后，rqt_console的窗口会输出ROS2的记录级别，每个级别大致含义如下：

• Fatal：消息表明系统将终止以试图保护自己免受损害。

• Error：消息指出了一些重大问题，这些问题不一定会损坏系统，但会妨碍系统正常运行。

• Warn：消息表示意外的活动或不理想的结果，可能代表更深层的问题，但不会直接损害功能。

• Info：消息指示事件和状态更新，作为系统按预期运行的可视验证。

• Debug：消息详细说明了系统执行的整个分步过程。

rqt_console只能看到默认严重级别和更严重级别的消息。默认的严重级别是Info，通常只有Debug消息是隐藏的，因为它是唯一比Info级别低的级别。例如，如果将默认级别设置为Warn，则只会看到严重级别为Warn、Error和Fatal的消息。

打开终端，输入以下指令重新设置默认的严重级别：

ros2 run turtlesim turtlesim_node --ros-args --log-level WARN # 设置默认的严重级别为Warn

19 诊断问题工具ros2doctor

19.1 背景

当ROS2未按设置预期运行时，可以使用ros2doctor工具检查其设置。 ros2doctor检查ROS2的所有方面，包括平台、版本、网络、环境、运行系统等，并警告您可能的错误和问题原因。

19.2 前提

ros2doctor是ros2cli包的一部分。只要安装了ros2cli（任何正常安装都应该已安装），就可以使用ros2doctor。

本教程使用turtlesim来说明一些示例。

19.3 示例

19.3.1 检查设置

首先打开终端，输入以下指令设置环境变量：

source install/setup.bash

输入以下指令，让我们用ros2doctor整体检查一下ROS2设置：

ros2 doctor

这将对所有设置模块进行检查，并返回警告和错误。

如果ROS2设置处于完美状态，将看到以下类似的消息：

All  checks passed

如果收到警告，则会显示如下内容：

: : UserWarning: 

然而，返回一些警告并不罕见，UserWarning并不意味着设置不可用，这更可能只是一种迹象，表明某些东西的配置方式并不理想。例如，如果使用不稳定的ROS2分布，ros2doctor将发现此警告：

UserWarning: Distribution  is not fully supported or tested. To get more consistent features, download a stable version at https://index.ros.org/doc/ros2/Installation/

如果ros2doctor仅在系统中找到警告，仍然会收到All＜n＞checks passed消息。大多数检查被归类为警告而不是错误。这主要取决于用户来确定ros2doctor返回的反馈的重要性。如果它确实在设置中发现了一个罕见的错误，由UserWarning:error:表示，检查将被视为失败，且将在问题反馈列表中看到类似的消息如下：

1/3 checks failed

Failed modules:  network

错误表示系统缺少对ROS2至关重要的设置或功能。应纠正错误以确保系统正常运行。

19.3.2 检查系统

使用ros2doctor还可以检查正在运行的ROS2系统，以确定问题的可能原因。让我们结合海龟机器人，学习如何使用ros2doctor检测运行系统的工作情况。打开一个新终端，运行海龟：

ros2 run turtlesim turtlesim_node # 运行海龟

打开一个新的终端，运行海龟控制器：

ros2 run turtlesim turtle_teleop_key # 运行海龟控制器

再打开一个新的终端，输入以下指令：

ros2 doctor

将看到上次检查设置运行ros2doctor时出现的警告和错误（如果有的话）。以下是与系统本身相关的几个新警告：

UserWarning: Publisher without subscriber detected on /turtle1/color_sensor.
UserWarning: Publisher without subscriber detected on /turtle1/pose.

/turtlesim节点似乎向两个未被订阅的主题发布了数据，ros2doctor认为这可能会导致问题。如果运行命令来回应/color_sensor和/pose主题，这些警告将消失，因为发布者拥有了订阅者，我们来尝试一下。

打开一个新终端，输入以下指令：

ros2 topic echo /turtle1/color_sensor

再打开一个新终端，输入以下指令：

ros2 topic echo /turtle1/pose

在终端重新输入以下指令：

ros2 doctor

可以看到，发布者没有订阅者的警告消失了。在具有多个节点的复杂系统中，ros2doctor对于识别通信问题的可能原因非常有用。

19.3.3 获取报告

虽然ros2doctor会让您知道有关网络、系统等的警告，但使用--report参数运行它会提供更多详细信息，帮助您分析问题。

如果您收到有关网络设置的警告，并想确切了解导致该警告的配置部分，则可能需要使用--report。

当您需要打开支持票证以获得ROS2的帮助时，它也非常有用。您可以将报告的相关部分复制并粘贴到票证中，以便帮助您的人员更好地了解您的环境并提供更好的帮助。

要获取完整报告，在终端中输入以下命令：

ros2 doctor --report

它将返回分为五组的信息列表：

NETWORK CONFIGURATION
...

PLATFORM INFORMATION
...

RMW MIDDLEWARE
...

ROS 2 INFORMATION
...

TOPIC LIST
...

我们可以将此处的信息与运行ros2doctor时得到的警告进行交叉检查。例如，如果ros2doctor返回警告您的发行版“未得到充分支持或测试”，则可以查看报告的ROS2 INFORMATION部分：

distribution name      : 
distribution type      : ros2
distribution status    : prerelease
release platforms      : {'': ['']}

在这里可以看到发行状态是预发布的，这解释了为什么不完全支持它。

19.4 总结

ros2doctor将通知您ROS2安装和运行系统中的问题。通过使用--report参数，您可以更深入地了解这些警告背后的信息。

记住，ros2doctor不是调试工具；它对代码或系统实现端的错误没有帮助。

ros2doctor的README将告诉您有关不同参数的更多信息：

https://github.com/ros2/ros2cli/tree/humble/ros2doctor

20 记录与回放工具ros2bag

20.1 从命令行记录包

20.1.1 背景

ros2bag是一个命令行工具，用于记录发布在系统话题上的数据。它累积传递给任意数量主题的数据，并将其保存在数据库中。然后，您可以回放数据以再现测试和实验的结果。录制话题也是分享您的作品并允许他人重新创作的好方法。

20.1.2 前提

您应该已经将ros2bag安装为常规ROS2设置的一部分。如果您从Debian安装，但系统无法识别该命令，请按如下方式安装：

sudo apt-get install ros-humble-ros2bag \
                     ros-humble-rosbag2-storage-default-plugins

本教程含有以前教程中涉及的概念，如节点和话题。它还使用了turtlesim功能包。

一如既往，不要忘记在您打开的每一个新终端中设置环境变量。

20.1.3 任务

20.1.3.1 设置

您将在turtlesim系统中记录键盘输入，以便稍后保存和回放，因此首先启动/turtlesim和/teolep_turtle节点。

打开终端，输入以下指令运行海龟：

ros2 run turtlesim turtlesim_node # 运行海龟

打开一个新的终端，输入以下指令运行海龟控制器：

ros2 run turtlesim turtle_teleop_key # 运行海龟控制器

让我们也创建一个新目录来存储保存的录音：

mkdir ~/bag_files
cd bag_files

20.1.3.2 选择话题

ros2bag只能记录发布在话题上的数据。若要查看系统话题题列表，输入以下指令：

ros2 topic list

将返回：

/parameter_events
/rosout
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

在话题教程中，您了解到/turtle_teleop节点在/turtle1/cmd_vel话题上发布命令，以使海龟在turtlesim中移动。

要查看/tuttle1/cmd_vel正在发布的数据，请运行以下命令：

ros2 topic echo /turtle1/cmd_vel

一开始什么都不会出现，因为控制终端没有发布任何数据。返回运行控制海龟的终端并选择它，使其处于活动状态。使用箭头键移动海龟，您将看到在运行ros2 topic echo的终端上发布的数据。

linear:
  x: 2.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0
  ---

20.1.3.3 记录话题

要记录发布到话题的数据，请使用语法命令：

ros2 bag record 

在您选择的话题上运行此命令之前，请打开一个新终端并移动到之前创建的bag_files目录中，因为rosbag文件将保存在您运行它的目录中。

运行命令：

ros2 bag record /turtle1/cmd_vel

您将在终端中看到以下消息（日期和时间不同）：

[INFO] [rosbag2_storage]: Opened database 'rosbag2_2019_10_11-05_18_45'.
[INFO] [rosbag2_transport]: Listening for topics...
[INFO] [rosbag2_transport]: Subscribed to topic '/turtle1/cmd_vel'
[INFO] [rosbag2_transport]: All requested topics are subscribed. Stopping discovery...

现在，ros2bag正在记录/turtle1/cmd_vel主题上发布的数据。回到控制海龟的终端，让海龟运动一会儿，然后按下Ctrl+C停止记录。

数据将以rosbag2_year_month_dayhour_minute_second的格式存储在一个包文件中。

20.1.3.4 记录多个话题

您还可以记录多个话题，以及更改ros2bag保存到的包文件的名称。

运行以下命令：

ros2 bag record -o subset /turtle1/cmd_vel /turtle1/pose

-o参数允许您为包文件选择特定的的名称，在本例中为subset是文件名。

要一次记录多个话题，只需列出用空格分隔的每个话题。您将看到以下消息，确认正在录制两个话题。

[INFO] [rosbag2_storage]: Opened database 'subset'.
[INFO] [rosbag2_transport]: Listening for topics...
[INFO] [rosbag2_transport]: Subscribed to topic '/turtle1/cmd_vel'
[INFO] [rosbag2_transport]: Subscribed to topic '/turtle1/pose'
[INFO] [rosbag2_transport]: All requested topics are subscribed. Stopping discovery...

完成后，可以移动海龟并按Ctrl+C结束。

还有一个参数-a可以添加到命令中，它记录系统上的所有话题。

20.1.3.5 包文件信息

您可以通过运行以下命令查看有关记录的详细信息：

ros2 bag info 

在subset包文件上运行此命令将返回包文件的信息列表：

ros2 bag info subset

Files:             subset.db3
Bag size:          228.5 KiB
Storage id:        sqlite3
Duration:          48.47s
Start:             Oct 11 2019 06:09:09.12 (1570799349.12)
End                Oct 11 2019 06:09:57.60 (1570799397.60)
Messages:          3013
Topic information: Topic: /turtle1/cmd_vel | Type: geometry_msgs/msg/Twist | Count: 9 | Serialization Format: cdr
                 Topic: /turtle1/pose | Type: turtlesim/msg/Pose | Count: 3004 | Serialization Format: cdr

要查看单个消息，您必须打开数据库（在本例中为sqlite3）来检查它，这超出了ROS2的范围。

20.1.3.6 回放话题数据

在回放包文件之前，在正在运行的控制终端中输入Ctrl+C。然后确保您的海龟窗口可见，以便您可以看到正在运行的包文件。

输入以下指令：

ros2 bag play subset

终端将返回消息：

[INFO] [rosbag2_storage]: Opened database 'subset'.

您的海龟将遵循您在记录时输入的相同路径（虽然不是100%准确；turtlesim对系统计时的微小变化很敏感）。

因为子subset记录了/turtle1/pose话题，所以只要您运行了turtlesim，即使您没有移动，ros2 bag play命令也不会退出。 这是因为，只要/turtlesim节点处于活动状态，它就会定期发布/turtle1/pose话题上的数据。在上面的ros2bag信息示例结果中，您可能注意到/tuttle1/cmd_vel主题的计数信息仅为9；这就是我们在记录时按下箭头键的次数。 请注意，/tuttle1/pose的计数值超过3000；在我们记录的时候，关于这个话题的数据被发布了3000次。 要了解发布海龟姿态数据的频率，可以运行以下命令：

ros2 topic hz /turtle1/pose

20.2 从节点记录包

20.2.1 背景

rosbag2不仅仅提供了ros2包命令行工具。它还提供了一个Python API，用于从自己的源代码中读取和写入包。这允许您订阅话题并将接收到的数据保存到包中，同时对该数据执行您选择的任何其他处理。例如，保存来自话题的数据和处理该数据的结果，而无需将处理后的数据发送到话题上进行记录。因为任何数据都可以记录在包中，所以也可以保存由话题以外的其他源生成的数据，例如训练集的合成数据。例如，这对于快速生成包含大量样本的包非常有用，这些样本在很长的时间内传播。

20.2.2 前提

您应该已经将rosbag2包安装为常规ROS2设置的一部分了。

如果您是从Linux上的Debian软件包安装的，那么它可能是默认安装的。如果不是，可以使用此命令安装它。

sudo apt install ros-humble-rosbag2

本教程讨论使用ROS 2 bags。您应该已经完成了基本的ROS 2 bag教程。

20.2.3 任务

20.2.3.1 创建功能包

打开一个新终端并为ROS2设置环境变量，以便ROS2命令能够正常工作。

按照以下说明创建名为ros2_ws的新工作空间。

进入到ros2_ws/src目录并创建新功能包：

ros2 pkg create --build-type ament_python bag_recorder_nodes_py --dependencies rclpy rosbag2_py example_interfaces std_msgs

您的终端将返回一条消息，验证功能包bag_recorder_nodes_py及其所有必要文件和文件夹的创建。--dependencies参数将自动向package.xml.txt添加必要的依赖项。在这种情况下，功能包将使用rosbag2_py和rclpy。消息定义还需要依赖example_interfaces。

20.2.3.2 更新package.xml与setup.py

因为在功能包创建期间使用了--dependencies选项，所以不必手动将依赖项添加到package.xml中。但是，一如既往，请确保将描述、维护者电子邮件和名称以及许可证信息添加到package.xml中。

Python bag writing tutorial
Your Name
Apache License 2.0

此外，还要确保将这些信息添加到setup.py文件中。

maintainer='Your Name',
maintainer_email='[email protected]',
description='Python bag writing tutorial',
license='Apache License 2.0',

20.2.3.3 编写节点

在ros2_ws/src/bag_recorder_nodes_py/bag_recorder_nodes_py目录中，创建一个名为simple_bag_recorder.py的新文件，并将以下代码粘贴到其中。

import rclpy
from rclpy.node import Node
from rclpy.serialization import serialize_message
from std_msgs.msg import String

import rosbag2_py

class SimpleBagRecorder(Node):
    def __init__(self):
        super().__init__('simple_bag_recorder')
        self.writer = rosbag2_py.SequentialWriter()

        storage_options = rosbag2_py._storage.StorageOptions(
            uri='my_bag',
            storage_id='sqlite3')
        converter_options = rosbag2_py._storage.ConverterOptions('', '')
        self.writer.open(storage_options, converter_options)

        topic_info = rosbag2_py._storage.TopicMetadata(
            name='chatter',
            type='std_msgs/msg/String',
            serialization_format='cdr')
        self.writer.create_topic(topic_info)

        self.subscription = self.create_subscription(
            String,
            'chatter',
            self.topic_callback,
            10)
        self.subscription

    def topic_callback(self, msg):
        self.writer.write(
            'chatter',
            serialize_message(msg),
            self.get_clock().now().nanoseconds)


def main(args=None):
    rclpy.init(args=args)
    sbr = SimpleBagRecorder()
    rclpy.spin(sbr)
    rclpy.shutdown()


if __name__ == '__main__':
    main()

20.2.3.4 检查代码

顶部的import语句是功能包依赖项。请注意，为使用包文件所需的功能和结构导入了rosbag2_py包。

在类构造函数中，我们首先创建将用于写入包的writer对象。我们正在创建一个SequentialWriter，它按照收到的顺序将消息写入包中。

self.writer = rosbag2_py.SequentialWriter()

现在我们有了一个writer对象，我们可以使用它打开包。我们指定要创建的包的URI和格式（sqlite3），其他选项保留默认值。使用默认的转换选项，它将不执行转换，并以接收消息的以序列化格式存储消息。

storage_options = rosbag2_py._storage.StorageOptions(
    uri='my_bag',
    storage_id='sqlite3')
converter_options = rosbag2_py._storage.ConverterOptions('', '')
self.writer.open(storage_options, converter_options)

接下来，我们需要告诉writer我们希望存储的话题。这是通过创建TopicMetadata对象并将其注册到writer来完成的。此对象指定话题名称、话题消息类型和序列化格式。

topic_info = rosbag2_py._storage.TopicMetadata(
    name='chatter',
    type='std_msgs/msg/String',
    serialization_format='cdr')
self.writer.create_topic(topic_info)

现在writer设置为记录我们传递给它的数据，我们创建一个订阅并为它指定一个回调。我们将在回调中将数据写入包。

self.subscription = self.create_subscription(
    String,
    'chatter',
    self.topic_callback,
    10)
self.subscription

回调以非序列化的形式接收消息（这是rcppy API的标准），并将消息传递给writer，指定数据所针对的话题和要与消息一起记录的时间戳。然而，writer需要将序列化的信息存储在包中。这意味着我们需要在将数据传递给writer之前对其进行序列化。因此，我们调用serialize_message()并将其结果传递给writer，而不是直接传递消息。

def topic_callback(self, msg):
    self.writer.write(
        'chatter',
        serialize_message(msg),
        self.get_clock().now().nanoseconds)

该文件以用于创建节点实例并启动ROS处理它的主函数结束。

def main(args=None):
    rclpy.init(args=args)
    sbr = SimpleBagRecorder()
    rclpy.spin(sbr)
    rclpy.shutdown()

20.2.3.5 添加入口点

打开bag_recorder_nodes_py包中的setup.py文件，并为节点添加入口点。

entry_points={
    'console_scripts': [
        'simple_bag_recorder = bag_recorder_nodes_py.simple_bag_recorder:main',
        'data_generator_node = bag_recorder_nodes_py.data_generator_node:main',
    ],
},

20.2.3.6 编译与运行

进入工作空间的根目录ros2_ws，编译功能包。

colcon build --packages-select bag_recorder_nodes_py

打开新终端，进入到ros2_ws，设置环境变量。

source install/setup.bash

现在运行节点。

ros2 run bag_recorder_nodes_py simple_bag_recorder

打开第二个终端并运行talker示例节点。

ros2 run demo_nodes_cpp talker

这将开始发布有关chatter话题的数据。当包写入节点接收到该数据时，它会将其写入my_bag包。如果my_bag目录已经存在，则必须在运行simple_bag_recorder节点之前先删除它。这是因为默认情况下，rosbag2不会覆盖现有的包，因此目标目录不能存在。

终止两个节点。然后，在一个终端中启动listener示例节点。

ros2 run demo_nodes_cpp listener

在另一个终端中，使用ros2bag回放节点录制的包。

ros2 bag play my_bag

您将看到listener节点正在接收来自包的消息。

如果希望再次运行包写入节点，首先需要删除my_bag目录。

20.3 从节点记录合成数据

任何数据都可以记录到一个包中，而不仅仅是通过话题接收的数据。从自己的节点写入包的常见用例是生成和存储合成数据。在本节中，将学习如何编写生成一些数据并将其存储在包中的节点。我们将演示两种方法。第一种使用带有计时器的节点；如果数据生成在节点外部，例如直接从硬件（例如相机）读取数据，则可以使用这种方法。第二种方法不使用节点；这是当不需要使用ROS基础设施的任何功能时可以使用的方法。

20.3.1 编写节点

在ros2_ws/src/bag_recorder_nodes_py/bag_recorder_nodes_py目录中，创建一个名为data_generator_node.py的新文件，并将以下代码粘贴到其中。

import rclpy
from rclpy.node import Node
from rclpy.serialization import serialize_message
from example_interfaces.msg import Int32

import rosbag2_py

class DataGeneratorNode(Node):
    def __init__(self):
        super().__init__('data_generator_node')
        self.data = Int32()
        self.data.data = 0
        self.writer = rosbag2_py.SequentialWriter()

        storage_options = rosbag2_py._storage.StorageOptions(
            uri='timed_synthetic_bag',
            storage_id='sqlite3')
        converter_options = rosbag2_py._storage.ConverterOptions('', '')
        self.writer.open(storage_options, converter_options)

        topic_info = rosbag2_py._storage.TopicMetadata(
            name='synthetic',
            type='example_interfaces/msg/Int32',
            serialization_format='cdr')
        self.writer.create_topic(topic_info)

        self.timer = self.create_timer(1, self.timer_callback)

    def timer_callback(self):
        self.writer.write(
            'synthetic',
            serialize_message(self.data),
            self.get_clock().now().nanoseconds)
        self.data.data += 1


def main(args=None):
    rclpy.init(args=args)
    dgn = DataGeneratorNode()
    rclpy.spin(dgn)
    rclpy.shutdown()


if __name__ == '__main__':
    main()

20.3.2 检查代码

此代码的大部分与之前的示例相同。这里描述重要的区别。

首先，更改包的名称。

storage_options = rosbag2_py._storage.StorageOptions(
    uri='timed_synthetic_bag',
    storage_id='sqlite3')

话题的名称需要更改，存储的数据类型也需要更改。

topic_info = rosbag2_py._storage.TopicMetadata(
    name='synthetic',
    type='example_interfaces/msg/Int32',
    serialization_format='cdr')
self.writer.create_topic(topic_info)

此节点具有计时器，而不是对话题的订阅。计时器以1秒为启动周期，并在启动时调用给定的成员函数。

self.timer = self.create_timer(1, self.timer_callback)

在计时器回调中，我们生成（或以其他方式获得，例如从连接到某些硬件的串行端口读取）希望存储在包中的数据。与前面的示例一样，数据尚未序列化，因此我们必须在将其传递给writer之前对其进行序列化。

self.writer.write(
    'synthetic',
    serialize_message(self.data),
    self.get_clock().now().nanoseconds)

20.3.3 添加入口点

打开bag_recorder_nodes_py包中的setup.py文件，并为节点添加入口点。

entry_points={
    'console_scripts': [
        'simple_bag_recorder = bag_recorder_nodes_py.simple_bag_recorder:main',
        'data_generator_node = bag_recorder_nodes_py.data_generator_node:main',
    ],
},

20.3.4 编译与运行

进入工作空间的根目录ros2_ws，编译功能包。

colcon build --packages-select bag_recorder_nodes_py

打开新终端，进入到ros2_ws，设置环境变量。

source install/setup.bash

如果timed_synthetic_bag目录已经存在，则必须在运行节点之前先将其删除。

现在运行节点。

ros2 run bag_recorder_nodes_py data_generator_node

等待30秒左右，然后终止节点。接下来，回放创建的包。

ros2 bag play timed_synthetic_bag

打开另一个终端并响应//synthetic话题。

ros2 topic echo /synthetic

您将看到生成并存储在包中的数据以每秒一条消息的速度打印到控制台。

20.4 从可执行文件记录合成数据

现在，您可以创建一个包来存储来自话题以外的源的数据，您将学习如何从非节点可执行文件生成和记录合成数据。这种方法的优点是代码更简单，可以快速创建大量数据。

20.4.1 编写可执行文件

在ros2_ws/src/bag_recorder_nodes_py/bag_recorder_nodes.py目录中，创建一个名为data_generator_executable.py的新文件，并将以下代码粘贴到其中。

from rclpy.clock import Clock
from rclpy.duration import Duration
from rclpy.serialization import serialize_message
from example_interfaces.msg import Int32

import rosbag2_py


def main(args=None):
    writer = rosbag2_py.SequentialWriter()

    storage_options = rosbag2_py._storage.StorageOptions(
        uri='big_synthetic_bag',
        storage_id='sqlite3')
    converter_options = rosbag2_py._storage.ConverterOptions('', '')
    writer.open(storage_options, converter_options)

    topic_info = rosbag2_py._storage.TopicMetadata(
        name='synthetic',
        type='example_interfaces/msg/Int32',
        serialization_format='cdr')
    writer.create_topic(topic_info)

    time_stamp = Clock().now()
    for ii in range(0, 100):
        data = Int32()
        data.data = ii
        writer.write(
            'synthetic',
            serialize_message(data),
            time_stamp.nanoseconds)
        time_stamp += Duration(seconds=1)

if __name__ == '__main__':
    main()

20.4.2 检查代码

将此样本与前一个样本进行比较，会发现它们并没有那么不同。唯一显著的区别是使用for循环来驱动数据生成，而不是使用计时器。

请注意，我们现在也在为数据生成时间戳，而不是依赖于每个样本的当前系统时间。时间戳可以是您需要的任何值。数据将以这些时间戳给定的速率播放，因此这是控制样本默认播放速度的有用方法。还要注意，虽然每个样本之间的间隔是整整一秒，但这个可执行文件不需要在每个样本之间等待一秒。这使我们能够在比回放所需时间短得多的时间内生成大量涵盖广泛时间跨度的数据。

time_stamp = Clock().now()
for ii in range(0, 100):
    data = Int32()
    data.data = ii
    writer.write(
        'synthetic',
        serialize_message(data),
        time_stamp.nanoseconds)
    time_stamp += Duration(seconds=1)

20.4.3 添加入口点

打开bag_recorder_nodes_py包中的setup.py文件，并为节点添加入口点。

entry_points={
    'console_scripts': [
        'simple_bag_recorder = bag_recorder_nodes_py.simple_bag_recorder:main',
        'data_generator_node = bag_recorder_nodes_py.data_generator_node:main',
        'data_generator_executable = bag_recorder_nodes_py.data_generator_executable:main',
    ],
},

20.4.4 编译与运行

进入工作空间的根目录ros2_ws，编译功能包。

colcon build --packages-select bag_recorder_nodes_py

打开新终端，进入到ros2_ws，设置环境变量。

source install/setup.bash

如果big_synthetic_bag目录已经存在，则必须在运行可执行文件之前先将其删除。

现在运行节点。

ros2 run bag_recorder_nodes_py data_generator_executable

请注意，可执行文件的运行和完成速度非常快。

现在回放创建的包。

ros2 bag play big_synthetic_bag

打开另一个终端并响应//synthetic话题。

ros2 topic echo /synthetic

您将看到生成并存储在包中的数据以每秒一条消息的速度打印到控制台。即使包生成得很快，它仍然以时间戳指示的速率回放。

20.5 总结

您可以使用ros2bag命令记录ROS2系统中传递的话题数据。无论你是与他人分享你的工作，还是反思自己的实验，这都是一个很好的工具。

您创建了一个节点，该节点将接收到的有关话题的数据记录到包中。您测试了使用节点录制包，并通过回放包来验证数据是否已录制。该方法可用于记录具有比在话题上接收到的更多数据的包，例如处理接收到的数据所获得的结果。然后，您继续创建一个节点和一个可执行文件，以生成合成数据并将其存储在包中。后一种方法尤其适用于生成可作为训练集使用的合成数据。

21 启动工具launch

在开发篇，facer所有节点的任务都已经实现。使用之前的方法运行节点，由于运行的节点会占用终端，所以一个终端只能运行一个节点。要完整的运行facer，我们不得不打开很多终端，输入指令设置环境变量后，再输入运行节点指令。在前面的教程中，我们一直在为运行的每个新节点打开新终端。当创建越来越多节点，同时运行的系统更复杂，打开终端并重新设置环境变量等细节将变得低效繁杂。现在我们来学习可以解决上述问题的启动工具launch。

21.1 创建启动文件

21.1.1 背景

ROS2中的启动系统负责帮助用户描述其系统的配置，然后按照描述执行。系统的配置包括要运行什么程序、在哪里运行它们、传递它们的参数以及ROS特定的约定，这些约定通过为每个组件提供不同的配置，使易于在整个系统中重用组件。它还负责监测启动流程的状态，并报告和或对这些流程状态的变化作出反应。

用Python、XML或YAML编写的启动文件可以启动和停止不同的节点，也可以触发和处理各种事件。启动文件可以同时启动和配置包含ROS2节点的多个可执行文件，完成启动文件编写后，使用ros2launch命令运行单个启动文件将立即启动整个系统的所有节点及其配置。

设计文件详细说明了ROS 2的launch系统的设计目标（目前并非所有功能都可用）:

https://design.ros2.org/articles/roslaunch.html

21.1.2 前提

本教程使用turtlesim功能包。

您还需要使用自己喜欢的文本编辑器。

一如既往，不要忘记在您打开的每一个新终端中设置环境变量。

21.1.3 示例

21.1.3.1 设置

创建一个新的目录来存储launch文件：

mkdir launch

21.1.3.2 编写启动文件

让我们使用turtlesim包及其可执行文件创建一个ROS2启动文件。如上所述，这可以是Python、XML或YAML，我们使用Python文件。创建launch/turtlesim_mimic_launch.py文件，添加以下内容。

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            namespace='turtlesim1',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='turtlesim',
            namespace='turtlesim2',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='turtlesim',
            executable='mimic',
            name='mimic',
            remappings=[
                ('/input/pose', '/turtlesim1/turtle1/pose'),
                ('/output/cmd_vel', '/turtlesim2/turtle1/cmd_vel'),
            ]
        )
    ])

21.1.3.3 检查启动文件

上面的所有启动文件可启动一个由三个节点组成的系统，所有这些节点都来自于turtlesim功能包。该启动文件的目的是启动两个海龟模拟窗口，并让一只海龟模仿另一只海龟的运动。

当启动两个turtlesim节点时，它们之间的唯一区别是命名空间值。独有的命名空间允许系统启动两个节点，而不发生节点名称或话题名称冲突。这个系统中的两只海龟都会在同一话题上接收命令，并在同一个话题上发布它们的姿态。使用独有的名称空间，可以区分针对不同海龟的消息。

最后一个节点也来自于turtlesim功能包，但它是一个不同的可执行文件：mimic。此节点以重新映射的形式添加了配置信息。mimic节点的/input/pose话题被重新映射到/turtlesim1/turtle1/pose，/output/cmd_vel话题被映射到/tuttlesim2/turtle1/cmd_vel。这意味着mimic节点将订阅/turtlesim1/sim的姿态话题，并将其重新发布到速度命令话题，以供/turtlesim2/sim订阅。换句话说，turtlesim2将模仿turtlesim1的运动。

下面我们详细查看一下代码。

这些导入语句引入了一些启动模块：

from launch import LaunchDescription
from launch_ros.actions import Node

接下来，开始描述启动：

def generate_launch_description():
	return LaunchDescription([
	
	])

在启动描述中的前两条语句启动了两个海龟模拟窗口：

Node(
    package='turtlesim',
    namespace='turtlesim1',
    executable='turtlesim_node',
    name='sim'
),
Node(
    package='turtlesim',
    namespace='turtlesim2',
    executable='turtlesim_node',
    name='sim'
),

最后使用重映射启动模仿节点：

Node(
    package='turtlesim',
    executable='mimic',
    name='mimic',
    remappings=[
      ('/input/pose', '/turtlesim1/turtle1/pose'),
      ('/output/cmd_vel', '/turtlesim2/turtle1/cmd_vel'),
    ]
)

21.1.3.4 运行启动文件

要运行上面创建的启动文件，进入launch目录并运行以下命令：

ros2 launch turtlesim_mimic_launch.py

运行启动文件后，两个turtlesim窗口将打开，将看到以下启动文件已启动节点的消息：

[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [turtlesim_node-1]: process started with pid [11714]
[INFO] [turtlesim_node-2]: process started with pid [11715]
[INFO] [mimic-3]: process started with pid [11716]

要查看系统的运行情况，请打开一个新终端并在/turtlesim1/turtle1/cmd_vel话题上运行ros2 topic pub命令，使第一只乌龟运动：

ros2 topic pub -r 1 /turtlesim1/turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: -1.8}}"

将会看到第二只海龟在模仿第一只海龟的运动。

21.1.4 总结

启动文件简化了运行具有许多节点和特定配置细节的复杂系统。您可以使用Python、XML或YAML创建启动文件，并使用ros2启动命令运行它们。

21.2 集成启动文件到功能包

21.2.1 背景

在前面我们了解了如何编写独立的启动文件，直接运行了启动文件。本现将展示如何将启动文件添加到现有的功能包中，以及通常使用的约定。

21.1.2 前提

您应该已经完成了如何创建ROS 2功能包的教程。

一如既往，不要忘记在您打开的每一个新终端中设置环境变量。

21.1.3 示例

21.1.3.1 创建功能包

创建一个工作空间：

mkdir -p launch_ws/src
cd launch_ws/src

创建一个功能包：

ros2 pkg create py_launch_example --build-type ament_python

21.1.3.2 创建存放启动文件的目录

按照惯例，功能包的所有启动文件都存储在功能包内的launch目录中。确保在上面创建的功能包的顶层创建启动目录：

mkdir py_launch_example/launch

对于Python功能包，功能包的目录应如下所示：

src/
  py_launch_example/
  	launch/
    package.xml
    py_launch_example/
    resource/
    setup.py
    setup.cfg
    test/

为了让colcon找到启动文件，我们需要使用setup.py的data_files参数，通知Python的安装工具安装启动文件，打开setup.py文件添加以下内容：

import os
from glob import glob
from setuptools import setup

package_name = 'py_launch_example'

setup(
    # Other parameters ...
    data_files=[
        # ... Other data files
        # Include all launch files.
        (os.path.join('share', package_name), glob('launch/*launch.[pxy][yma]*'))
    ]
)

21.1.3.3 编写启动文件

在launch目录中，创建一个名为my_script_launch.py的新启动文件，建议使用这种命名方式，但不是必需的。但是启动文件名需要以launch.py结尾，这样才能被ros2 launch识别并自动完成。

启动文件应定义generate_launch_description()函数，其返回值是调用LaunchDescription()函数的结果。打开启动文件填入以下内容：

import launch
import launch_ros.actions

def generate_launch_description():
    return launch.LaunchDescription([
        launch_ros.actions.Node(
            package='demo_nodes_cpp',
            executable='talker',
            name='talker'),
  ])

对于包含启动文件的功能包，最好在功能包的package.xml中添加对ros2launch包的依赖，这有助于确保在编译功能包后ros2启动命令可用，它还确保识别所有启动文件格式。打开package.xml添加以下内容：

ros2launch

21.1.3.4 编译并运行启动文件

回到工作空间目录，编译功能包：

colcon build

设置环境变量：

source install/setup.bash

在编译成功设置环境变量后，应该能够按如下方式运行启动文件：

ros2 launch py_launch_example my_script_launch.py

文档

launch文档提供了更多详细信息：

https://github.com/ros2/launch/blob/humble/launch/doc/source/architecture.rst

launch源代码提供了更多详细信息：

https://github.com/ros2/launch
https://github.com/ros2/launch_ros

21.3 使用取代

21.3.1 背景

启动文件用于启动节点、服务和执行进程，这些操作可能有影响其行为的参数。可以在参数中使用取代，以在描述可重用的启动文件时提供更大的灵活性。取代（substitutions）是仅在执行启动描述期间才会被计算的变量，可用于获取特定信息，例如启动配置、环境变量或计算一个任意的Python表达式。

本教程将展示在ROS2启动文件中取代的用法示例。

21.3.2 前提

本教程使用turtlesim功能包。本教程还假设您熟悉如何创建功能包。

一如既往，不要忘记在您打开的每一个新终端中设置环境变量。

21.3.3 示例

21.3.3.1 创建与设置功能包

创建一个名为launch_tutorial的python功能包：

ros2 pkg create launch_tutorial --build-type ament_python

进入功能包创建launch文件夹：

mkdir launch_tutorial/launch

最后，为了让启动文件能够被安装，确保添加如下内容到setup.py：

import os
from glob import glob
from setuptools import setup

package_name = 'launch_tutorial'

setup(
    # Other parameters ...
    data_files=[
        # ... Other data files
        # Include all launch files.
        (os.path.join('share', package_name), glob('launch/*launch.[pxy][yma]*'))
    ]
)

21.3.3.2 父启动文件

让我们创建一个启动文件，该文件将调用参数并将其传递给另一个启动文件。为此，请在launch_tutorial功能包的launch目录下创建example_main.launch.py，并添加以下内容：

from launch_ros.substitutions import FindPackageShare

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution, TextSubstitution


def generate_launch_description():
    colors = {
        'background_r': '200'
    }

    return LaunchDescription([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([
                PathJoinSubstitution([
                    FindPackageShare('launch_tutorial'),
                    'example_substitutions.launch.py'
                ])
            ]),
            launch_arguments={
                'turtlesim_ns': 'turtlesim2',
                'use_provided_red': 'True',
                'new_background_r': TextSubstitution(text=str(colors['background_r']))
            }.items()
        )
    ])

在example_main.launch.py文件中，FindPackageShare取代用于找到launch_tutorial功能包的路径。然后使用PathJoinSubstitution取代将路径与example_substitutions.launch.py文件名连接。

PathJoinSubstitution([
    FindPackageShare('launch_tutorial'),
    'example_substitutions.launch.py'
])

launch_argument字典中的turtlesim_ns和use_provided_red参数被传递到IncludeLaunchDescription，TextSubstitution取代用于定义new_background_r参数，其值为colors字典中的background_r键对应的值。

launch_arguments={
    'turtlesim_ns': 'turtlesim2',
    'use_provided_red': 'True',
    'new_background_r': TextSubstitution(text=str(colors['background_r']))
}.items()

21.3.3.3 取代示例启动文件

接下来，同样在launch目录创建一个名为example_substitutions.launch.py的启动文件，填入以下内容：

from launch_ros.actions import Node

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression


def generate_launch_description():
    turtlesim_ns = LaunchConfiguration('turtlesim_ns')
    use_provided_red = LaunchConfiguration('use_provided_red')
    new_background_r = LaunchConfiguration('new_background_r')

    turtlesim_ns_launch_arg = DeclareLaunchArgument(
        'turtlesim_ns',
        default_value='turtlesim1'
    )
    use_provided_red_launch_arg = DeclareLaunchArgument(
        'use_provided_red',
        default_value='False'
    )
    new_background_r_launch_arg = DeclareLaunchArgument(
        'new_background_r',
        default_value='200'
    )

    turtlesim_node = Node(
        package='turtlesim',
        namespace=turtlesim_ns,
        executable='turtlesim_node',
        name='sim'
    )
    spawn_turtle = ExecuteProcess(
        cmd=[[
            'ros2 service call ',
            turtlesim_ns,
            '/spawn ',
            'turtlesim/srv/Spawn ',
            '"{x: 2, y: 2, theta: 0.2}"'
        ]],
        shell=True
    )
    change_background_r = ExecuteProcess(
        cmd=[[
            'ros2 param set ',
            turtlesim_ns,
            '/sim background_r ',
            '120'
        ]],
        shell=True
    )
    change_background_r_conditioned = ExecuteProcess(
        condition=IfCondition(
            PythonExpression([
                new_background_r,
                ' == 200',
                ' and ',
                use_provided_red
            ])
        ),
        cmd=[[
            'ros2 param set ',
            turtlesim_ns,
            '/sim background_r ',
            new_background_r
        ]],
        shell=True
    )

    return LaunchDescription([
        turtlesim_ns_launch_arg,
        use_provided_red_launch_arg,
        new_background_r_launch_arg,
        turtlesim_node,
        spawn_turtle,
        change_background_r,
        TimerAction(
            period=2.0,
            actions=[change_background_r_conditioned],
        )
    ])

在example_substitutions.launch.py文件中，定义了turtlesim_ns、use_provided_red和new_background_r等启动配置。它们用于将启动参数的值存储在上述变量中，并将其传递给所需的操作。这些LaunchConfiguration取代允许我们在启动描述的任何部分获取启动参数的值。

DeclareLaunchArgument用于定义可以从上述启动文件或控制台传递的启动参数。

turtlesim_ns = LaunchConfiguration('turtlesim_ns')
use_provided_red = LaunchConfiguration('use_provided_red')
new_background_r = LaunchConfiguration('new_background_r')

turtlesim_ns_launch_arg = DeclareLaunchArgument(
    'turtlesim_ns',
    default_value='turtlesim1'
)
use_provided_red_launch_arg = DeclareLaunchArgument(
    'use_provided_red',
    default_value='False'
)
new_background_r_launch_arg = DeclareLaunchArgument(
    'new_background_r',
    default_value='200'
)

turtlesim_node节点的命名空间设置为turtlesim_ns。

turtlesim_node = Node(
    package='turtlesim',
    namespace=turtlesim_ns,
    executable='turtlesim_node',
    name='sim'
)

然后，用相应的cmd参数定义名为spawn_turtle的ExecuteProcess操作。该命令调用turtlesim节点的派生服务。

此外，LaunchConfiguration取代用于获取turtlesim_ns启动参数的值，以构造命令字符串。

spawn_turtle = ExecuteProcess(
    cmd=[[
        'ros2 service call ',
        turtlesim_ns,
        '/spawn ',
        'turtlesim/srv/Spawn ',
        '"{x: 2, y: 2, theta: 0.2}"'
    ]],
    shell=True
)

相同的方法用于change_background_r和change_background_r_conditioned操作，这些操作会更改海龟背景的红色参数。不同之处在于，只有当提供的new_background_r参数等于200并且use_provided_red 启动参数设置为True时，才会执行change_background_r_conditioned操作。IfCondition内的求值是使用PythonExpression取代完成的。

change_background_r = ExecuteProcess(
    cmd=[[
        'ros2 param set ',
        turtlesim_ns,
        '/sim background_r ',
        '120'
    ]],
    shell=True
)
change_background_r_conditioned = ExecuteProcess(
    condition=IfCondition(
        PythonExpression([
            new_background_r,
            ' == 200',
            ' and ',
            use_provided_red
        ])
    ),
    cmd=[[
        'ros2 param set ',
        turtlesim_ns,
        '/sim background_r ',
        new_background_r
    ]],
    shell=True
)

21.3.3.4 编译功能包

回到工作空间目录，编译功能包：

colcon build

设置环境变量：

source install/setup.bash

21.3.4 运行示例

现在，可以启动example_main.launch.py文件了：

ros2 launch launch_tutorial example_main.launch.py

这将执行以下操作：

1. 以蓝色背景启动一个turtlesim节点。

2. 生成第二只海龟。

3. 将颜色更改为紫色。

4. 如果提供的background_r参数为200且use_provided_red参数为True，则在两秒后将颜色更改为粉色。

21.3.5 修改启动参数

如果要修改提供的启动参数，可以在example_main.launch的launch_arguments字典中更新它们，或者使用首选参数启动example_substitutions.launch.py。要查看可能提供给启动文件的参数，请运行以下命令：

ros2 launch launch_tutorial example_substitutions.launch.py --show-args

这将显示可能提供给启动文件的参数及其默认值。

Arguments (pass arguments as ':='):

    'turtlesim_ns':
        no description given
        (default: 'turtlesim1')

    'use_provided_red':
        no description given
        (default: 'False')

    'new_background_r':
        no description given
        (default: '200')

现在，可以将所需的参数传递到启动文件了，如下所示：

ros2 launch launch_tutorial example_substitutions.launch.py turtlesim_ns:='turtlesim3' use_provided_red:='True' new_background_r:=200

文档

launch文档提供了有关可用取代的详细信息：

https://github.com/ros2/launch/blob/humble/launch/doc/source/architecture.rst

21.3.6 总结

在本教程中，您学习了在启动文件中使用取代。您了解了使用取代创建可重用启动文件的可能性和能力。

21.4 使用事件处理器

21.4.1 背景

ROS2中的launch是一个执行和管理用户定义进程的系统。它负责监控其启动的进程的状态，以及报告和应对这些进程状态的变化。这些变化称为事件，可以通过向启动系统注册事件处理器来处理。事件处理器可以为特定事件注册，并且可以用于监视进程的状态。此外，它们可用于定义一组复杂的规则，这些规则可用于动态修改启动文件。

本教程将展示ROS2启动文件中事件处理程序的使用示例。

21.4.2 前提

本教程使用turtlesim功能包。本教程还假设您已经创建了一个名为launch_tutorial的ament_python编译类型功能包。

本教程扩展了使用取代教程中的代码。

21.4.3 使用事件处理器

21.4.3.1 事件处理器的示例启动文件

在launch_tutorial功能包的launch文件夹下创建一个名为example_event_handlers.launch.py的python文件。

from launch_ros.actions import Node

from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, EmitEvent, ExecuteProcess,
                            LogInfo, RegisterEventHandler, TimerAction)
from launch.conditions import IfCondition
from launch.event_handlers import (OnExecutionComplete, OnProcessExit,
                                OnProcessIO, OnProcessStart, OnShutdown)
from launch.events import Shutdown
from launch.substitutions import (EnvironmentVariable, FindExecutable,
                                LaunchConfiguration, LocalSubstitution,
                                PythonExpression)


def generate_launch_description():
    turtlesim_ns = LaunchConfiguration('turtlesim_ns')
    use_provided_red = LaunchConfiguration('use_provided_red')
    new_background_r = LaunchConfiguration('new_background_r')

    turtlesim_ns_launch_arg = DeclareLaunchArgument(
        'turtlesim_ns',
        default_value='turtlesim1'
    )
    use_provided_red_launch_arg = DeclareLaunchArgument(
        'use_provided_red',
        default_value='False'
    )
    new_background_r_launch_arg = DeclareLaunchArgument(
        'new_background_r',
        default_value='200'
    )

    turtlesim_node = Node(
        package='turtlesim',
        namespace=turtlesim_ns,
        executable='turtlesim_node',
        name='sim'
    )
    spawn_turtle = ExecuteProcess(
        cmd=[[
            FindExecutable(name='ros2'),
            ' service call ',
            turtlesim_ns,
            '/spawn ',
            'turtlesim/srv/Spawn ',
            '"{x: 2, y: 2, theta: 0.2}"'
        ]],
        shell=True
    )
    change_background_r = ExecuteProcess(
        cmd=[[
            FindExecutable(name='ros2'),
            ' param set ',
            turtlesim_ns,
            '/sim background_r ',
            '120'
        ]],
        shell=True
    )
    change_background_r_conditioned = ExecuteProcess(
        condition=IfCondition(
            PythonExpression([
                new_background_r,
                ' == 200',
                ' and ',
                use_provided_red
            ])
        ),
        cmd=[[
            FindExecutable(name='ros2'),
            ' param set ',
            turtlesim_ns,
            '/sim background_r ',
            new_background_r
        ]],
        shell=True
    )

    return LaunchDescription([
        turtlesim_ns_launch_arg,
        use_provided_red_launch_arg,
        new_background_r_launch_arg,
        turtlesim_node,
        RegisterEventHandler(
            OnProcessStart(
                target_action=turtlesim_node,
                on_start=[
                    LogInfo(msg='Turtlesim started, spawning turtle'),
                    spawn_turtle
                ]
            )
        ),
        RegisterEventHandler(
            OnProcessIO(
                target_action=spawn_turtle,
                on_stdout=lambda event: LogInfo(
                    msg='Spawn request says "{}"'.format(
                        event.text.decode().strip())
                )
            )
        ),
        RegisterEventHandler(
            OnExecutionComplete(
                target_action=spawn_turtle,
                on_completion=[
                    LogInfo(msg='Spawn finished'),
                    change_background_r,
                    TimerAction(
                        period=2.0,
                        actions=[change_background_r_conditioned],
                    )
                ]
            )
        ),
        RegisterEventHandler(
            OnProcessExit(
                target_action=turtlesim_node,
                on_exit=[
                    LogInfo(msg=(EnvironmentVariable(name='USER'),
                            ' closed the turtlesim window')),
                    EmitEvent(event=Shutdown(
                        reason='Window closed'))
                ]
            )
        ),
        RegisterEventHandler(
            OnShutdown(
                on_shutdown=[LogInfo(
                    msg=['Launch was asked to shutdown: ',
                        LocalSubstitution('event.reason')]
                )]
            )
        ),
    ])

启动描述中定义了OnProcessStart、OnProcessIO、OnExecutionComplete、OnProcessExit和OnShutdown事件的RegisterEventHandler操作。

OnProcessStart事件处理器用于注册在turtlesim节点启动时执行的回调函数。当turtlesim节点启动时，它会将消息记录到控制台并执行spawn_turtle动作。

RegisterEventHandler(
    OnProcessStart(
        target_action=turtlesim_node,
        on_start=[
            LogInfo(msg='Turtlesim started, spawning turtle'),
            spawn_turtle
        ]
    )
),

OnProcessIO事件处理器用于注册当spawn_turtle动作写入其标准输出时执行的回调函数。它记录派生请求的结果。

RegisterEventHandler(
    OnProcessIO(
        target_action=spawn_turtle,
        on_stdout=lambda event: LogInfo(
            msg='Spawn request says "{}"'.format(
                event.text.decode().strip())
        )
    )
),

OnExecutionComplete事件处理器用于注册在spawn_turtle动作完成时执行的回调函数。它将一条消息记录到控制台，并在spawn_turtle动作完成时执行change_background_r和change_background_r_conditioned动作。

RegisterEventHandler(
    OnExecutionComplete(
        target_action=spawn_turtle,
        on_completion=[
            LogInfo(msg='Spawn finished'),
            change_background_r,
            TimerAction(
                period=2.0,
                actions=[change_background_r_conditioned],
            )
        ]
    )
),

OnProcessExit事件处理器用于注册在turtlesim节点退出时执行的回调函数。它向控制台记录一条消息，并执行EmitEvent动作，以在turtlesim节点退出时发出Shutdown事件。这意味着启动进程将在turtlesim窗口关闭时关闭。

RegisterEventHandler(
    OnProcessExit(
        target_action=turtlesim_node,
        on_exit=[
            LogInfo(msg=(EnvironmentVariable(name='USER'),
                    ' closed the turtlesim window')),
            EmitEvent(event=Shutdown(
                reason='Window closed'))
        ]
    )
),

OnShutdown事件处理器用于注册一个回调函数，该函数在启动文件被要求关闭时执行。它会向控制台记录一条消息，说明为什么要求关闭启动文件。它会记录消息，并说明关闭的原因，例如关闭了turtlesim窗口或用户发出的ctrl+c信号。

RegisterEventHandler(
    OnShutdown(
        on_shutdown=[LogInfo(
            msg=['Launch was asked to shutdown: ',
                LocalSubstitution('event.reason')]
        )]
    )
),

21.4.4 编译功能包

回到工作空间目录，编译功能包：

colcon build

设置环境变量：

source install/setup.bash

21.4.5 运行示例

输入以下指令运行启动文件：

ros2 launch launch_tutorial example_event_handlers.launch.py turtlesim_ns:='turtlesim3' use_provided_red:='True' new_background_r:=200

这将执行以下操作： 1.以蓝色背景启动一个turtlesim节点。 2.生成第二只海龟。 3.将颜色更改为紫色。 4.如果提供的background_r参数为200且use_prided_red参数为True，则在两秒后将颜色更改为粉色。 5.当turtlesim窗口关闭时关闭启动文件。

此外，它将在以下情况下将消息记录到控制台： 1.turtlesim节点启动。 2.执行spawn动作。 3.执行change_background_r动作。 4.执行change_background_r_conditioned动作。 5.turtlesim节点退出。 6.启动进程被要求关闭。

21.4.6 文档

launch文档提供了有关可用事件处理器的详细信息：

https://github.com/ros2/launch/blob/humble/launch/doc/source/architecture.rst

21.4.7 总结

在本教程中，您了解了如何在启动文件中使用事件处理器。您了解了它们的语法和用法示例，以定义一组复杂的规则来动态修改启动文件。

21.5 管理大型项目

21.5.1 背景

本教程介绍了为大型项目编写启动文件的一些技巧。重点是如何构造启动文件，以便在不同的情况下尽可能地重用它们。此外，它还介绍了不同ROS2启动工具的使用示例，如参数、YAML文件、重映射、命名空间、默认参数和RViz配置。

21.5.2 前提

本教程使用turtlesim和turtle_tf2_py功能包。本教程还假设您已经创建了一个名为launch_tutorial的ament_python编译类型的新功能包。

21.5.3 介绍

机器人上的大型应用程序通常涉及多个互连节点，每个节点可以具有多个参数。在海龟模拟器中模拟多只海龟可以作为一个很好的例子。海龟模拟由多个海龟节点、全局配置以及TF广播和监听节点组成。在所有节点之间，存在大量影响这些节点的行为和外观的ROS参数。ROS2启动文件允许我们启动所有节点并在一个地方设置相应的参数。在教程结束时，您将编译在launch_tutorial功能包中的launch_turtlesim.launch.py启动文件。此启动文件将启动负责模拟海龟、启动TF广播器和收听器、加载参数、启动RViz配置等任务的节点。在本教程中，我们将介绍此启动文件和所有相关功能。

21.5.4 编写启动文件

21.5.4.1 顶层组织

编写启动文件的过程中的一个目标应该是使它们尽可能地可重用。这可以通过将相关节点和配置集群到单独的启动文件中来实现。然后，可以编写专用于特定配置的顶层启动文件。这将允许在完全相同的机器人之间移动，而完全不改变启动文件。即使是从一个真实的机器人到一个模拟的机器人这样的移动，也只需要几个改变就可以完成。

现在我们将讨论使这成为可能的顶层启动文件结构。首先，我们将创建一个启动文件，该文件将调用单独的启动文件。为此，让我们创建在launch_tutorial功能包launch目录下的launch_turtlesim.launch.py文件。

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
   turtlesim_world_1 = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('launch_tutorial'), 'launch'),
         '/turtlesim_world_1.launch.py'])
      )
   turtlesim_world_2 = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('launch_tutorial'), 'launch'),
         '/turtlesim_world_2.launch.py'])
      )
   broadcaster_listener_nodes = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('launch_tutorial'), 'launch'),
         '/broadcaster_listener.launch.py']),
      launch_arguments={'target_frame': 'carrot1'}.items(),
      )
   mimic_node = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('launch_tutorial'), 'launch'),
         '/mimic.launch.py'])
      )
   fixed_frame_node = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('launch_tutorial'), 'launch'),
         '/fixed_broadcaster.launch.py'])
      )
   rviz_node = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('launch_tutorial'), 'launch'),
         '/turtlesim_rviz.launch.py'])
      )

   return LaunchDescription([
      turtlesim_world_1,
      turtlesim_world_2,
      broadcaster_listener_nodes,
      mimic_node,
      fixed_frame_node,
      rviz_node
   ])

此启动文件包括一组其它启动文件。每一个这些包含的启动文件都包含节点、参数，可能还有嵌套的包含，它们属于系统的一部分。确切地说，我们启动了两个海龟模拟世界、TF广播器、TF监听器、模拟、固定坐标系广播器和RViz节点。

设计提示：顶层启动文件应该很短，包括与应用程序子组件相对应的其他文件，以及经常更改的参数。

稍后我们将看到，按照下面的方式编写启动文件可以很容易地交换系统的一部分。但是，有时由于性能和使用原因，某些节点或启动文件必须单独运行。

设计提示：在决定应用程序需要多少顶级启动文件时，要注意权衡。

21.5.4.2 参数

21.5.4.2.1 在启动文件设置参数

我们将首先编写一个启动文件，开始我们的第一次海龟模拟。首先，创建一个名为turtlesim_world_1.launch.py的新文件。

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, TextSubstitution

from launch_ros.actions import Node


def generate_launch_description():
   background_r_launch_arg = DeclareLaunchArgument(
      'background_r', default_value=TextSubstitution(text='0')
   )
   background_g_launch_arg = DeclareLaunchArgument(
      'background_g', default_value=TextSubstitution(text='84')
   )
   background_b_launch_arg = DeclareLaunchArgument(
      'background_b', default_value=TextSubstitution(text='122')
   )

   return LaunchDescription([
      background_r_launch_arg,
      background_g_launch_arg,
      background_b_launch_arg,
      Node(
         package='turtlesim',
         executable='turtlesim_node',
         name='sim',
         parameters=[{
            'background_r': LaunchConfiguration('background_r'),
            'background_g': LaunchConfiguration('background_g'),
            'background_b': LaunchConfiguration('background_b'),
         }]
      ),
   ])

此启动文件启动turtlesim_node节点，该节点启动turtlesim仿真，并定义仿真配置参数后传递给节点。

21.5.4.2 加载YAML文件的参数

在第二次启动中，我们将使用不同的配置开始第二次海龟模拟。现在创建一个turtlesim_world_2.launch.py文件。

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
   config = os.path.join(
      get_package_share_directory('launch_tutorial'),
      'config',
      'turtlesim.yaml'
      )

   return LaunchDescription([
      Node(
         package='turtlesim',
         executable='turtlesim_node',
         namespace='turtlesim2',
         name='sim',
         parameters=[config]
      )
   ])

此启动文件将使用直接从YAML配置文件加载的参数值启动相同的turtlesim_node。在YAML文件中定义参数可以方便地存储和加载变量。此外，可以从当前的ROS2参数列表轻松导出YAML文件。

现在让我们创建一个配置文件，位于功能包config文件夹中的turtlesim.yaml，该文件将由启动文件加载。

/turtlesim2/sim:
   ros__parameters:
      background_b: 255
      background_g: 86
      background_r: 150

如果我们现在运行turtlesim_world_2.launch.py启动文件，我们将使用预配置的背景色启动turtlesim_node。

21.5.4.3 在YAML文件中使用通配符

有时我们需要在多个节点中设置相同的参数。这些节点可以具有不同的命名空间或名称，但仍然具有相同的参数。定义单独的YAML文件来显式定义命名空间和节点名称是低效的。一种解决方案是使用通配符（作为文本值中未知字符的替代）将参数应用于多个不同的节点。

现在，让我们创建一个新的turtlesim_world_3.launch.py文件，类似于turtlesim_world_2.launch.py以包括另一个turtlesim_node节点。

...
Node(
   package='turtlesim',
   executable='turtlesim_node',
   namespace='turtlesim3',
   name='sim',
   parameters=[config]
)

然而，加载相同的YAML文件不会影响第三个turtlesim世界的外观。原因是其参数存储在另一个命名空间下，如下所示：

/turtlesim3/sim:
   background_b
   background_g
   background_r

因此，我们可以使用通配符语法，而不是为使用相同参数的相同节点创建新配置。/**将为每个节点分配相同的参数，尽管节点名称和命名空间不同。

我们现在更新turtlesim.yaml，以如下方式保存在/config文件夹中：

/**:
   ros__parameters:
      background_b: 255
      background_g: 86
      background_r: 150

现在包括turtlesim_world_3.launch.py启动描述到主启动文件中。在我们的启动描述中使用该配置文件将为turtlesim3/sim和turtlesim2/sim节点中的指定值分配background_b、background_g和background_r参数。

21.5.4.3 命名空间

正如您可能已经注意到的，我们已经在turtlesim_world_2.launch.py中定义了turlesim世界的命名空间。独有的命名空间允许系统启动两个类似的节点，而不会发生节点名称或话题名称冲突。

namespace='turtlesim2',

但是，如果启动文件包含大量节点，则为每个节点定义命名空间可能会变得乏味。为了解决这个问题，可以使用PushRosNamespace为每个启动文件描述定义全局命名空间。每个嵌套节点都将自动继承该命名空间。

为此，首先我们需要从turtlesim_world_2.launch.py中删除namespace='turtlesim2'行。之后我们需要更新launch_turtleism.launch.py以包含以下行：

from launch.actions import GroupAction
from launch_ros.actions import PushRosNamespace

   ...
   turtlesim_world_2 = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
         get_package_share_directory('launch_tutorial'), 'launch'),
         '/turtlesim_world_2.launch.py'])
      )
   turtlesim_world_2_with_namespace = GroupAction(
     actions=[
         PushRosNamespace('turtlesim2'),
         turtlesim_world_2,
      ]
   )

最后，我们在returnLaunchDescription语句中将turtlesim_world_2替换为turtlesim_world_2_with_namespace。因此，turtlesim_world_2.launch.py中启动描述的每个节点都将有一个turtlesim2命名空间。

21.5.4.4 复用节点

现在创建一个broadcaster_listener.launch.py文件。

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node


def generate_launch_description():
   return LaunchDescription([
      DeclareLaunchArgument(
         'target_frame', default_value='turtle1',
         description='Target frame name.'
      ),
      Node(
         package='turtle_tf2_py',
         executable='turtle_tf2_broadcaster',
         name='broadcaster1',
         parameters=[
            {'turtlename': 'turtle1'}
         ]
      ),
      Node(
         package='turtle_tf2_py',
         executable='turtle_tf2_broadcaster',
         name='broadcaster2',
         parameters=[
            {'turtlename': 'turtle2'}
         ]
      ),
      Node(
         package='turtle_tf2_py',
         executable='turtle_tf2_listener',
         name='listener',
         parameters=[
            {'target_frame': LaunchConfiguration('target_frame')}
         ]
      ),
   ])

在这个文件中，我们声明了target_frame启动参数，默认值为turtle1。默认值意味着启动文件可以接收一个参数以转发到其节点，或者如果没有提供该参数，它会将默认值传递给其节点。

然后，我们在启动期间两次启动turtle_tf2_broadcast节点，使用了不同的名称和参数。这允许我们复制相同的节点而不发生冲突。

我们还启动了一个turtle_tf2_listener节点，并设置了上面声明和获取的target_frame参数。

21.5.4.5 优先参数

回想一下，我们在顶层启动文件调用了broadcaster_listener.launch.py文件。除此之外，我们还传递了target_frame启动参数，如下所示：

broadcaster_listener_nodes = IncludeLaunchDescription(
   PythonLaunchDescriptionSource([os.path.join(
      get_package_share_directory('launch_tutorial'), 'launch'),
      '/broadcaster_listener.launch.py']),
   launch_arguments={'target_frame': 'carrot1'}.items(),
   )

此语法允许我们将默认目标更改为carrot1。如果您希望turtle2跟随turtle1而不是carrot1，只需删除定义launch_arguments的行。这将为target_frame指定默认值，即turtle1。

21.5.4.6 重新映射

现在创建一个mimic.launch.py文件。

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
   return LaunchDescription([
      Node(
         package='turtlesim',
         executable='mimic',
         name='mimic',
         remappings=[
            ('/input/pose', '/turtle2/pose'),
            ('/output/cmd_vel', '/turtlesim2/turtle1/cmd_vel'),
         ]
      )
   ])

该启动文件将启动模拟节点，该节点将向一个模拟对象发出命令，使其跟随另一个模拟节点。该节点被设计为接收话题/input/pose。在本例中，我们希望从/turtle2/pose话题重新映射目标姿态。最后，我们将/output/cmd_vel话题重新映射到/turtlesim2/turtle1/cmd_vel。这样一来，turtlesim2模拟窗口中的turtle1将跟随我们最初的turtlesim模拟运动。

21.5.4.7 配置文件

让我们创建一个turtlesim_rviz.launch.py文件。

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
   rviz_config = os.path.join(
      get_package_share_directory('turtle_tf2_py'),
      'rviz',
      'turtle_rviz.rviz'
      )

   return LaunchDescription([
      Node(
         package='rviz2',
         executable='rviz2',
         name='rviz2',
         arguments=['-d', rviz_config]
      )
   ])

此启动文件将使用turtle_tf2_py功能包中定义的配置文件启动RViz。此RViz配置将设置世界坐标系，启用TF可视化，并使用自顶向下的视图启动RViz。

21.5.4.8 环境变量

现在，让我们创建最后一个名为fixed_broadcaster.launch.py的启动文件。

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import EnvironmentVariable, LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
   return LaunchDescription([
      DeclareLaunchArgument(
            'node_prefix',
            default_value=[EnvironmentVariable('USER'), '_'],
            description='prefix for node name'
      ),
      Node(
            package='turtle_tf2_py',
            executable='fixed_frame_tf2_broadcaster',
            name=[LaunchConfiguration('node_prefix'), 'fixed_broadcaster'],
      ),
   ])

此启动文件显示了在启动文件中调用环境变量的方式。环境变量可用于定义或推送命名空间，以区分不同计算机或机器人上的节点。

21.5.5 运行启动文件

21.5.5.1 更新setup.py

打开setup.py并添加以下行，以便安装launch文件夹中的启动文件和config文件夹中的配置文件。data_files字段现在应该如下所示：

data_files=[
      ...
      (os.path.join('share', package_name, 'launch'),
         glob(os.path.join('launch', '*.launch.py'))),
      (os.path.join('share', package_name, 'config'),
         glob(os.path.join('config', '*.yaml'))),
   ],

21.5.5.2 编译与运行

要最终看到代码的效果，请使用以下命令编译功能包并启动顶级启动文件：

ros2 launch launch_tutorial launch_turtlesim.launch.py

现在，您将看到两个模拟窗口。第一个窗口有两只海龟，第二个窗口有一只海龟。在第一个模拟中，窗口左下角的地方繁殖了turtle2。它的目标是到达在x轴上相对于turtle1坐标5米远的carrot1坐标。

第二个模拟中的turtlesim2/turtle1旨在模仿turtle2的行为。

如果要控制turtle1，请运行控制节点。

ros2 run turtlesim turtle_teleop_key

您将看到以下类似结果：

除此之外，RViz应该已经启动。它将显示相对于世界坐标系的所有海龟坐标，其原点位于左下角。

21.5.6 总结

在本教程中，您了解了使用ROS2启动文件管理大型项目的各种技巧和实践。

22 机器人坐标系管理工具tf2

22.1 tf2简介

22.1.1 安装示例

让我们从安装示例功能包及其依赖项开始。

sudo apt-get install ros-humble-turtle-tf2-py ros-humble-tf2-tools ros-humble-tf-transformations

22.1.2 运行示例

现在我们已经安装了示例功能包，让我们运行示例。 首先，打开一个新终端并设置环境变量，以便命令正常工作。 然后运行以下命令：

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

你会看到两只海龟。

在第二个终端窗口中键入以下命令：

ros2 run turtlesim turtle_teleop_key

选择第二个终端窗口，以便捕获您的击键以控制海龟。一旦模拟启动，您可以使用键盘箭头键控制中间的那只海龟。你可以看到，另一只乌龟不断地移动，跟随你正在控制的海龟。

本演示使用tf2库创建三个坐标系：世界坐标系、turtle1坐标系和turtle2坐标系。本教程使用tf2广播器发布海龟坐标，使用tf2收听器计算海龟坐标之间的差异，并移动另一只海龟跟随控制的海龟。

22.1.3 tf2的工具

现在让我们来看看如何使用tf2创建的这个示例。我们可以使用tf2_tools来看看tf2在幕后做了什么。

22.1.3.1 view_frames

view_frames创建tf2在ROS上广播的坐标系的示意图。

ros2 run tf2_tools view_frames

你将看到：

Listening to tf data during 5 seconds...
Generating graph in frames.pdf file...

这里，tf2收听者正在收听通过ROS广播的坐标系，并绘制坐标系之间如何关联的树形图。要查看树形图，请用pdf查看器打开生成的frames.pdf。

在这里，我们可以看到tf2广播的三个坐标系：world、turtle1和turtle2。world坐标系是turtle1和turtle2的父坐标系。view_frames还报告何时接收到最旧和最新的坐标变换，以及tf2坐标发布到tf2的速度。

22.1.3.2 tf2_echo

tf2_echo报告通过ROS广播的任意两个坐标系之间的变换。

用法：

ros2 run tf2_ros tf2_echo [reference_frame] [target_frame]

让我们来看看海龟2坐标系相对于海龟1坐标系的变换，这相当于：

ros2 run tf2_ros tf2_echo turtle2 turtle1

当tf2_echo侦听器接收通过ROS2广播的坐标系时，您将看到显示的变换。

At time 1622031731.625364060
- Translation: [2.796, 1.039, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.202, 0.979]
At time 1622031732.614745114
- Translation: [1.608, 0.250, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.032, 0.999]

当控制海龟移动时，你会看到两只海龟相对移动时的变化。

22.1.4 rvzi与tf2

rviz是一个用于检查tf2坐标系的可视化工具。让我们用rviz看看我们的海龟坐标系。让我们使用turtle_rviz.rviz配置文件启动rvzi：

ros2 run rviz2 rviz2 -d $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz

在侧栏中，您将看到tf2广播的坐标系。当您驾驶海龟时，您将看见坐标系在rviz中移动。

22.2 编写静态广播

22.2.1 背景

发布静态变换有助于定义机器人基座与其传感器或非移动部件之间的关系。例如，在激光扫描仪中心的一个框架中进行激光扫描测量是最容易理解的。

这是一个涵盖静态变换基础知识的独立教程，由两部分组成。在第一部分中，我们将编写代码将静态转换发布到tf2。在第二部分中，将解释如何在tf2_ros中使用命令行static_transform_publisher。

在接下来的两个教程中，我们将编写代码来重现tf2简介教程中的示例。之后，以下教程将重点介绍如何使用更高级的tf2功能。

22.2.2 前提

在之前的教程，你应该学习了如何创建工作空间与功能包。

22.2.3 示例

22.2.3.1 创建功能包

首先，我们将创建一个用于本教程和以下教程的功能包。名为learning_tf2_py的功能包将依赖geometry_msgs、python3-numpy、rcppy、tf2_ros_py和turtlesim。

打开一个新终端并设置环境变量，以便ros2命令能够正常工作。进入到工作区的src文件夹并创建新功能包：

ros2 pkg create --build-type ament_python learning_tf2_py

终端将返回一条消息，以验证功能包learning_tf2_py及其所有必要文件和文件夹的创建。

22.2.3.2 编写静态广播节点

让我们先创建源文件。在src/learning_tf2_py/learning_tf2_py目录中，输入以下命令下载示例静态广播器代码：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/static_turtle_tf2_broadcaster.py

使用文本编辑器打开文件。

import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster


def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return q


class StaticFramePublisher(Node):
    """
    Broadcast transforms that never change.

    This example publishes transforms from `world` to a static turtle frame.
    The transforms are only published once at startup, and are constant for all
    time.
    """

    def __init__(self, transformation):
        super().__init__('static_turtle_tf2_broadcaster')

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        self.make_transforms(transformation)

    def make_transforms(self, transformation):
        t = TransformStamped()

        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = transformation[1]

        t.transform.translation.x = float(transformation[2])
        t.transform.translation.y = float(transformation[3])
        t.transform.translation.z = float(transformation[4])
        quat = quaternion_from_euler(
            float(transformation[5]), float(transformation[6]), float(transformation[7]))
        t.transform.rotation.x = quat[0]
        t.transform.rotation.y = quat[1]
        t.transform.rotation.z = quat[2]
        t.transform.rotation.w = quat[3]

        self.tf_static_broadcaster.sendTransform(t)


def main():
    logger = rclpy.logging.get_logger('logger')

    # obtain parameters from command line arguments
    if len(sys.argv) != 8:
        logger.info('Invalid number of parameters. Usage: \n'
                    '$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster'
                    'child_frame_name x y z roll pitch yaw')
        sys.exit(1)

    if sys.argv[1] == 'world':
        logger.info('Your static turtle name cannot be "world"')
        sys.exit(2)

    # pass parameters and initialize node
    rclpy.init()
    node = StaticFramePublisher(sys.argv)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

检查代码

现在，让我们看看将静态海龟位姿发布到tf2的相关代码。第一行导入所需的功能包。首先，我们从geometry_msgs导入TransformStamped，它为我们将发布到转换树的消息提供了模板。

from geometry_msgs.msg import TransformStamped

然后，导入rclpy，以便可以使用它的Node类。

import rclpy
from rclpy.node import Node

tf2_ros功能包提供了StaticTransformBroadcaster，使静态转换的发布变得容易。要使用StaticTransformBroadcaster，我们需要从tf2_ros模块导入它。

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

StaticFramePublisher类构造函数使用名称static_turtle_tf2_broadcast初始化节点。然后，创建StaticTransformBroadcaster，它将在启动时发送一个静态转换。

self.tf_static_broadcaster = StaticTransformBroadcaster(self)
self.make_transforms(transformation)

在这里，我们创建一个TransformStamped对象，它将是填充后发送的消息。在传递实际的转换值之前，我们需要为其提供适当的元数据。

1. 我们需要给正在发布的转换一个时间戳，我们只需要用当前时间self.get_clock().now()标记它。

2. 然后，我们需要设置正在创建的父坐标系的名称，在本例中是world。

3. 最后，我们需要设置正在创建的子坐标系的名称。

t = TransformStamped()

t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = transformation[1]

在这里，我们填充海龟的6D姿势（平移和旋转）。

t.transform.translation.x = float(transformation[2])
t.transform.translation.y = float(transformation[3])
t.transform.translation.z = float(transformation[4])
quat = quaternion_from_euler(
    float(transformation[5]), float(transformation[6]), float(transformation[7]))
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]

最后，我们使用sendTransform()函数广播静态转换。

self.tf_static_broadcaster.sendTransform(t)

添加依赖

进入src/learning_tf2_py目录，其中包含了已创建好的setup.py，setup.cfg，package.xml文件。

使用文本编辑器打开package.xml。

如创建功能包教程中所述，确保填写、和标签：

Learning tf2 with rclpy
Your Name
Apache License 2.0

在上面的行之后，添加与节点的import语句相对应的以下依赖项：

geometry_msgs
python3-numpy
rclpy
tf2_ros_py
turtlesim

这在执行代码时声明了所需的geometry_msgs、python3-numpy、rcppy、tf2_ros_py和turtlesim依赖项。确保保存文件。

添加入口点

要允许ros2 run命令运行节点，必须向setup.py添加入口点（位于src/learning_tf2_py目录中）。

在“console_scripts”括号之间添加以下行：

'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

编译

最好在工作空间的根目录中运行rosdep，以在编译之前检查缺少的依赖项：

rosdep install -i --from-path src --rosdistro humble -y

仍然在工作空间的根目录中，编译功能包：

colcon build --packages-select learning_tf2_py

打开一个新终端，进入到工作空间的根目录，设置环境变量：

. install/setup.bash

运行

现在运行static_turtle_tf2_broadcaster节点：

ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

这设置了一个海龟位姿广播，让mystaticturtle浮在离地面1米的地方。

现在，我们可以通过回显tf_static话题来检查静态转换是否已发布：

ros2 topic echo /tf_static

如果一切顺利，您应该看到一个静态转换。

transforms:
- header:
   stamp:
      sec: 1622908754
      nanosec: 208515730
   frame_id: world
child_frame_id: mystaticturtle
transform:
   translation:
      x: 0.0
      y: 0.0
      z: 1.0
   rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

22.2.3.3 发布静态变换的正确方法

本教程旨在展示如何使用StaticTransformBroadcaster发布静态转换。在真正的开发过程中，您不必自己编写这些代码，应该使用专用的tf2_ros工具来完成。tf2_ros提供了一个名为statictransformpublisher的可执行文件，它可以用作命令行工具或添加到启动文件中的节点。

使用以米为单位的x/y/z偏移和以弧度为单位的roll/pitch/yaw，将静态坐标变换发布到tf2。在我们的例子中，roll/pitch/yaw分别指围绕x/y/z轴的旋转。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

使用米和四元数的x/y/z偏移将静态坐标变换发布到tf2。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

statictransformpublisher被设计为一个命令行工具，既可以手动使用，也可以在启动文件中用于设置静态转换。例如：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments = ['--x', '0', '--y', '0', '--z', '1', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']
        ),
    ])

注意，除了--frame id和--child frame id之外的所有参数都是可选的；如果未指定特定选项，则将假定身份。

22.2.4 总结

在本教程中，您学习了静态变换如何用于定义坐标系之间的静态关系，例如mystaticturtle与world坐标系之间的关系。此外，您还了解了静态变换如何通过将数据关联到公共坐标系来帮助理解传感器数据，例如来自激光扫描仪的数据。最后，您编写了自己的节点来将静态转换发布到tf2，并学习了如何使用static_transform_publisher和启动文件发布所需的静态转换。

22.3 编写广播

22.4 背景

在接下来的两个教程中，我们将编写代码来重现tf2简介教程中的示例。之后，以下教程将重点介绍如何使用更高级的tf2特性，包括在转换查找和时间旅行中使用超时。

22.5 前提

本教程假设您对ROS 2有一定的了解，并且您已经完成了tf2简介教程。在以前的教程中，您学习了如何创建工作空间和创建功能包。您还创建了learning_tf2_py功能包，我们将从中继续工作。

22.6 示例

22.6.1 编写广播节点

让我们先创建源文件。转到我们在上一教程中创建的learning_tf2_py功能包。在src/learning_tf2_py/learning_tf2_py目录中，输入以下命令下载示例广播器代码：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

使用文本编辑器打开文件。

import math

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster

from turtlesim.msg import Pose


def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return q


class FramePublisher(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')

        # Declare and acquire `turtlename` parameter
        self.turtlename = self.declare_parameter(
          'turtlename', 'turtle').get_parameter_value().string_value

        # Initialize the transform broadcaster
        self.tf_broadcaster = TransformBroadcaster(self)

        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.handle_turtle_pose,
            1)
        self.subscription  # prevent unused variable warning

    def handle_turtle_pose(self, msg):
        t = TransformStamped()

        # Read message content and assign it to
        # corresponding tf variables
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = self.turtlename

        # Turtle only exists in 2D, thus we get x and y translation
        # coordinates from the message and set the z coordinate to 0
        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        # For the same reason, turtle can only rotate around one axis
        # and this why we set rotation in x and y to 0 and obtain
        # rotation in z axis from the message
        q = quaternion_from_euler(0, 0, msg.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        # Send the transformation
        self.tf_broadcaster.sendTransform(t)


def main():
    rclpy.init()
    node = FramePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

22.6.2 检查代码

现在，让我们来看看将海龟位姿发布到tf2的相关代码。首先，我们定义并获取一个参数turtlename，该参数指定海龟名称，例如turtle1或turtle2。

self.turtlename = self.declare_parameter(
  'turtlename', 'turtle').get_parameter_value().string_value

然后，节点订阅话题 turtleX/pose，并对每个传入的消息运行函数handle_turtle_pose。

self .subscription = self.create_subscription(
    Pose,
    f'/{self.turtlename}/pose',
    self.handle_turtle_pose,
    1)

现在，我们创建一个TransformStamped对象，并为其提供适当的元数据。

1. 我们需要给正在发布的转换一个时间戳，我们只需要用当前时间self.get_clock().now()标记它。这将返回节点使用的当前时间。

2. 然后，我们需要设置正在创建的父坐标系的名称，在本例中是world。

3. 最后，我们需要设置正在创建的子坐标系的名称。

海龟位姿消息的处理程序函数广播海龟的平移和旋转，并将其发布为从world坐标系到turtleX坐标系的转换。

t = TransformStamped()

# Read message content and assign it to
# corresponding tf variables
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = self.turtlename

在这里，我们将3D海龟位姿的信息复制到3D变换中。

# Turtle only exists in 2D, thus we get x and y translation
# coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg.x
t.transform.translation.y = msg.y
t.transform.translation.z = 0.0

# For the same reason, turtle can only rotate around one axis
# and this why we set rotation in x and y to 0 and obtain
# rotation in z axis from the message
q = quaternion_from_euler(0, 0, msg.theta)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]

最后，我们将构建的转换传递给TransformBroadcaster的sendTransform方法，该方法将负责广播。

# Send the transformation
self.tf_broadcaster.sendTransform(t)

您还可以通过实例化tf2_ros.StaticTransformBroadcaster而不是tf2_ros.TransformBroadcaster来发布具有相同模式的静态转换。静态转换将发布在/tf_static话题上，仅在需要时发送，而不是定期发送。

22.6.3 添加入口点

要允许ros2 run命令运行节点，必须向setup.py添加入口点（位于src/learning_tf2_py目录中）。

在“console_scripts”括号之间添加以下行：

'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

22.6.4 编写启动文件

现在为这个示例创建一个启动文件。使用文本编辑器，在启动文件夹中创建一个名为turtle_tf2_demo.launch.py的新文件，并添加以下行：

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

22.6.5 添加依赖

进入src/learning_tf2_py目录，其中包含了已创建好的setup.py，setup.cfg，package.xml文件。

使用文本编辑器打开package.xml。添加与启动文件的导入语句相对应的以下依赖项：

launch
launch_ros

这将在执行代码时声明额外所需的launch和launch_ros依赖项。确保保存文件。

22.6.6 更新setup.py

重新打开setup.py并添加该行，以便安装launch/文件夹中的启动文件。data_files字段现在应该如下所示：

data_files=[
    ...
    (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
],

同时在文件顶部添加适当的导入语句：

import os
from glob import glob

22.6.7 编译

在工作空间的根目录中运行rosdep以检查缺少的依赖项。

rosdep install -i --from-path src --rosdistro humble -y

仍然在工作空间的根目录中，编译您的功能包：

colcon build --packages-select learning_tf2_py

打开一个新终端，进入到工作空间的根目录，并设置环境变量：

. install/setup.bash

22.6.8 运行

现在运行启动文件，启动turtlesim模拟节点和turtle_tf2_broadcast节点：

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

在第二个终端窗口中，输入以下命令：

ros2 run turtlesim turtle_teleop_key

现在，您将看到，模拟已经从一只您可以控制的海龟开始。

现在，使用tf2_echo工具检查海龟位姿是否真的在向tf2广播：

ros2 run tf2_ros tf2_echo world turtle1

这应该是第一只乌龟的位姿。使用箭头键（确保您的turtle_teleop_key终端窗口处于活动状态，而不是模拟器窗口）控制海龟。在控制台输出中，您将看到以下类似内容：

At time 1625137663.912474878
- Translation: [5.276, 7.930, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137664.950813527
- Translation: [3.750, 6.563, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137665.906280726
- Translation: [2.320, 5.282, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137666.850775673
- Translation: [2.153, 5.133, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.365, 0.931]

如果您运行tf2_echo进行world和turtle2之间的变换，您应该不会看到变换，因为第二只海龟还没有出现。然而，当我们在下一个教程中添加第二只海龟时，turtle2的位姿将广播给tf2。

22.7 总结

在本教程中，您学习了如何向tf2广播机器人的位姿（海龟的位置和方向）以及如何使用tf2_echo工具。要实际使用向tf2广播的转换，您应该转到下一个关于创建tf2监听器的教程。

22.4 编写监听器

22.4.1 背景

在之前的教程中，我们创建了一个tf2广播器来向tf2发布海龟的位姿。

在本教程中，我们将创建一个tf2监听器来开始使用tf2。

22.4.2 前提

本教程假设您已完成tf2广播器教程（Python）。在上一个教程中，我们创建了一个learning_tf2_py功能包，我们将从中继续学习。

22.4.3 示例

22.4.3.1 编写监听节点

让我们先创建源文件。转到我们在上一教程中创建的learning_tf2_py功能包。在src/learning_tf2_py/learning_tf2_py目录中，输入以下命令下载示例监听器代码：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py

使用文本编辑器打开文件。

import math

from geometry_msgs.msg import Twist

import rclpy
from rclpy.node import Node

from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

from turtlesim.srv import Spawn


class FrameListener(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_listener')

        # Declare and acquire `target_frame` parameter
        self.target_frame = self.declare_parameter(
          'target_frame', 'turtle1').get_parameter_value().string_value

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # Create a client to spawn a turtle
        self.spawner = self.create_client(Spawn, 'spawn')
        # Boolean values to store the information
        # if the service for spawning turtle is available
        self.turtle_spawning_service_ready = False
        # if the turtle was successfully spawned
        self.turtle_spawned = False

        # Create turtle2 velocity publisher
        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)

        # Call on_timer function every second
        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        # Store frame names in variables that will be used to
        # compute transformations
        from_frame_rel = self.target_frame
        to_frame_rel = 'turtle2'

        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                # Look up for the transformation between target_frame and turtle2 frames
                # and send velocity commands for turtle2 to reach target_frame
                try:
                    t = self.tf_buffer.lookup_transform(
                        to_frame_rel,
                        from_frame_rel,
                        rclpy.time.Time())
                except TransformException as ex:
                    self.get_logger().info(
                        f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
                    return

                msg = Twist()
                scale_rotation_rate = 1.0
                msg.angular.z = scale_rotation_rate * math.atan2(
                    t.transform.translation.y,
                    t.transform.translation.x)

                scale_forward_speed = 0.5
                msg.linear.x = scale_forward_speed * math.sqrt(
                    t.transform.translation.x ** 2 +
                    t.transform.translation.y ** 2)

                self.publisher.publish(msg)
            else:
                if self.result.done():
                    self.get_logger().info(
                        f'Successfully spawned {self.result.result().name}')
                    self.turtle_spawned = True
                else:
                    self.get_logger().info('Spawn is not finished')
        else:
            if self.spawner.service_is_ready():
                # Initialize request with turtle name and coordinates
                # Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
                request = Spawn.Request()
                request.name = 'turtle2'
                request.x = float(4)
                request.y = float(2)
                request.theta = float(0)
                # Call request
                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                # Check if the service is ready
                self.get_logger().info('Service is not ready')


def main():
    rclpy.init()
    node = FrameListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

22.4.3.2 检查代码

要了解产卵龟背后的服务是如何工作的，请参考编写一个简单的服务和客户端（Python）教程。

现在，让我们来看看与访问坐标系转换的相关代码。tf2_ros包提供了TransformListener的实现，以帮助简化接收转换的任务。

from tf2_ros.transform_listener import TransformListener

在这里，我们创建一个TransformListener对象。一旦创建了监听器，它就开始通过网络接收tf2转换，并将它们缓冲长达10秒。

self.tf_listener = TransformListener(self.tf_buffer, self)

最后，我们向监听器查询特定的转换。我们使用以下参数调用lookup_transform方法：

1. 目标坐标系

2. 源坐标系

3. 我们想要转变的时间

提供rcpy.time.Time()将为我们提供最新的可用转换。所有这些都封装在try except代码块中，以处理可能的异常。

t = self.tf_buffer.lookup_transform(
    to_frame_rel,
    from_frame_rel,
    rclpy.time.Time())

22.4.3.3 添加入口点

要允许ros2 run命令运行节点，必须将入口点添加到setup.py（位于src/learning_tf2_py目录中）。

'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',

22.4.3.4 更新启动文件

使用文本编辑器打开名为turtle_tf2_demo.launch.py的启动文件，在启动描述中添加两个新节点，添加一个启动参数，然后添加导入。生成的文件应如下所示：

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
        DeclareLaunchArgument(
            'target_frame', default_value='turtle1',
            description='Target frame name.'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster2',
            parameters=[
                {'turtlename': 'turtle2'}
            ]
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_listener',
            name='listener',
            parameters=[
                {'target_frame': LaunchConfiguration('target_frame')}
            ]
        ),
    ])

这将声明一个target_frame启动参数，为我们将生成的第二只海龟启动广播器，并启动订阅这些转换的监听器。

22.4.3.5 编译

在工作空间的根目录中运行rosdep以检查缺少的依赖项。

rosdep install -i --from-path src --rosdistro humble -y

仍然在工作空间的根目录中，编译您的功能包：

colcon build --packages-select learning_tf2_py

打开一个新终端，进入到工作空间的根目录，并设置环境变量：

. install/setup.bash

22.4.3.6 运行

现在，您可以开始完整的海龟示例了：

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

你应该看到有两只海龟的模拟窗口。在第二个终端窗口中，键入以下命令：

ros2 run turtlesim turtle_teleop_key

若要查看情况是否正常，只需使用箭头键（确保终端窗口处于活动状态，而不是模拟器窗口）驾驶第一只海龟，您将看到第二只乌龟跟随第一只海龟！

22.4.4 总结

在本教程中，您学习了如何使用tf2访问坐标变换。您也已经完成编写了在tf2简介教程中首次尝试的海龟示例。

22.5 添加坐标系

22.5.1 背景

在之前的教程中，我们通过编写tf2广播器和tf2监听器重新创建了海龟演示。本教程将教您如何向变换树中添加额外的固定坐标系和动态坐标系。事实上，在tf2中添加坐标系与创建tf2广播非常相似，但本示例将向您展示tf2的一些附加功能。

对于许多与转换相关的任务，在局部坐标系内思考更容易。例如，在激光扫描仪中心的一个框架中进行激光扫描测量是最容易理解的。tf2允许您为系统中的每个传感器、链接或关节定义局部坐标系。当从一个坐标系转换到另一个坐标系时，tf2将处理所有引入的隐藏中间坐标系转换。

22.5.2 tf2 tree

tf2建立了坐标系的树结构，因此不允许在坐标系结构中形成闭环。这意味着一个坐标系只有一个父坐标系，但可以有多个子坐标系。目前，我们的tf2树包含三个坐标系：world、turtle1和turtle2。这两个turtlez坐标系是world坐标系的子坐标系。如果我们想向tf2添加一个新的坐标系，那么三个现有坐标系中的一个必须是父坐标系，而新的坐标系将成为其子坐标系。

22.5.3 示例

22.5.3.1 编写固定坐标系广播器

在我们的海龟示例中，我们将添加一个新的坐标系carrot1，它将是海龟的子坐标系。这个坐标系将作为第二只海龟的目标。

让我们先创建源文件。转到我们在先前教程中创建的learning_tf2_py功能包。输入以下命令下载固定坐标系广播器代码：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/fixed_frame_tf2_broadcaster.py

现在打开fixed_frame_tf2_broadcaster.py文件。

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster


class FixedFrameBroadcaster(Node):

   def __init__(self):
       super().__init__('fixed_frame_tf2_broadcaster')
       self.tf_broadcaster = TransformBroadcaster(self)
       self.timer = self.create_timer(0.1, self.broadcast_timer_callback)

   def broadcast_timer_callback(self):
       t = TransformStamped()

       t.header.stamp = self.get_clock().now().to_msg()
       t.header.frame_id = 'turtle1'
       t.child_frame_id = 'carrot1'
       t.transform.translation.x = 0.0
       t.transform.translation.y = 2.0
       t.transform.translation.z = 0.0
       t.transform.rotation.x = 0.0
       t.transform.rotation.y = 0.0
       t.transform.rotation.z = 0.0
       t.transform.rotation.w = 1.0

       self.tf_broadcaster.sendTransform(t)


def main():
    rclpy.init()
    node = FixedFrameBroadcaster()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

代码与tf2广播器教程示例非常相似，唯一的区别是此处的转换不会随时间而改变。

22.5.3.1.1 检查代码

让我们看看这段代码中的关键行。在这里，我们创建了一个新的变换，从父坐标系turtle1到新的子坐标系carrot1。carrot1坐标系相对于turtle1坐标系在y轴上偏移了2米。

t = TransformStamped()

t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'turtle1'
t.child_frame_id = 'carrot1'
t.transform.translation.x = 0.0
t.transform.translation.y = 2.0
t.transform.translation.z = 0.0

22.5.3.1.2 添加入口点

要允许ros2 run命令运行节点，必须将入口点添加到setup.py（位于src/learning_tf2_py目录中）。

'fixed_frame_tf2_broadcaster = learning_tf2_py.fixed_frame_tf2_broadcaster:main',

22.5.3.1.3 编写启动文件

现在，让我们为这个示例创建一个启动文件。使用文本编辑器，创建一个名为launch/turtle_tf2_fixed_frame_demo.launch.py的新文件，并添加以下行：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_py'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_py',
            executable='fixed_frame_tf2_broadcaster',
            name='fixed_broadcaster',
        ),
    ])

此启动文件导入所需的功能包，然后创建一个demo_nodes变量，该变量将存储我们在上一教程的启动文件中创建的节点。

代码的最后一部分将使用fixed_frame_tf2_broadcast节点将我们的固定carrot1坐标系添加到turtlesim世界。

Node(
    package='learning_tf2_py',
    executable='fixed_frame_tf2_broadcaster',
    name='fixed_broadcaster',
),

22.5.3.1.4 编译

在工作空间的根目录中运行rosdep以检查缺少的依赖项。

rosdep install -i --from-path src --rosdistro humble -y

仍然在工作空间的根目录中，编译您的功能包：

colcon build --packages-select learning_tf2_py

打开一个新终端，进入到工作空间的根目录，并设置环境变量：

. install/setup.bash

22.5.3.1.5 运行

现在，您可以运行启动文件了：

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

您应该注意到，新的carrot1坐标系出现在转换树中。

如果你驾驶第一只乌龟四处走动，你应该注意到，尽管我们添加了一个新的坐标系，但行为与上一教程没有改变。这是因为添加额外的坐标系不会影响其他坐标系，而我们的监听器仍在使用先前定义的坐标系。

所以，如果我们希望第二只乌龟跟随carrot而不是第一只海龟，我们需要更改target_frame的值。这可以通过两种方式实现。一种方法是直接从控制台将target_frame参数传递给启动文件：

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1

第二种方法是更新启动文件。为此，请打开turtle_tf2_fixed_frame_demo.launch.py文件，并通过launch_arguments参数添加“target_frame”：“carrot1”参数。

def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        ...,
        launch_arguments={'target_frame': 'carrot1'}.items(),
        )

现在只需重新编译功能包，重新启动turtle_tf2_fixed_frame_demo.launch.py，您将看到第二只乌龟跟随carrot，而不是第一只乌龟！

22.5.3.2 编写动态坐标系广播器

我们在本教程中发布的额外坐标系是一个固定坐标系，相对于父坐标系不会随时间变化。但是，如果您想发布移动坐标系，可以对广播机构进行编码，以随时间改变坐标系。让我们更改carrot1坐标系，使其随时间相对于turtle1坐标系发生变化。现在输入以下命令下载动态坐标系广播器代码：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/dynamic_frame_tf2_broadcaster.py

现在打开dynamic_frame_tf2_broadcaster.py文件：

import math

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster


class DynamicFrameBroadcaster(Node):

    def __init__(self):
        super().__init__('dynamic_frame_tf2_broadcaster')
        self.tf_broadcaster = TransformBroadcaster(self)
        self.timer = self.create_timer(0.1, self.broadcast_timer_callback)

    def broadcast_timer_callback(self):
        seconds, _ = self.get_clock().now().seconds_nanoseconds()
        x = seconds * math.pi

        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'turtle1'
        t.child_frame_id = 'carrot1'
        t.transform.translation.x = 10 * math.sin(x)
        t.transform.translation.y = 10 * math.cos(x)
        t.transform.translation.z = 0.0
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0

        self.tf_broadcaster.sendTransform(t)


def main():
    rclpy.init()
    node = DynamicFrameBroadcaster()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

22.5.3.2.1 检查代码

我们在当前时间使用sin（）和cos（）函数，而不是固定定义x和y偏移量，以便carrot1的偏移量可以不断变化。

seconds, _ = self.get_clock().now().seconds_nanoseconds()
x = seconds * math.pi
...
t.transform.translation.x = 10 * math.sin(x)
t.transform.translation.y = 10 * math.cos(x)

22.5.3.2.2 添加入口点

要允许ros2 run命令运行节点，必须将入口点添加到setup.py（位于src/learning_tf2_py目录中）。

'dynamic_frame_tf2_broadcaster = learning_tf2_py.dynamic_frame_tf2_broadcaster:main',

22.5.3.2.3 编写启动文件

要测试此代码，请创建一个新的启动文件launch/turtle_tf2_dynamic_frame_demo.launch.py。复制并粘贴以下代码：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_py'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
       launch_arguments={'target_frame': 'carrot1'}.items(),
       )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_py',
            executable='dynamic_frame_tf2_broadcaster',
            name='dynamic_broadcaster',
        ),
    ])

22.5.3.2.4 编译

在工作空间的根目录中运行rosdep以检查缺少的依赖项。

rosdep install -i --from-path src --rosdistro humble -y

仍然在工作空间的根目录中，编译您的功能包：

colcon build --packages-select learning_tf2_py

打开一个新终端，进入到工作空间的根目录，并设置环境变量：

. install/setup.bash

22.5.3.2.5 运行

现在，您可以运行启动文件了：

ros2 launch learning_tf2_py turtle_tf2_dynamic_frame_demo.launch.py

你应该看到，第二只乌龟正在跟随 carrot不断变化位置。

22.5.4 总结

在本教程中，您了解了tf2转换树其结构及特性。您还了解到，在局部坐标系内进行思考是最简单的，并学会了为该局部坐标系添加额外的固定坐标系和动态坐标系。

22.6 使用时间

22.6.1 背景

在之前的教程中，我们通过编写tf2广播器和tf2监听器重新创建了海龟示例。我们还学习了如何向转换树添加新坐标系。现在我们将了解更多关于timeout参数的信息，该参数使lookup_transform在抛出异常之前在指定的持续时间内等待指定的转换。该工具可用于监听以不同速率发布的转换或具有不可靠网络和不可忽略延迟的传入源。本教程将教您如何使用lookup_transform函数中的超时来等待tf2树上的可用转换。

22.6.2 示例

22.6.2.1 更新监听器节点

编辑turtle_tf2_listener.py并删除传递给第76行lookup_transform()调用的timeout=Duration(seconds=1.0)参数。它应该如下所示：

trans = self._tf_buffer.lookup_transform(
   to_frame_rel,
   from_frame_rel,
   now)

此外，导入我们将在文件开头处理的其他异常：

from tf2_ros import LookupException, ConnectivityException, ExtrapolationException

通过添加新导入的异常和raise语句来编辑第81行的异常处理，以查看异常：

except (LookupException, ConnectivityException, ExtrapolationException):
   self.get_logger().info('transform not ready')
   raise
   return

如果您现在尝试运行启动文件，您将注意到它正在失败：

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

22.6.2.2 修复监听器节点

现在您应该注意到lookup_transform()失败了。它告诉您坐标系不存在或数据在未来。要解决这个问题，请在第76行编辑代码，如下所示（返回timeout参数）：

trans = self._tf_buffer.lookup_transform(
   to_frame_rel,
   from_frame_rel,
   now,
   timeout=rclpy.duration.Duration(seconds=1.0))

lookup_transform可以接受四个参数，其中最后一个是可选的超时。它将在等待超时的持续时间内阻塞。完成此更改后，请从上面添加的except()块中删除raise行，否则代码将继续失败。

现在可以运行启动文件了。

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

您应该注意到，lookup_transform()实际上会阻塞，直到两个海龟之间的转换可用（这通常需要几毫秒）。一旦达到超时（在这种情况下为一秒），只有在转换仍然不可用时才会引发异常。

22.6.3 总结

在本教程中，您了解了有关lookup_transform函数及其超时特性的更多信息。您还学习了如何捕获和处理tf2可能引发的其他异常。

22.7 时间旅行

22.7.1 背景

在前面的教程中，我们讨论了tf2和时间的基础知识。本教程将使我们更进一步，并揭示一个强大的tf2技巧：时间旅行。简而言之，tf2库的一个关键特性是它能够在时间和空间上转换数据。

这个tf2时间旅行功能可以用于各种任务，比如长时间监控机器人的位姿，或者构建一个跟随领导者“脚步”的跟随机器人。我们将使用该时间旅行功能来查找时间上的变换，并将turtle2编程为模拟5秒前的turtle1动作。

22.7.2 时间旅行

首先，让我们回到上一篇教程“使用时间”的结尾。转到learning_tf2_py功能包。

现在，我们不让第二只乌龟去carrot现在所在的地方，而是让第二个乌龟去5秒前第一只carrot所在的地方。将turtle_tf2_listener.py文件中的lookup_transform（）调用编辑为:

when = self.get_clock().now() - rclpy.time.Duration(seconds=5.0)
trans = self.tf_buffer.lookup_transform(
    to_frame_rel,
    from_frame_rel,
    when,
    timeout=rclpy.duration.Duration(seconds=0.05))

现在，如果你运行这个，在前5秒，第二只乌龟将不知道去哪里，因为我们还没有5秒的carrot姿势历史。但这5秒后会发生什么？让我们试一试：

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

你现在应该注意到，你的乌龟正在不受控制地四处行驶，就像这张截图一样。让我们试着理解这种行为背后的原因。

1. 在我们的代码中，我们问了tf2以下问题：“相对于5秒前的turtle2，5秒前carrot1的位姿是什么？”。这意味着我们正在根据第二只乌龟5秒前的位置以及第一只carrot1 5秒前所在的位置来控制第二只海龟。

2. 然而，我们真正想问的是：“相对于5秒前turtle2的位置，carrot1的位姿是什么？”。

22.7.3 lookup_transform()高级接口

为了问tf2这个特定的问题，我们将使用一个高级API，它使我们能够明确地说出何时获取指定的转换。这是通过调用带有附加参数的lookup_transform_full()方法实现的。您的代码现在将如下所示：

when = self.get_clock().now() - rclpy.time.Duration(seconds=5.0)
trans = self.tf_buffer.lookup_transform_full(
        target_frame=to_frame_rel,
        target_time=rclpy.time.Time(),
        source_frame=from_frame_rel,
        source_time=when,
        fixed_frame='world',
        timeout=rclpy.duration.Duration(seconds=0.05))

lookup_transform_full()的高级API采用六个参数：

1. 目标坐标系

2. 转换到的时间

3. 源坐标系

4. 评估源坐标系的时间

5. 不随时间变化的坐标系，在本例中为world坐标系

6. 等待目标坐标系可用的时间

综上所述，tf2在后台执行以下操作。在过去，它计算从carrot1到world的变换。在world内，tf2时间旅行从过去到现在。在当前时间，tf2计算从world到turtle2的变换。

22.7.4 检查结果

让我们再次运行模拟，这次使用高级时间旅行API：

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

是的，第二只乌龟被引导到5秒前第一只carrot所在的地方！

22.7.5 总结

在本教程中，您已经看到了tf2的一个高级功能。您了解了tf2可以及时转换数据，并学习了如何使用turtlesim示例进行转换。tf2允许您回到过去，使用lookup_transform_full()在海龟的旧位姿和当前位姿之间进行坐标转换。

22.8 四元数基础知识

22.8.1 背景

四元数是方向的四元组表示，它比旋转矩阵更简洁。四元数对于分析涉及三维旋转的情况非常有效。四元数广泛应用于机器人、量子力学、计算机视觉和3D动画。

你可以在维基百科上了解更多关于基础数学概念的信息。您还可以观看一个可探索的视频系列，将3blue1brown制作的四元数可视化。

在本教程中，您将学习四元数和转换方法如何在ROS2中工作。

22.8.2 前提

然而，这不是一个硬性要求，您可以使用任何其他最适合您的几何变换库。您可以查看transforms3d、scipy.spatial.transform、pytransform3d、numpy-quaternion或blender.mathutils等库。

22.8.3 四元数的组成

ROS2使用四元数来跟踪和应用旋转。四元数有4个分量（x，y，z，w）。在ROS 2中，w是最后一个，但在一些库中，如Eigen，w可以放在第一个位置。通常使用的不绕x/y/z轴旋转的单位四元数是（0，0，0，1），可以通过以下方式创建：

#include 
...

tf2::Quaternion q;
// Create a quaternion from roll/pitch/yaw in radians (0, 0, 0)
q.setRPY(0, 0, 0);
// Print the quaternion components (0, 0, 0, 1)
RCLCPP_INFO(this->get_logger(), "%f %f %f %f",
            q.x(), q.y(), q.z(), q.w());

四元数的大小应始终为1。如果数值错误导致四元数大小不是1，ROS 2将打印警告。要避免这些警告，请规范化四元数：

q.normalize();

22.8.4 ROS2中的四元数类型

ROS2使用两种四元数数据类型，tf2::Quaternion及其等价的geometry_msgs::msg::Quaternion。要在C++中进行转换四元数数据类型，请使用tf2_geometry_msgs的方法。

C++

#include 
...

tf2::Quaternion tf2_quat, tf2_quat_from_msg;
tf2_quat.setRPY(roll, pitch, yaw);
// Convert tf2::Quaternion to geometry_msgs::msg::Quaternion
geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(tf2_quat);

// Convert geometry_msgs::msg::Quaternion to tf2::Quaternion
tf2::convert(msg_quat, tf2_quat_from_msg);
// or
tf2::fromMsg(msg_quat, tf2_quat_from_msg);

Python

from geometry_msgs.msg import Quaternion
...

# Create a list of floats, which is compatible with tf2
# Quaternion methods
quat_tf = [0.0, 1.0, 0.0, 0.0]

# Convert a list to geometry_msgs.msg.Quaternion
msg_quat = Quaternion(x=quat_tf[0], y=quat_tf[1], z=quat_tf[2], w=quat_tf[3])

22.8.5 四元数运算

22.8.5.1 在RPY中思考，然后转换为四元数

我们很容易想到关于轴的旋转，但很难想到四元数。一种建议是根据滚动（绕X轴）、俯仰（绕Y轴）和偏航（绕Z轴）计算目标旋转，然后转换为四元数。

# quaternion_from_euler method is available in turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py
q = quaternion_from_euler(1.5707, 0, -1.5707)
print(f'The quaternion representation is x: {q[0]} y: {q[1]} z: {q[2]} w: {q[3]}.')

22.8.5.2 应用四元数旋转

若要将一个四元数的旋转应用于位姿，只需将位姿的上一个四元数乘以表示所需旋转的四元数。这个乘法的顺序很重要。

C++

#include 
...

tf2::Quaternion q_orig, q_rot, q_new;

q_orig.setRPY(0.0, 0.0, 0.0);
// Rotate the previous pose by 180* about X
q_rot.setRPY(3.14159, 0.0, 0.0);
q_new = q_rot * q_orig;
q_new.normalize();

Python

q_orig = quaternion_from_euler(0, 0, 0)
# Rotate the previous pose by 180* about X
q_rot = quaternion_from_euler(3.14159, 0, 0)
q_new = quaternion_multiply(q_rot, q_orig)

22.8.5.3 反转四元数

反转四元数的一种简单方法是倒置w分量：

q[3] = -q[3]

22.8.5.4 相对旋转数

假设同一坐标系中有两个四元数，q_1和q_2。您需要找到以以下方式将q_1转换为q_2的相对旋转q_r：

q_2 = q_r * q_1

你可以像求解矩阵方程一样求解q_r。反转q_1并右乘两边。同样，乘法的顺序很重要：

q_r = q_2 * q_1_inverse

下面是一个在python中获取从上一个机器人位姿到当前机器人位姿的相对旋转的示例：

def quaternion_multiply(q0, q1):
    """
    Multiplies two quaternions.

    Input
    :param q0: A 4 element array containing the first quaternion (q01, q11, q21, q31)
    :param q1: A 4 element array containing the second quaternion (q02, q12, q22, q32)

    Output
    :return: A 4 element array containing the final quaternion (q03,q13,q23,q33)

    """
    # Extract the values from q0
    w0 = q0[0]
    x0 = q0[1]
    y0 = q0[2]
    z0 = q0[3]

    # Extract the values from q1
    w1 = q1[0]
    x1 = q1[1]
    y1 = q1[2]
    z1 = q1[3]

    # Computer the product of the two quaternions, term by term
    q0q1_w = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1
    q0q1_x = w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1
    q0q1_y = w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1
    q0q1_z = w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1

    # Create a 4 element array containing the final quaternion
    final_quaternion = np.array([q0q1_w, q0q1_x, q0q1_y, q0q1_z])

    # Return a 4 element array containing the final quaternion (q02,q12,q22,q32)
    return final_quaternion

q1_inv[0] = prev_pose.pose.orientation.x
q1_inv[1] = prev_pose.pose.orientation.y
q1_inv[2] = prev_pose.pose.orientation.z
q1_inv[3] = -prev_pose.pose.orientation.w # Negate for inverse

q2[0] = current_pose.pose.orientation.x
q2[1] = current_pose.pose.orientation.y
q2[2] = current_pose.pose.orientation.z
q2[3] = current_pose.pose.orientation.w

qr = quaternion_multiply(q2, q1_inv)

22.8.6 总结

在本教程中，您学习了四元数的基本概念及其相关的数学运算，如反转和旋转。您还了解了它在ROS2中的用法示例以及两个独立的四元数类之间的转换方法。

23 机器人建模方法URDF

URDF全称为Unified Robot Description Format，中文可以翻译为“统一机器人描述格式”。与计算机文件中的txt文本格式、jpg图像格式等类似，URDF是一种基于XML规范、用于描述机器人结构的格式。根据该格式的设计者所言，设计这一格式的目的在于提供一种尽可能通用的机器人描述规范。

从机构学角度讲，机器人通常被建模为由连杆和关节组成的结构。连杆是带有质量属性的刚体，而关节是连接、限制两个刚体相对运动的结构。关节也被成为运动副。通过关节将连杆依次连接起来，就构成了一个个运动链（也就是这里所定义的机器人模型）。一个URDF文档即描述了这样的一系列关节与连杆的相对关系、惯性属性、几何特点和碰撞模型。具体来说，包括机器人模型的运动学与动力学描述 、机器人的几何表示、机器人的碰撞模型。

23.1 机器人的组成

机器人一般是由硬件结构、驱动系统、传感器系统、控制系统四大部分组成。

硬件结构就是底盘、外壳、电机等实打实可以看到的设备。

驱动系统就是可以驱使这些设备正常使用的装置，比如电机的驱动器，电源管理系统等。

传感系统包括电机上的编码器、板载的IMU、安装的摄像头、雷达等等，便于机器人感知自己的状态和外部的环境。

控制系统就是ROS 2软件开发过程的主要载体，一般是树莓派、电脑等计算平台，以及里边的操作系统和应用软件。

23.1.1 连杆Link的描述

• ：用来描述机器人某个刚体部分的外观和物理属性，外观包括尺寸、颜色、形状，物理属性包括质量、惯性矩阵、碰撞参数等。link标签中的name表示该连杆的名称，我们可以自定义，未来joint连接link的时候，会使用到这个名称。一些标签作用如下：

  • ：用来描述机器人的外观，重在描述机器人看上去的状态，也就是视觉效果。比如：

    • ：表示几何形状，里边使用调用了一个在三维软件中提前设计好的蓝色外观，就是这个stl文件，看上去和真实机器人是一致的。

    • ：表示坐标系相对初始位置的偏移，分别是x、y、z方向上的平移，和roll、pitch、raw旋转，不需要偏移的话，就全为0。

    • ：描述碰撞参数。

  • ：描述机器人运动过程中的状态，比如机器人与外界如何接触算作碰撞。

23.1.2 关节Joint描述

机器人模型中的刚体最终要通过关节joint连接之后，才能产生相对运动。标签中的name表示该关节的名称，type表示该关节的类型，URDF中的关节有六种运动类型。一些标签作用如下：

• ：描述父连杆。

• ：描述子连杆，子连杆会相对父连杆发生运动。

• ：表示两个连杆坐标系之间的关系，也就是图中红色的向量，可以理解为这两个连杆该如何安装到一起。

• ：表示关节运动轴的单位向量，比如z等于1，就表示这个旋转运动是围绕z轴的正方向进行的。

• ：就表示运动的一些限制了，比如最小位置，最大位置，和最大速度等。

23.2 构建机器人模型

在本教程中，我们将构建一个机器人的视觉模型，它看起来像R2D2。在以后的教程中，您将学习如何表达模型，添加一些物理属性，并使用xacro生成更整洁的代码，但目前，我们将专注于使视觉几何正确。

继续之前，请确保已安装joint_state_publisher功能包。如果安装了urdf_tutorial二进制文件，情况应该已经如此。如果没有，请更新您的安装以包含该功能包：

sudo apt update
sudo apt install ros-humble-urdf-tutorial

本教程中提到的所有机器人模型以及源文件都可以在urdf_tutorial功能包中找到。功能包安装路径为：

/opt/ros/humble/share/urdf_tutorial

23.2.1 一个形状

首先，我们将探索一个简单的形状。这是你能做的最简单的事情。

为了将XML翻译成英语，这是一个名为myfirst的机器人，它只包含一个link（连杆，也称为部分），其视觉组件只是一个0.6米长、0.2米半径的圆柱体。

要检查模型，请启动display.launch.py文件：

ros2 launch urdf_tutorial display.launch.py model:=urdf/01-myfirst.urdf

这里发生了三件事：

1. 加载指定的模型并将其保存为参数

2. 运行节点以发布sensor_msgs/msg/JointState和变换

3. 使用配置文件启动Rviz

注意，上面的launch命令假设您是从urdf_tutorial功能包目录执行它的（即：urdf目录是当前工作目录的直接子目录）。如果不是这样，01-myfirst.urdf的相对路径将无效，并且当启动器尝试将urdf作为参数加载时，您将收到一个错误。

一个稍微修改的参数允许它在不考虑当前工作目录的情况下工作：

ros2 launch urdf_tutorial display.launch.py model:=`ros2 pkg prefix --share urdf_tutorial`/urdf/01-myfirst.urdf

如果不是从urdf_tutorial功能包位置运行这些教程中给出的所有示例启动命令，则必须更改这些命令。启动display.launch.py后，您应该会看到RViz显示以下内容：

需要注意的事项：

• 固定坐标系是网格中心所在的变换坐标系。这里，它是由我们的base_link定义的坐标系。

• 默认情况下，视觉元素（圆柱体）的原点位于其几何图形的中心。因此，一半圆柱体位于网格下方。

23.2.2 多个形状

现在让我们来看看如何添加多个形状/连杆。如果我们只是向urdf添加更多的连杆元素，解析器将不知道将它们放在哪里。所以，我们必须添加关节。关节元素可以指柔性关节和非柔性关节。我们将从非柔性或固定的关节开始。

注意我们是如何定义0.6m x 0.1m x 0.2m长方体的。关节是根据父关节和子关节定义的。URDF最终是一个具有一个根连杆的树结构。这意味着leg的位置取决于base_link的位置。

ros2 launch urdf_tutorial display.launch.py model:=urdf/02-multipleshapes.urdf

这两个形状彼此重叠，因为它们共享相同的原点。如果我们希望它们不重叠，我们必须定义更多的原点。

23.2.3 设置原点

R2D2的leg附着在躯干的上半部分，在侧面。这就是我们指定关节的原点的位置。此外，它不连接到leg的中间，而是连接到上部，因此我们也必须偏移leg的原点。我们还需要旋转leg，使其直立。

让我们从检查关节的原点开始。它是根据父对象的参考坐标系定义的。所以我们在y方向上是-0.22米（向左，但相对于轴向右），在z方向上是0.25米（向上）。这意味着无论子连杆的可视原点标签如何，子连杆的原点都将向上并向右。由于我们没有指定rpy（roll pitch yaw）属性，因此默认情况下，子坐标系将与父坐标系具有相同的方向。 现在，看看leg的可视原点，它有xyz和rpy偏移。这定义了可视元素的中心相对于其关节原点的位置。由于我们希望leg附着在顶部，所以我们通过将z偏移设置为-0.3米来向下偏移原点。由于我们希望leg的长部分与z轴平行，所以我们围绕Y轴旋转可视部分的弧度为圆周率/2。

ros2 launch urdf_tutorial display.launch.py model:=urdf/03-origins.urdf

启动文件运行的功能包将根据URDF为模型中的每个连杆创建TF坐标系。Rviz使用这些信息来确定每个形状的显示位置。如果一个TF坐标系不存在一个给定的URDF连杆，则它将以白色放置在原点。

23.2.4 设置材料

“好吧，”我听到你说。“这很可爱，但不是每个人都拥有B21。我的机器人和R2D2不是红色的！”这是一个很好的观点。让我们看看材料标签。

身体现在是蓝色的了。我们定义了一种新材料，称为“蓝色”，红色、绿色、蓝色和alpha通道分别定义为0,0,0.8和1。所有值都可以在[0,1]范围内。然后，base_link的可视元素引用该材料。白色材料的定义类似。您还可以从可视元素中定义材料标签，甚至在其他连杆中引用它。如果你重新定义它，没有人会抱怨。还可以使用纹理指定用于为物体上色的图像文件。

ros2 launch urdf_tutorial display.launch.py model:=urdf/04-materials.urdf

23.2.5 完成模型

现在，我们用更多的形状完成模型：脚、轮子和头。最值得注意的是，我们添加了一个球体和一些网格。我们还将添加一些稍后使用的其他部件。

ros2 launch urdf_tutorial display.launch.py model:=urdf/05-visual.urdf

如何添加球体应该是不言自明的：

这里的网格是从PR2借来的。它们是单独的文件，您必须为其指定路径。您应该使用package://NAME_OF_PACKAGE/path符号。本教程的网格位于urdf_tutorial功能包中名为mesh的文件夹中。

可以以多种不同的格式导入网格。STL相当常见，但引擎也支持DAE，DAE可以有自己的颜色数据，这意味着您不必指定颜色/材料。这些文件通常在单独的文件中。这些网格也引用了网格文件夹中的.tif文件。也可以使用相对缩放参数或边界框大小调整网格大小。我们也可以在完全不同的功能包中引用网格。

至此，一个类似R2D2的URDF模型就建立好了。现在，您可以继续下一个教程，使其移动。

23.3 构建可移动机器人模型

在本教程中，我们将修改上一教程中创建的R2D2模型，使其具有可移动关节。在之前的模型中，所有关节都是固定的。现在我们将探讨其他三种重要类型的关节：连续关节、旋转关节和棱柱关节。

继续之前，请确保已安装所有必备组件。有关所需内容的信息，请参见上一教程。

同样，本教程中提到的所有机器人模型都可以在urdf_tutorial功能包中找到。

这是带有柔性关节的新urdf。您可以将其与以前的版本进行比较，以查看所有更改，但我们将只关注三个示例关节。

要可视化和控制此模型，请运行与上一教程相同的命令：

ros2 launch urdf_tutorial display.launch.py model:=urdf/06-flexible.urdf

这也将弹出一个GUI，允许您控制所有非固定关节的值。改变这些值，可以看到关节将发生移动。

23.3.1 头部

身体和头部之间的连接是一个连续的关节，这意味着它可以呈现从负无穷大到正无穷大的任何角度。车轮也是这样建模的，因此它们可以永远在两个方向上滚动。

我们必须添加的唯一附加信息是旋转轴，这里由xyz三元组指定，它指定头部将围绕其旋转的向量。因为我们希望它绕z轴旋转，所以我们指定向量“0 0 1”。

23.3.2 夹具

左右夹具关节都建模为旋转关节。这意味着它们采用与连续关节相同的方式旋转，但它们有严格的限制。因此，我们必须包含指定关节上限和下限（以弧度为单位）的限制标签。我们还必须指定该关节的最大速度和作用力，但暂时实际值对我们的目的来说并不重要。

23.3.3 夹具臂

左右夹持器关节都建模为旋转关节。这意味着它们以与连续关节相同的方式旋转，但它们有严格的限制。因此，我们必须包含指定关节上限和下限（以弧度为单位）的限制标记。我们还必须指定该关节的最大速度和作用力，但实际值对我们的目的来说并不重要。

夹具臂是一种不同类型的关节，即棱柱关节。这意味着它沿着轴移动，而不是围绕轴移动。这种平移运动使我们的机器人模型能够伸展和缩回其夹具臂。

棱柱臂的限制采用与旋转关节相同的方式指定，但单位是米，而不是弧度。

23.3.4 其它类型关节

还有两种其他类型的关节可以在空间中移动。虽然棱柱关节只能沿一维移动，但平面关节可以在平面或二维中移动。此外，浮动关节是不受约束的，可以在三维空间中任意移动。这些关节不能仅由一个数字指定，因此不包含在本教程中。

23.3.5 指定位姿

当您在GUI中移动滑块时，模型在Rviz中移动。这是怎么做到的？首先，GUI解析URDF并查找所有非固定关节及其限制。然后，它使用滑块的值发布sensor_msgs/msg/JointState消息。然后，robot_state_publisher使用它们来计算不同部分之间的所有变换。然后，生成的变换树用于显示Rviz中的所有形状。

23.3.6 下一步

现在您已经有了一个可视化的功能模型，您可以添加一些物理属性，或者开始使用xacro来简化代码。

23.4 添加物理属性与碰撞属性

23.4.1 碰撞

到目前为止，我们只给连杆们指定了一个单一的子元素visual(可视化)，它定义了机器人的外观。然而，为了使碰撞检测工作或模拟机器人，我们还需要定义碰撞（collision）元素。

这是我们新的base_link代码。

碰撞元素是连杆对象的直接子元素，与可视标签处于同一级别。碰撞元素定义其形状的方式与可视元素相同，使用几何标签。几何标签的格式与可视标签的格式完全相同。也可以采用与可视标签相同的方式指定碰撞标签的原点。

在许多情况下，您希望碰撞几何体、原点与视觉几何体、原始完全相同。然而，有两种主要情况下您不会：

• 更快的处理。对两个网格进行碰撞检测比对两个简单几何体进行碰撞检测要复杂得多。因此，您可能希望用碰撞元素中更简单的几何体替换网格。

• 安全区。您可能希望限制靠近敏感设备的移动。例如，如果我们不想让任何东西与R2D2的头部碰撞，我们可以将碰撞几何体定义为一个包裹着它的头部的圆柱体，以防止任何东西过于靠近它的头部。

23.4.2 物理属性

为了让模型正确模拟，您需要定义机器人的几个物理属性，即Gazebo这样的物理引擎所需的属性。

23.4.3 惯性

每个被模拟的连杆元素都需要一个惯性标签。这里有一个简单的例子。

此元素也是连杆对象的子元素。质量以千克为单位。3x3旋转惯性矩阵由惯性元素指定。由于这是对称的，因此只能由6个元素表示。

这些信息可以通过建模程序（如MeshLab）提供给您。几何图元（圆柱体、长方体、球体）的惯性可以使用惯性矩张量列表来计算。

惯性张量取决于物体的质量和质量分布。可近似假设物体体积中的质量分布相等，并根据物体的形状计算惯性张量。

如果不确定设置多大的值，ixx/iyy/izz=1e-3或更小的矩阵通常是中等尺寸连杆的合理默认值（它对应于边长0.1 m、质量0.6 kg的盒子）。识别矩阵是一个特别糟糕的选择，因为它通常太高（它对应于一个边长0.1米、质量为600千克的盒子！）。

还可以指定原点标签以指定重心和惯性参考系（相对于连杆的参考系）。

当使用实时控制器时，零（或几乎零）的惯性元素会导致机器人模型在没有警告的情况下崩溃，并且所有连杆都会显示为其原点与世界原点重合。

23.4.4 接触系数

您还可以定义连杆彼此接触时的行为。这是通过冲突标签的子元素contact_coefficients完成的。需要指定三个属性：

• 摩擦系数mu

• 刚度系数kp

• 阻尼系数kd

23.4.5 关节动力

关节的移动方式由关节的动力标签定义。这里有两个属性：

• 物理静摩擦friction。对于棱柱关节，单位为N。对于旋转关节，单位为N * m。

• 物理阻尼值damping。对于棱柱关节，单位为 N * s/m。对于旋转关节，N * m * s/rad。

如果未指定，这些系数默认为零。

23.4.6 其他标签

在纯URDF领域（即不包括Gazebo特定标签），还有两个标签可帮助定义关节：calibration和safety controller。

23.4.7 下一步

将会在下一教程学习使用xacro，可以减少代码量和烦人的数学运算。

23.5 使用Xacro简洁代码

现在，如果你在家里用这些步骤设计完成自己的机器人，你可能会厌倦做各种数学运算来正确解析非常简单的机器人描述。幸运的是，你可以使用xacro功能包让你的生活更简单。它做了三件非常有用的事情。

• 常量

• 简单数学

• 宏

在本教程中，我们将使用这些快捷方式，以帮助减少URDF文件的总体大小，并使其更易于阅读和维护。

23.5.1 使用Xacro

顾名思义，xacro是一种用于XML的宏语言。xacro程序运行所有宏并输出结果。典型用法如下：

xacro model.xacro > model.urdf

您还可以在启动文件中自动生成urdf。这很方便，因为它可以保持最新状态，而且不会占用硬盘空间。但是，生成确实需要时间，因此请注意运行启动文件可能需要更长的时间。

path_to_urdf = get_package_share_path('pr2_description') / 'robots' / 'pr2.urdf.xacro'
robot_state_publisher_node = launch_ros.actions.Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    parameters=[{
        'robot_description': ParameterValue(
            Command(['xacro ', str(path_to_urdf)]), value_type=str
        )
    }]
)

在URDF文件的顶部，必须指定一个命名空间，以便正确解析文件。例如，以下是有效xacro文件的前两行：

23.5.2 常量

让我们快速查看一下R2D2中的base_link。

这里的信息有点多余。我们指定圆柱体的长度和半径两次。更糟糕的是，如果我们想改变这一点，我们需要在两个不同的地方这样做。

幸运的是，xacro允许您指定充当常量的属性。我们可以将上面的代码改写为这样。

这两个值在前两行中指定。它们可以在任何地方、任何级别、使用之前或之后进行定义。通常把常量放置在顶端。我们使用美元符号和大括号来表示值，而不是在几何元素中指定实际半径。此代码将生成与上面所示相同的代码。

${}中的键将会被对应的值所替换，并且可以将其与属性中的其他文本组合。

这将生成

然而，${}中的内容不一定只是一个属性，这就引出了我们的下一节…

23.5.3 数学

您可以使用四个基本操作（+、-、*、/）、一元减号和括号在${}构造中构建任意复杂的表达式。示例：

您还可以使用更多的基本数学运算，如sin和cos。

23.5.4 宏

这是xacro功能包中最大、最有用的组件。

23.5.4.1 简单宏

让我们来看一个简单的无用宏。

（这是无用的，因为如果未指定原点，它的值与此值相同。）此代码将生成以下内容。

从技术上讲，名称不是必需的元素，但您需要指定它才能使用它。xacro:$NAME/的每个实例都被xacro:macro标签的内容替换。注意，即使它不完全相同（两个属性的顺序发生了变化），生成的XML也是等效的。如果找不到具有指定名称的xacro，则不会展开它，也不会生成错误。

23.5.4.2 参数化宏

您还可以参数化宏，使它们不会每次都生成相同的精确文本。当与数学功能相结合时，这将更加强大。

首先，让我们以R2D2中使用的一个简单宏为例。

这可以被下面的代码使用：

参数的作用与属性类似，您可以在表达式中使用它们。

也可以将整个代码块用作参数。

要指定块参数，请在其参数名称前包含星号。可以使用insert_block命令插入块，并能根据需要多次插入块。

23.5.5 实际使用

xacro语言在它允许你做的事情上非常灵活。除了上面显示的默认惯性宏之外，以下是一些在R2D2模型中使用xacro的有用方法。

要查看由xacro文件生成的模型，请运行与先前教程相同的命令：

ros2 launch urdf_tutorial display.launch.py model:=urdf/08-macroed.urdf.xacro

（启动文件一直在运行xacro命令，但由于没有可扩展的宏，这无关紧要）。

23.5.5.1 腿部宏

通常，您希望在不同位置创建多个外观相似的对象。你可以使用一个宏和一些简单的数学来减少你必须写的代码量，就像我们对R2的两条腿做的那样。

常见技巧1：使用名称前缀获取两个名称相似的对象。 常见技巧2：使用数学计算关节原点。如果你改变了机器人的大小，用一些数学方法改变一个属性来计算关节偏移会省去很多麻烦。 常见技巧3：使用反射参数，并将其设置为1或-1。例如使用reflect参数将腿放在base_to${prefix}leg原点的两侧。

23.6 将URDF与robot_state_publisher一起使用

23.6.1 背景

本教程将向您展示如何建模行走机器人，将状态发布为tf2消息，并在Rviz中查看模拟。首先，我们创建描述机器人组件的URDF模型。接下来，我们编写一个节点来模拟运动并发布JointState和变换。然后，我们使用robot_state_publisher将整个robot状态发布到/tf2。

23.6.2 前提

一如既往，不要忘记在您打开的每一个新终端中设置环境变量。

23.6.3 任务

23.6.3.1 创建功能包

mkdir -p ~/second_ros2_ws/src  # change as needed
cd ~/second_ros2_ws/src
ros2 pkg create urdf_tutorial_r2d2 --build-type ament_python --dependencies rclpy
cd urdf_tutorial_r2d2

现在您应该看到一个urdf_tutorial_r2d2文件夹。接下来，您将对其进行几个更改。

23.6.3.2 创建URDF文件

创建存储urdf文件的目录：

mkdir -p urdf

下载URDF文件并将其另存为~/sec_ros2_ws/src/URDF_tutorial_r2d2/URDF/r2d2.URDF.xml。下载地址：

http://docs.ros.org/en/humble/_downloads/872802005223ffdb75b1ab7b25ad445b/r2d2.urdf.xml)

下载Rviz配置文件并将它另存为~/sec_ros_2/ws/src/URDF_tutorial_rd2/URDF/r2d2.Rviz。下载地址：

http://docs.ros.org/en/humble/_downloads/96d68aef72c4f27f32af5961ef48c475/r2d2.rviz

23.6.3.3 发布状态

现在我们需要一种方法来指定机器人处于什么状态。为此，我们必须指定所有三个关节和整体里程。

启动您喜爱的编辑器，将以下代码粘贴到~/second_ros2_ws/src/urf_tutorial_r2d2/urdf_tutorial.r2d2/state_publisher.py。

from math import sin, cos, pi
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformStamped

class StatePublisher(Node):

    def __init__(self):
        rclpy.init()
        super().__init__('state_publisher')

        qos_profile = QoSProfile(depth=10)
        self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
        self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
        self.nodeName = self.get_name()
        self.get_logger().info("{0} started".format(self.nodeName))

        degree = pi / 180.0
        loop_rate = self.create_rate(30)

        # robot state
        tilt = 0.
        tinc = degree
        swivel = 0.
        angle = 0.
        height = 0.
        hinc = 0.005

        # message declarations
        odom_trans = TransformStamped()
        odom_trans.header.frame_id = 'odom'
        odom_trans.child_frame_id = 'axis'
        joint_state = JointState()

        try:
            while rclpy.ok():
                rclpy.spin_once(self)

                # update joint_state
                now = self.get_clock().now()
                joint_state.header.stamp = now.to_msg()
                joint_state.name = ['swivel', 'tilt', 'periscope']
                joint_state.position = [swivel, tilt, height]

                # update transform
                # (moving in a circle with radius=2)
                odom_trans.header.stamp = now.to_msg()
                odom_trans.transform.translation.x = cos(angle)*2
                odom_trans.transform.translation.y = sin(angle)*2
                odom_trans.transform.translation.z = 0.7
                odom_trans.transform.rotation = \
                    euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw

                # send the joint state and transform
                self.joint_pub.publish(joint_state)
                self.broadcaster.sendTransform(odom_trans)

                # Create new robot state
                tilt += tinc
                if tilt < -0.5 or tilt > 0.0:
                    tinc *= -1
                height += hinc
                if height > 0.2 or height < 0.0:
                    hinc *= -1
                swivel += degree
                angle += degree/4

                # This will adjust as needed per iteration
                loop_rate.sleep()

        except KeyboardInterrupt:
            pass

def euler_to_quaternion(roll, pitch, yaw):
    qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
    qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
    qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
    qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
    return Quaternion(x=qx, y=qy, z=qz, w=qw)

def main():
    node = StatePublisher()

if __name__ == '__main__':
    main()

23.6.3.4 创建启动文件

创建新的~/second_ros2_ws/src/urdf_tutorial_r2d2/launch文件夹。打开编辑器并粘贴以下代码，将其另存为~/second_ros2_ws/src/urdf_tutorial_r2d2/launch/demo.launch.py。

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    urdf_file_name = 'r2d2.urdf.xml'
    urdf = os.path.join(
        get_package_share_directory('urdf_tutorial_r2d2'),
        urdf_file_name)
    with open(urdf, 'r') as infp:
        robot_desc = infp.read()

    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
            arguments=[urdf]),
        Node(
            package='urdf_tutorial_r2d2',
            executable='state_publisher',
            name='state_publisher',
            output='screen'),
    ])

23.6.3.5 编辑启动文件

您必须告诉编译工具colcon如何安装Python包。按如下方式编辑~/second_ros2_ws/src/urdf_tutorial_r2d2/setup.py文件：

包含import语句。

import os
from glob import glob
from setuptools import setup
from setuptools import find_packages

在data_files中添加这两行。

data_files=[
  ...
  (os.path.join('share', package_name), glob('launch/*.py')),
  (os.path.join('share', package_name), glob('urdf/*'))
],

修改entry_points表，以便以后可以从控制台运行“state_publisher”。

'console_scripts': [
    'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
],

保存setup.py文件及其更改。

23.6.3.6 安装功能包

cd ~/second_ros2_ws
colcon build --symlink-install --packages-select urdf_tutorial_r2d2
source install/setup.bash

23.6.3.7 查看结果

启动功能包：

ros2 launch urdf_tutorial_r2d2 demo.launch.py

打开新终端，运行Rviz:

rviz2 -d ~/second_ros2_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz

23.6.4总结

您创建了一个JointState发布节点，并将其与robot_state_publisher耦合，以模拟行走机器人。这些示例中使用的代码地址：

https://github.com/benbongalon/ros2-urdf-tutorial/tree/master/urdf_tutorial

24 三维物理仿真平台Gazebo

24.1 介绍

Gazebo是ROS系统中最为常用的三维物理仿真平台，支持动力学引擎，可以实现高质量的图形渲染，不仅可以模拟机器人及周边环境，还可以加入摩擦力、弹性系数等物理属性。可以帮助我们验证机器人算法、优化机器人设计、测试机器人场景应用，为机器人开发提供更多可能。

简而言之，Gazebo是三维仿真平台，核心功能是创造数据。

官网地址：

https://gazebosim.org/home

24.1.1 安装

打开终端，输入以下指令安装安装Gazebo：

sudo apt install ros-humble-gazebo-*

24.1.2 导入模型库

24.1.2.1 下载模型文件

模型库地址：

https://github.com/osrf/gazebo_models

该库包含许多常用模型，将其下载为压缩包格式。

24.1.2.2 创建模型文件夹

cd ~/.gazebo/
mkdir -p models

./gazebo文件夹默认被隐藏，需要按下“Ctrl+H”才能看到被隐藏的文件。

24.1.2.3 拷贝模型文件

最后，将下载好的压缩包拷贝至新建的models文件夹下并解压。再次打开Gazebo便可以加载我们下载好的models了。

24.1.2.4 测试

打开Gazebo，在“insert”面板中选择模型导入，可查看效果。

gazebo

24.1.3 Gazebo界面

这节将对Gazebo图形用户界面进行介绍。我们将学习到一些基本操作，例如按钮的功能以及如何在场景中导航。

gazebo交互界面由许多组成部分，接下来将逐一介绍。

24.1.3.1 场景

场景是模拟器的主要部分，是仿真模型显示的地方，你可以在这操作仿真对象，使其与环境进行交互。

24.1.3.2 面板

面板分为左右两侧，可以把两侧的面板设置为显示、隐藏或调整它的大小。

启动Gazebo时，默认情况下会显示左侧面板。面板中有三个选项卡：

• WORLD: 该选项卡显示当前场景中的模型，并允许您查看和修改模型参数，例如它们的位姿。您也可以通过展开“GUI”选项并调整相机位姿来更改相机视角。

• INSERT: 该选项卡是向仿真场景中添加新对象（模型）。要查看模型列表，需要单击箭头展开文件夹。单击（并释放）要插入的模型，然后在场景中再次单击以添加它。

• LAYERS: 该选项卡组织并显示仿真中可用的不同可视化组（如果有）。一个图层可以包含一个或多个模型。打开或关闭图层将显示或隐藏该图层中的模型。这是一个可选功能，因此在大多数情况下，此选项卡将为空。

默认情况下，右侧面板处于隐藏状态。单击并拖动该栏以将其打开。右面板可用于与选定模型的移动部件（关节）交互。如果场景中没有选择模型，面板将不显示任何信息。

24.1.3.3 工具栏

Gazebo界面有两个工具栏。一个位于场景上方，另一个位于下方。

顶部工具栏为主工具栏，包含一些与模拟器交互时最常用的选项，如按钮：选择、移动、旋转和缩放；创建简单形状（例如立方体、球体、圆柱体）以及复制/粘贴。

• 选择模型（Select mode）：在场景中做标注；

• 转换模式（Translate mode）：选择要移动的模型；

• 旋转模式（Rotate mode）：选择要旋转的模型；

• 缩放模式（Scale mode）：选择要缩放的模型；

• 撤消/重做（Undo/Redo）：撤消/重做场景中的操作；

• 灯光（Lights）：将灯光添加到场景；

• 复制/粘贴（Copy/Paste）：复制/粘贴场景中的模型；

• 对齐（Align）：将模型彼此对齐；

• 捕捉（Snap）：将一个模型捕捉到另一个模型；

• 更改视图（Change view）：从不同角度查看场景。

底部工具栏显示有关仿真的数据，如仿真时间（Simulation time）及其与真实时间（Real time）的关系。

• “仿真时间”是指当仿真运行时，时间在仿真环境中过得有多快。仿真可以比真实时间慢或快，具体取决于运行仿真所需的计算量。

• “真实时间”是指在仿真环境中运行时实际经过的时间。仿真时间和真实时间的比率称为实时因子。

世界状态每迭代一次就更新一次。您可以在底部工具栏的右侧看到迭代次数。每次迭代都会将仿真推进一个固定的秒数，称为步长。默认情况下，步长为1ms。您可以按“暂停”按钮暂停仿真，并使用“步长”按键一次执行多个步长。

24.1.3.4 菜单栏

像大多数应用程序一样，Gazebo顶部有一个应用程序菜单。某些菜单选项会显示工具栏中。在场景中，右键单击上下文菜单选项，可查看各种菜单。注意：有些Linux桌面会隐藏应用程序菜单。如果看不到菜单，请将光标移到应用程序窗口的顶部，菜单就会出现。

右键单击模型将打开一个包含各种选项的上下文菜单。另外，鼠标在场景中导航时非常有用，使用鼠标可以在场景中导航和更改视角。

24.1.4 自定义模型

24.1.4.1 urdf与sdf

urdf

统一机器人描述格式（urdf）是ROS用于描述机器人的所有元素的XML文件格式。要在gazebo中使用urdf文件，必须添加一些特定用于仿真的标签才能与gazebo一起正常使用。

尽管urdf在ROS中是一种有用且标准化的格式，但它们缺少许多功能，并且尚未进行更新以应对机器人技术的不断发展的需求。urdf只能单独指定单个机器人的运动学和动力学特性，无法指定世界中机器人本身的位姿。它也不是通用的描述格式，因为它不能指定关节环（平行连接），并且缺乏摩擦和其他特性。此外，它不能指定非机器人，例如灯光，高度图等。在实现方面，urdf语法大量使用XML属性破坏了正确的格式设置，这反过来又使urdf更加不灵活。

sdf

为了解决此问题，创建了一种称为仿真描述格式（sdf）的新格式 ，供gazebo使用，以解决urdf的缺点。sdf是从世界级到机器人级的所有内容的完整描述，能够描述机器人、静态和动态物体、照明、地形甚至物理学的各方面的信息。sdf可以精确描述机器人的各类性质，除了传统的运动学特性之外，还可以为机器人定义传感器、表面属性、纹理、关节摩擦等；sdf还提供了定义各种环境的方法。包括环境光照、地形等。sdf也使用XML格式进行描述。总结而言，sdf是urdf的进化版，能够更好的描述真实的模拟条件。

24.1.4.2 格式选择

尽管目前有一些sdf与urdf的之间的转换方法，但往往十分复杂且易出错。因此，建议刚开始就根据自己的需求选择最合适的模型格式。

• 必须使用urdf的情况：要使用rviz进行可视化操作。

• 必须使用sdf的情况：研究并联机器人，或机器人中存在封闭链结构。

• 建议使用urdf的情况：要尽快做出仿真用以演示效果；使用Solidworks建模，想方便地导出ROS三维模型。

• 建议使用sdf的情况：想深入研究ROS-gazebo仿真，使仿真的动力学特性更加真实；想开发自己专用的Gazebo仿真插件。

24.2 设置机器人仿真

24.2.1 前提

确保已安装Gazebo

24.2.2 任务

24.2.2.1 启动仿真

在这个演示中，您将在Gazebo中模拟一个简单的差动驱动机器人。您将使用Gazebo示例中定义的一个世界，名为visualize_lidar.sdf。要运行此示例，您应该在终端中执行以下命令：

ign gazebo -v 4 -r visualize_lidar.sdf

当模拟运行时，您可以使用ign命令行工具检查Gazebo提供的话题：

ign topic -l

将会输出：

/clock
/gazebo/resource_paths
/gui/camera/pose
/gui/record_video/stats
/model/vehicle_blue/odometry
/model/vehicle_blue/tf
/stats
/world/visualize_lidar_world/clock
/world/visualize_lidar_world/dynamic_pose/info
/world/visualize_lidar_world/pose/info
/world/visualize_lidar_world/scene/deletion
/world/visualize_lidar_world/scene/info
/world/visualize_lidar_world/state
/world/visualize_lidar_world/stats

由于您尚未启动ROS 2节点，所以ros2话题列表的输出应该没有任何机器人话题：

ros2 topic list

将会输出：

/parameter_events
/rosout

24.2.2.2 集成ROS2与Gazebo

为了能够与ROS2通信我们的模拟，您需要使用一个名为ros_gz_bridge的包。使用该包可在ROS2和Gazebo Transport之间交换消息。存储库提供了ROS和Gazebo之间集成的软件包源码：

https://github.com/gazebosim/ros_gz/tree/ros2#from-source

• ros_gz：提供所有其它包的Metapackage。

• ros_gz_image：使用image_transport将图像从Gazebo传输到ROS的单向传输桥。

• ros_gz_bridge：Gazebo Transport和ROS之间的双向运输桥。

• ros_gz_sim：方便的启动文件和可执行文件，用于将Gazebo Sim与ROS一起使用。

• ros_gz_sim_demos：ROS与Gazebo集成示例。

• ros_gz_point_cloud：用于从Gazebo Sim模拟向ros发布点云的插件。

其中ros_gz_bridge提供了一个网络桥接，使ROS2和Gazebo Transport之间能够交换消息。它的支持仅限于某些消息类型，以下消息类型可以桥接话题：

ROS type	Gazebo Transport Type
builtin_interfaces/Time	gz.msgs.Time
geometry_msgs/Point	gz.msgs.Vector3d
geometry_msgs/Pose	gz.msgs.Pose
geometry_msgs/msg/PoseArray	gz.msgs.Pose_V
geometry_msgs/PoseStamped	gz.msgs.Pose
geometry_msgs/PoseWithCovariance	gz.msgs.PoseWithCovariance
geometry_msgs/Quaternion	gz.msgs.Quaternion
geometry_msgs/Transform	gz.msgs.Pose
geometry_msgs/TransformStamped	gz.msgs.Pose
geometry_msgs/Twist	gz.msgs.Twist
geometry_msgs/TwistWithCovariance	gz.msgs.TwistWithCovariance
geometry_msgs/Vector3	gz.msgs.Vector3d
geometry_msgs/Wrench	gz.msgs.Wrench
gps_msgs/GPSFix	gz.msgs.NavSat
nav_msgs/Odometry	gz.msgs.Odometry
nav_msgs/Odometry	gz.msgs.OdometryWithCovariance
rcl_interfaces/ParameterValue	gz.msgs.Any
ros_gz_interfaces/Contact	gz.msgs.Contact
ros_gz_interfaces/Contacts	gz.msgs.Contacts
ros_gz_interfaces/Dataframe	gz.msgs.Dataframe
ros_gz_interfaces/Entity	gz.msgs.Entity
ros_gz_interfaces/msg/Float32Array	gz.msgs.Float_V
ros_gz_interfaces/GuiCamera	gz.msgs.GUICamera
ros_gz_interfaces/JointWrench	gz.msgs.JointWrench
ros_gz_interfaces/Light	gz.msgs.Light
ros_gz_interfaces/ParamVec	gz.msgs.Param
ros_gz_interfaces/ParamVec	gz.msgs.Param_V
ros_gz_interfaces/StringVec	gz.msgs.StringMsg_V
ros_gz_interfaces/TrackVisual	gz.msgs.TrackVisual
ros_gz_interfaces/VideoRecord	gz.msgs.VideoRecord
rosgraph_msgs/Clock	gz.msgs.Clock
sensor_msgs/BatteryState	gz.msgs.BatteryState
sensor_msgs/CameraInfo	gz.msgs.CameraInfo
sensor_msgs/FluidPressure	gz.msgs.FluidPressure
sensor_msgs/Image	gz.msgs.Image
sensor_msgs/Imu	gz.msgs.IMU
sensor_msgs/JointState	gz.msgs.Model
sensor_msgs/LaserScan	gz.msgs.LaserScan
sensor_msgs/MagneticField	gz.msgs.Magnetometer
sensor_msgs/NavSatFix	gz.msgs.NavSat
sensor_msgs/PointCloud2	gz.msgs.PointCloudPacked
std_msgs/Bool	gz.msgs.Boolean
std_msgs/ColorRGBA	gz.msgs.Color
std_msgs/Empty	gz.msgs.Empty
std_msgs/Float32	gz.msgs.Float
std_msgs/Float64	gz.msgs.Double
std_msgs/Header	gz.msgs.Header
std_msgs/Int32	gz.msgs.Int32
std_msgs/String	gz.msgs.StringMsg
std_msgs/UInt32	gz.msgs.UInt32
tf2_msgs/TFMessage	gz.msgs.Pose_V
trajectory_msgs/JointTrajectory	gz.msgs.JointTrajectory

以及以下服务：

ROS type	Gazebo request	Gazebo response
ros_gz_interfaces/srv/ControlWorld	gz.msgs.WorldControl	gz.msgs.Boolean

打开终端输入以下指令安装使ROS与Gazebo集成的软件包：

sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list'
 curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - sudo apt-get update # 添加ros功能包源
 
sudo apt install ros-humble-ros-ign # 安装ros_gz

打开终端输入以下指令查看相关帮助：

ros2 run ros_gz_bridge parameter_bridge -h

我们可以初始化一个双向桥，这样我们就可以让ROS作为发布者，让Gazebo作为订阅者，反之亦然。例如：

ros2 run ros_gz_bridge parameter_bridge /TOPIC@ROS_MSG@GZ_MSG

ros2 run ros_gz_bridge parameter_bridge命令只需从ros_gz_bidge包中运行parameter_bridge代码。然后，我们指定发送消息的话题/TOPIC。第一个@符号分隔话题名称和消息类型，第一个@符号后面是ROS消息类型。 ROS消息类型后跟@、[、]等符号，其中： @是双向桥。 [是从Gazebo到ROS的单向桥。 ]是从ROS到Gazebo的单项桥。

此时，您已准备好启动从ROS到Gazebo的网桥。特别是要为话题/model/vhicle_blue/cmd_vel创建一个网桥：

source /opt/ros/humble/setup.bash
ros2 run ros_gz_bridge parameter_bridge /model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist

有关ros_gz_bridge的详细信息，请查看：

https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge

一旦网桥运行，机器人就能够按照你的电机指令进行运作。有两个选项：

使用ros2 topic pub向话题发送命令：

ros2 topic pub /model/vehicle_blue/cmd_vel geometry_msgs/Twist "linear: { x: 0.1 }"

teleop_twist_keyboard包。该节点从键盘接收按键，并将其作为消息发布。您可以输入以下指令安装它：

sudo apt-get install ros-humble-teleop-twist-keyboard

teleop_twist_keyboard发布twist消息的默认话题是/cmd_vel，但您可以重新映射此话题以使用网桥中使用的话题：

source /opt/ros/humble/setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/model/vehicle_blue/cmd_vel

将会输出：

This node takes keypresses from the keyboard and publishes them
as Twist messages. It works best with a US keyboard layout.
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

currently:      speed 0.5       turn 1.0

24.2.2.3 可视化ROS 2雷达数据

差动驱动机器人有一个激光雷达。要将Gazebo生成的数据发送到ROS2，您需要启动另一个网桥。在这种情况下，Gazebo Transport话题/lidar2提供激光雷达的数据，您将在网桥中重新映射该话题。本话题将在话题/lidar_scan下提供：

source /opt/ros/humble/setup.bash
ros2 run ros_gz_bridge parameter_bridge /lidar2@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan --ros-args -r /lidar2:=/laser_scan

要在ROS2中可视化激光雷达的数据，可以使用Rviz2：

source /opt/ros/humble/setup.bash
rviz2

然后需要配置固定坐标系：

然后点击“添加”按钮，显示激光雷达：

现在您应该可以在Rviz2中看到激光雷达的数据：

24.3 总结

在本教程中，您使用Gazebo启动了机器人模拟，启动了带有驱动器和传感器的网桥，可视化了传感器的数据，并移动了一个差动驱动机器人。

常与Gazebo配合使用的是Rviz，是一款三维可视化工具，很好地兼容了各种基于ROS软件框架的机器人平台。Rviz可以用XML对机器人、周围物体等任何实物进行尺寸、质量、位置、材质、关节等属性的描述，并且再界面中呈现出来。Rviz还可以通过图形化方式，实时显示机器人传感器的信息、机器人的运动状态、周围环境的变化等。Rviz通过机器人模型参数、机器人发布的传感器信息等数据，为用户进行所有可检测信息的图形化。简而言之，Gazebo是三维仿真平台，核心功能是创造数据；Rviz是三维可视化工具，核心功能是显示数据。

安装与启动

若系统当中没有集成rviz，请输入以下指令安装：

sudo apt-get install rviz2

运行仿真环境

ros2 run rviz2 rviz2

25 测试

为什么要自动测试？

以下是我们为什么要进行自动化测试的一些好理由：

您可以更快地对代码进行增量更新。ROS有数百个具有许多相互依赖性的功能包，因此很难预测一个小小的更改可能会导致的问题。如果您的更改通过了单元测试，您可以更加确信您没有引入问题，或者至少这些问题不是您的错。

您可以更自信地重构代码。通过单元测试可以验证您在重构时没有引入任何错误。这给了你一个恐惧变化的美妙解放！

这导致了更好的代码设计。单元测试迫使您编写代码，以便更容易测试。这通常意味着保持底层功能和框架分离，这是我们ROS代码的设计目标之一。

它们可以防止重复出现的错误（错误回归）。为修复的每个bug编写单元测试是一个很好的做法。事实上，在修复bug之前编写单元测试。这将帮助您精确地，甚至确定地，重现错误，并更准确地了解问题所在。因此，您还将创建一个更好的补丁，然后可以使用回归测试来测试，以验证错误是否已修复。这样，如果代码稍后被修改，错误就不会被意外地重新引入。这也意味着更容易说服补丁的审查者问题已经解决，并且贡献的质量很高。

其他人可以更容易地处理您的代码（一种自动的文档形式）。当你做出改变时，很难弄清楚你是否破坏了别人的代码。单元测试是其他开发人员验证其更改的工具。自动测试记录您的编码决策，并自动与其他开发人员沟通他们的违规行为。因此，测试成为代码的文档，一个大多数时候不需要阅读的文档，当它确实需要检查时，测试系统将精确地指示要阅读的内容（哪些测试失败）。通过编写自动测试，您可以使其他贡献者更快。这改善了整个ROS项目。

如果我们有自动化的单元测试，就更容易成为ROS的贡献者。新的外部开发人员很难为您的组件做出贡献。当他们对代码进行更改时，往往是在盲目的情况下进行的，这是由大量的猜测驱动的。通过提供一系列自动化测试，您可以帮助他们完成任务。他们会立即得到更改的反馈。对项目做出贡献变得更容易，新的贡献者也更容易加入。此外，他们的第一个贡献质量更高，这减少了维护人员的工作量。双赢！

自动测试简化了维护。特别是对于成熟的功能包，它的变化更慢，而且大多需要更新到新的依赖项，自动测试套件有助于非常快速地确定包是否仍然有效。这使得决定软件包是否仍然受支持变得更加容易。

自动测试放大了持续集成的价值。回归测试，以及基于场景的需求测试，有助于组件的自动化测试的整体。您的组件可以根据它所依赖的其他API的演变进行更好的测试（CI服务端将更好、更准确地告诉您代码中出现的问题）。

也许写测试最重要的好处是测试让你成为一个好公民。测试长期影响质量。在许多开源项目中，这是一种公认的做法。通过编写回归测试，您正在为ROS生态系统的长期质量做出贡献。

这一切都是免费的吗？

当然，从来没有免费的午餐。为了获得测试的好处，需要一些投资。

您需要开发一个测试，这有时可能很困难或成本高昂。有时它也可能是不平凡的，因为测试应该是自动的。如果你的测试涉及到特殊的硬件（他们不应该：尝试使用模拟、模拟硬件或将测试范围缩小到较小的软件问题）或需要外部环境（例如人工操作员），事情就会变得特别棘手。

需要维护回归测试和其他自动测试。当组件的设计发生变化时，许多测试都会失效（例如，它们不再编译，或抛出与API设计相关的运行时异常）。这些测试之所以失败，不仅是因为重新设计重新引入了错误，还因为它们需要更新为新设计。偶尔，随着更大的重新设计，旧的回归测试应该被放弃。

大量测试可能需要很长时间才能运行，这会增加持续集成服务端的成本。

25.1 从命令行运行ROS2中的测试

25.1.1 编译并运行测试

要编译和运行测试，只需从colcon运行测试指令：

colcon test --cmake-args tests [package_selection_args]

注释：其中package_selection_args是colcon的可选功能包参数，用于限制编译和运行哪些功能包。

25.1.2 检查测试结果

要查看结果，只需从colcon运行测试结果指令：

colcon test-result --all

25.2 用Python编写基本测试

起点：我们假设您已经设置了一个基本的ament_python包，并且希望向其中添加一些测试。

如果您使用的是ament_cmake_python，请参阅ament_cmake_python文档，了解如何使测试不可恢复。测试内容和对colcon的调用保持不变。

25.2.3 功能包设置

25.2.3.1 setup.py

setup.py必须在调用setup（…）时对pytest具有测试依赖关系：

tests_require=['pytest'],

25.2.3.2 测试文件和文件夹

测试代码需要放在功能包根目录中名为tests的文件夹中。

任何包含要运行的测试的文件都必须具有模式test_FOO.py，其中FOO可以用任何东西替换。

25.2.3.3 功能包目录层级结构示例：

awesome_ros_package/
  awesome_ros_package/
      __init__.py
      fozzie.py
  package.xml
  setup.cfg
  setup.py
  tests/
      test_init.py
      test_copyright.py
      test_fozzie.py

25.2.3.4 测试内容

你现在可以尽情地写测试了。pytest上有很多资源，但简而言之，您可以使用test_前缀编写函数，并包含任何您想要的断言语句。

def test_math():
    assert 2 + 2 == 5   # This should fail for most mathematical systems

25.2.3.5 特殊命令

除了标准的colcon测试命令之外，您还可以从命令行使用--pytest args为pytest框架指定参数。例如，可以指定要运行的函数的名称:

colcon test --packages-select  --pytest-args -k name_of_the_test_function

附录

ROS官网：ROS: Home

ROS论坛：ROS Discourse

ROS Answers：Questions - ROS Answers: Open Source Q&A Forum

古月居：古月居 - ROS机器人知识分享社区

鱼香ROS：鱼香ROS

创客智造：www.ncnynl.com

rclpy：rclpy — rclpy 0.6.1 documentation

rosdep：ROS packages — rosdep 0.22.1 documentation

超载巴赫朋克：超载巴赫朋克 - OverloadBachPunk