序言
机器人操作系统(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]
作者：刘思培
Date: 2023.1.1
Table of contents
序言目录前言提醒结构致谢建议版权声明许可声明第一部分 基础篇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 特殊命令附录
foreword
remind
Before starting to learn ROS2, the author first takes you to understand Ubuntu. Presumably readers know that Linux is a free and open source operating system. Strictly speaking, the word Linux itself only means the Linux kernel, and the Ubuntu operating system is one of the operating systems that use the Linux kernel. Like many computer technologies, the update of ROS2 is very fast. Currently, the most compatible system for ROS2 is the Ubuntu operating system, so all the content of this tutorial is based on the current newer version of the Ubuntu22.04 LTS operating system, corresponding to the ROS2 version It's ROS2:Humble. Readers are also reminded to strictly install the version corresponding to Ubuntu and ROS2, otherwise there will be difficult-to-solve errors.
ROS2 can use a variety of programming languages for robot development. Due to the author's limited level of other programming languages, this tutorial mainly uses Python code. This tutorial only needs a computer, no other hardware peripherals are required.
structure
This tutorial is divided into three parts, the first part: basic articles; the second part: development articles; the third part: tools articles.
The first part, basic articles. In the basic chapter, we will first explain how to install related software. For starters, installation is not an easy task! During the installation, the reader will initially experience how to use the Linux terminal command. In addition to installing ROS2, you also need to install the turtle robot and the development tool VS Code. After installation, the author will use the terminal commands provided by ROS2 to help readers understand the concepts of ROS2. A robot is a complex of multiple functions, and each function contains multiple tasks. ROS2 implements these tasks through nodes, and exchanges information between tasks through three communication interfaces called topics, services, actions, and other communication methods. In addition, in order for developers to modify robot data in real time, ROS2 also has the concept of parameters. Readers will learn these related concepts through the turtle robot officially provided by ROS2. Workspaces and feature packages are containers for ROS2 projects and codes. After completing the study of the above knowledge, the naming conventions of these professional terms will be explained.
The second part is development. The concepts related to ROS2 learned in the basic chapter are covered in this chapter. Readers who already have an understanding of ROS-related concepts and need practical operations can skip the previous content and read this article directly. This article is the most important one of this tutorial. Based on my own learning and development experience, the author will tell readers how to use ROS2 with OpenCV to develop a simple face recognition robot. Through this case, I will briefly summarize the development of a ROS2 project. process. Before studying this article, readers should have a certain understanding of computer vision and machine learning software library OpenCV. There are many ways to learn on the Internet, so I won’t repeat them here. Due to the limited energy of the author, the code is not analyzed line by line in this article, but all necessary content has been added to the code comments. The main purpose of this article is to explain the process of using ROS2 to develop a project. According to this tutorial case, it is divided into the following steps: architecture design, creation of workspace, creation of communication interface, creation of nodes, implementation of node tasks, and use of parameters. The project code address is:
https://gitee.com/sipeiliu/facer
The third part is tools. The power of ROS2 is that there are many tools and advanced functions that can help developers improve efficiency, such as modular management tools, robot coordinate system management tools, 3D physical simulation platform, 3D visualization display platform, etc., the author will give a brief introduction . In addition, many functions of ROS2 (such as tf2, DDS, etc.) are not mentioned or in-depth in this tutorial, please refer to the official documentation for details. Since some tools are not used by the author in the course of work, and the content of this part is relatively messy, most of the content in this article is the original translation of official documents based on my own understanding. Most of the technical terms have not yet formed a unified naming convention. After translation, there will be difficulties in understanding, and it is inevitable to be obscure. Therefore, readers are advised to read the English version of the official document as much as possible, and this article can be used as a reference.
thank you
First of all, I would like to thank Mr. Zhang Haotian for his help in the process of learning ROS2, and thank Mr. Wei Peng for his valuable suggestions on this tutorial.
Special thanks to Lei Shicheng and Xiao Bo (in no particular order) for helping me complete the review and proofreading of this tutorial, and for their encouragement and support during this period.
Finally, I would like to thank the relevant outstanding developers for publishing many excellent articles to the Internet for free. This tutorial refers to and selects some of the contents of these articles.
suggestion
由于笔者精力有限，本教程并不涵盖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 official website:
https://ubuntu.com
1.1 install ros2
Due to the domestic network environment, or readers who do not want to spend too much time on the installation details, you can go directly to the "Other" section below to get a quick way to install ROS2 and related software.
To install ros2 under the Ubuntu operating system, four steps are required. The first step is to open the Ubuntu terminal to set the locale; the second step is to add the source; the third step is to execute the installation instructions. The fourth step is to set environment variables. Below, the author introduces these four steps one by one.
1.1.1 Regional Settings
Locales, which are language- and region-specific rules. Different systems, platforms, and software have different regional setting processing methods and different setting ranges. The content of regional settings includes: data format, currency amount format, decimal point symbol, thousandth symbol, measurement unit, currency symbol, date writing, Calendar types, text sorting, name formatting, addresses, and more. For example, if the reader selects the US English locale in the Ubuntu system, the date message will be displayed in the format "month-day-year" in the Ubuntu system, and the currency symbol will be represented by "$".
Open the terminal and enter the following command to display the locale of the current system:
locale # Display the locale of the current system in the form of key-value pairs
Note: Open the terminal shortcut 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等。
Encoding refers to the process of converting characters in human language into binary sequences that computers can process. UTF-8 is an encoding method for Unicode. It can be used to represent any character in the Unicode standard, and the first byte in its encoding is still compatible with ASCII, so that the software that originally processed ASCII characters can continue to be used without or with only a small number of modifications. Therefore, it has gradually become the preferred encoding method for email, web pages, and other applications that store or transmit text.
In order to ensure that ROS2 can be installed and run smoothly, regional settings are required. Open the terminal and enter the following command:
sudo apt update && sudo apt install locales # Update package list and install locale package sudo locale-gen en_US en_US.UTF-8 # Generate locale package for en_US.UTF-8 sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 # Mandatory setting locale is en_US.UTF-8, default setting locale is en_US.UTF-8 export LANG=en_US.UTF-8 # Set environment variable LANG=en_US.UTF-8
Notes:
-
sudo is a system management command. It is a tool that allows system administrators to allow ordinary users to execute some or all root commands. It can reduce the login and management time of root users and improve security.
-
Apt is a package management tool that can install, update, delete and manage deb packages.
-
export is an instruction to configure environment variables.
1.1.2 Add source
The Ubuntu operating system can use the terminal command to install the software directly. When executing this command, the Ubuntu system will find the /etc/apt/sources.list file in the file system, or in the /etc/apt/sources.list.d/ folder independent file. These files are called source lists, and the address information called sources is stored in the source list. The Ubuntu system finds the software available for download on the corresponding source server according to the address information, and then downloads and installs it.
In short, in Ubuntu, source refers to the address where software can be obtained. The purpose of adding sources before installing ROS2 is to tell the Ubuntu system where to download ROS2. The terminal operation steps for adding source officially provided by ROS2 are as follows. Open the terminal and enter the following command:
sudo apt install software-properties-common # Install tools to add source sudo add-apt-repository universe # Add universe source warehouse sudo apt update && sudo apt install curl gnupg lsb-release #Update package list and install curl,pnupg,lsb -release sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg # Authorize GPG key 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 # Add ros2 apt repository to sources list
Notes:
-
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中的每个节点应负责单个模块目的，用于实现某一具体任务。一个完整的机器人系统由许多协同工作的节点组成。
In the figure below, these circles represent some of the nodes of a robot, and these nodes have their own tasks, for example, one node is used to control the motor, one node is used to control the range finder, etc. Nodes do not exist independently, and nodes need to communicate with each other to work better. The connection represents the channel of communication, which can be understood as the network cable in life. The box represents the way of communication between nodes. For example, in daily life, we can communicate by calling, then the communication method is voice; we can also communicate by sending text messages, then the communication method is text. The small dots and ovals represent the specific content of communication. For example, we can call friends by voice, or send text messages to friends. Different communication methods correspond to different types, so small dots and small ellipses are used in the figure to distinguish them.
2.2 Node Features
Nodes are used to perform certain specific tasks. Assigning a specific function to a specific node is conducive to the modularization of the robot and will greatly improve the reusability of the node. Except for non-essential data transmission between nodes, a good low-coupling and high-cohesion state should be maintained.
Nodes are executables that can be run independently. Even if a node does not receive or send data, it can operate independently. Developers can run a single node at any time, just like turning on its power, and whether it can work normally depends on whether it obtains valid data.
Each node needs to have a unique name. For example, the two nodes /teleop_turtle and /turtlesim we run have their own unique names, otherwise the created robot will be unable to tell the difference between the left and right hands, which is unreasonable.
Nodes can be distributed. For example, node a of a robot is located on computer A, but node b is located on computer B. When building collaborative projects and large-scale projects, the distributed nature of nodes can play a huge role.
Each node can send and receive data to other nodes through communication mechanisms such as topics, services, and actions. Topics, actions, and services are the ways for nodes to communicate.
2.3 Node Instructions
Next, we use the turtle robot to understand the terminal commands related to nodes. First, according to the previous tutorial, open a new terminal and run turtle:
ros2 run turtlesim turtlesim_node # run turtlesim
Open a new terminal and run the turtle controller:
ros2 run turtlesim turtle_teleop_key # Run the turtle controller
2.3.1 ros2 node list
This command is used to view the node list, which will display the names of all running nodes. Open a new terminal and enter the following command:
ros2 node list # View node list
The result is as follows:
/teleop_turtle /turtlesim
It can be seen that when the command ros2 run turtlesim turtlesim_node is entered in the terminal, ROS2 will run the executable file turtlesim_node, thus starting a node named /turtlesim. Similarly, when the command ros2 run turtlesim turtle_teleop_key is entered in the terminal, ROS2 will run the executable file turtle_teleop_key, thus starting a node named /teleop_turtle.
2.3.2 ros2 node info node_name
After the node starts, you can use the terminal command to view the details of the node. Open a new terminal and enter the following command:
ros2 node info /turtlesim # View the details of the /turtlesim node, which will display the identity of the node
You can see the following output:
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
This command is used to view the information of a topic, and will display the message type and the number of publishers and subscribers of the topic. Open a new terminal and enter the following command:
ros2 topic info /turtle1/pose # View topic information
The result is as follows:
Type: turtlesim/msg/Pose Publisher count: 3 Subscription count: 0
Note that the message type of topic /turtle1/pose is turtlesim/msg/Pose, there are 3 nodes publishing this topic, and 0 nodes subscribing to this topic.
3.3.4 ros2 topic echo topic_name
This command is used to view the specific data transmitted by a topic. Open a new terminal and enter the following command:
ros2 topic echo /turtle1/pose # View topic data
The result is as follows:
x: ... y: ... theta: ... linear_velocity: ... sngular_vvelocity: ... --- x: ... y: ... theta: ... linear_velocity: ... angular_velocity: ... --- ...
It can be seen that the terminal outputs the position coordinates, rotation angle, linear velocity, and angular velocity of the turtle in the simulation window in real time. Therefore, it can also be judged that the specific data transmitted by the topic /turtle1/pose is the pose of the turtle.
3.3.5 ros2 topic hz topic_name
This command is used to view the posting frequency of a topic. Open a new terminal and enter the following command:
ros2 topic hz /turtle1/pose # Check the topic release frequency
The result is as follows:
average rate: ... min: ... max: ... std dev: ... window: ... average rate: ... min: ... max: ... std dev: ... window: ... ...
3.3.6 ros2 topic bw topic_name
This command is used to view the transmission bandwidth of a topic. Open a new terminal and enter the following command:
ros2 topic bw /turtle1/pose # View topic transmission bandwidth
The result is as follows:
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
This command is used to publish topic messages. Open a new terminal and enter the following command:
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}}" # Publish topic message, --once is an optional parameter, which means to publish a message and then exit. If you want to publish at frequency n HZ, use --rate n
The result is as follows:
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 # Run the turtle controller
4.3.1 ros2 service list
This command is used to view the service list. Open a new terminal and enter the following command:
ros2 service list # View service list
The result is as follows:
/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
So far, we have gradually understood that the function of the action /turtle1/rotate_absolute is to request the turtle to rotate a certain angle, and give real-time feedback on the progress of the completion, and the final returned result is the absolute angle of the turtle.
6 communication interface
6.1 Concept of communication interface
In the previous tutorials, readers have learned the concepts of topics, services, and actions, which are collectively referred to as the communication interface of ROS2. The communication interface is like the way of communication in our life. Through the message type of each communication interface, various nodes can be organically connected together.
6.2 Message types of the communication interface
In the previous article, we learned about the existence of these three communication interfaces through the terminal command of the turtle robot. Each communication interface has its own message type, which is saved and called in the form of a file. A message type is similar to a programming language structure, which is a container for carrying specific data, and a message type can be used by multiple communication interfaces. In order to improve the development efficiency of ROS2 users, ROS2 provides many standard message types. The files of these standard message types can be found in the ros/humble/share folder in the ROS2 installation path.
You can also create a custom message type file according to your own needs.
6.2.1 Topic message types
Open the /opt/ros/humble/share/turtlesim/msg directory, and you can see that there are two files with the suffix .msg, namely Color.msg and Pose.msg. Among them, Pose.msg is a message type file of topic class, and Pose is called the message type of topic turtle1/pose. Open the file and view the content of the Pose message type as follows:
float32 x float32 y float32 theta float32 linear_velocity float32 angular_velocity
As you can see, the message type of a topic is like the structure of a programming language. First specify the data type of the variable, then specify the name of the variable, and finally multiple variables form the message type. That is to say, the topic turtle1/pose pushes the pose information of the turtle, which consists of abscissa x, ordinate y, rotation angle theta, linear_velocitylinear_velocity, and angular velocity angular_velocity. These five pieces of data are all 32-bit floating-point types. Their values can vary.
The message type file suffix of the topic class is .msg.
6.2.2 Service message types
打开/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 动作的消息类型
Open the /opt/ros/humble/share/turtlesim/action directory, and you can see a file named RotateAbsolute.action. RotateAbsolute.action is an action message type file, and RotateAbsolute is the message type of the action /turtle1/rotate_absolute. Open the file and view the content of its message type as follows:
# 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
At the end of the Actions section, we've seen what /turtle1/rotate_absolute does. Among them, theta is the target of the request, specifically the angle at which the turtle is requested to rotate; delta is the returned result, specifically the final absolute angle of the turtle; remaining is the real-time feedback, specifically the angle that still needs to be rotated.
Different from services, actions have three types of data: target, feedback, and result. It can be seen that in its message type, the request, feedback, and target data are separated by "---", the requested data is defined above the first "---", and the feedback data is defined above the two "- --" defined between, the returned result data is defined under the second "---".
The message type file suffix of the action class is .action.
6.2.4 Nested data types
Nested data type means that a message type file contains other message type files, which is equivalent to nesting other structures in a structure in C language. Nested data types help to avoid redefinition and improve reusability. For example, now you need to use a topic message type file named B in a service message type file named B, you need to create two files and fill in the following content:
-
A.msg
uint32 a uint8 b
-
B.srv
# Request A c --- # Response bool status
6.3 Features of communication interface
The characteristics of the communication interface depend on the message type file it uses. The message type file can be nested. The characteristics of each message type are as follows:
The suffix of the topic message type file is .msg. Since it is a one-way transmission, only a single data needs to be defined.
The suffix of the service message type file is .srv, which includes request and return parts, which are distinguished by "---" in the file.
The suffix of the action message type file is .action, which includes three parts: request, feedback, and result, which are distinguished by "---" in the file.
6.4 Communication Interface Instructions
Next, we use the turtle robot to understand the terminal commands related to the communication interface. First, according to the previous tutorial, open a new terminal and run turtle:
ros2 run turtlesim turtlesim_node # run turtlesim
Open a new terminal and run the turtle controller:
ros2 run turtlesim turtle_teleop_key # Run the turtle controller
6.4.1 ros2 interface list
This command is used to view the communication interface list. Open a new terminal and enter the following command:
ros2 interface list # View the list of communication interfaces
The result is as follows:
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 Parameter commands
7.3.1 ros2 param list
This command is used to view the parameter list. Open a new terminal and enter the following command:
ros2 param list # View parameter list
The result is as follows:
/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
This command is used to query the data type and current value of a certain parameter. Open a new terminal and enter the following command:
ros2 param get /turtlesim background_g # View the type and value of the parameter
The result is as follows:
Integer value is: 86
7.3.3 ros2 param set node_name parameter_name value
This command is used to modify the value of a parameter. Open a new terminal and enter the following command:
ros2 param set /turtlesim background_g 150 # Modify parameter value
The result is as follows:
Set parameter successful
Indicates that the modification is successful.
7.3.4 ros2 param dump node_name
This command is used to save the parameters of a certain node into the parameter file. By default, the command prints to standard output (stdout), but you can also redirect parameter values to a file to save for later use. To save the current configuration of parameters to a file, open a new terminal and enter the following command:
ros2 param dump /turtlesim > turtlesim.yaml # save the parameters to the file turtlesim.yaml
You'll find a new file in the working directory where you ran the shell. If you open this file, you will see the following:
/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 Workspaces and Feature Packs
8.1 Workspace
8.1.1 Workspace concept
工作空间就是存放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功能包
Before explaining the Cmake function package, the author first briefly describes the Cmake tool. CMake is a cross-platform build tool that can describe the installation (compilation process) of all platforms with simple statements. It can output various makefiles or project files. Cmake does not directly build the final software, but generates scripts of other tools (such as Makefile), and then uses them according to the way this tool is built. Source files written in CMake are named CMakeLists.txt or have a .cmake extension.
In short, Cmake generates Makefile directly by calling CMakeLists.txt. There are countless source files in a project, which are placed in several directories according to type, function, and module. Makefile defines a series of rules to specify which files need to be compiled first, which files need to be compiled later, and which files need to be re-compiled. Compile, and even perform more complex functional operations. The commands in the Makefile file have certain specifications. Once the file is written, execute a make command on the Linux command line to automatically compile the entire project.
The Cmake function package mainly includes the following folders and files.
-
src: source code directory. The C++ (suffix .cpp) files we created ourselves are placed in this folder.
-
package.xml: This file contains meta information related to this feature package. We can open the file and fill in information such as the description of the feature pack and the license statement.
-
CMakeLists.txt: A file describing how to generate code in a feature package. The compilation tool colcon of ROS2 compiles the function package through CMakeLists.txt.
The command to create a new Cmake function package in ROS 2 is:
cd ~/ros2_ws/src # Enter the src directory ros2 pkg create --build-type ament_cmake <package_name> # Create a Cmake function package, <package_name> is the function package name
8.2.2.2 Python function package
在讲解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 工作空间与功能包
As you can see from the previous section, a single workspace can contain any number of feature packages, each in its own folder. It is also possible to include packages of different build types (CMake, Python, etc.) in one workspace, but you cannot have nested packages. The best practice is to create feature packages under the src folder of your workspace. This keeps the top layer of the workspace "clean". The directory structure of a single workspace and function packages should have the following hierarchical relationship:
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 Function packages and nodes
A feature package can contain multiple executable files, these executable files can be files for creating nodes, or script files for other purposes. A package can contain multiple nodes, and we should only place nodes in a package that are created for a common purpose. If a node is likened to cells, then a functional package is an organ made up of these cells.
There are two reasons for not putting all nodes in one function package. One is that the ROS2 function package can be packaged for others to install directly. When some functions in a function package need to be modified, other functions will not be affected. The normal use of packages is conducive to modularization and distribution deployment. The second is that the number of nodes in the function package is arbitrary. When the details of a certain function of the robot need to be optimized or perfected, it is only necessary to enter the function package that implements this function to add new nodes, which is conducive to expansion.
Taking the Python function package as an example, the Python file for creating a node is placed in a subfolder with the same name as the function package. The directory structure of a single function package and executable files should have the following hierarchical relationship:
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 参数命名
The parameters need to be defined in the program, each parameter contains a key and a value, where the key is a string and the value can be one of the following types: bool, int, double, string, byte[], bool[], int [], double[] or string[]. When the programming language is Python, the example is as follows:
self.declare_parameter('robot_name', 'mbot') # Create a parameter and set the default value of the parameter
Part II Development
10 facer designs
10.1 Background introduction
RCL (ROS Client Library) is the client library of ROS2. In fact, it is an API of ROS2, which provides interfaces for ROS2 topics, services, parameters, and actions. For the Python language, ROS2 provides the rclpy library to operate the ROS2 nodes; for C++, ROS2 provides the rclcpp library to operate the ROS2 nodes. So when we use Python to write ROS2 nodes to implement functions such as communication, we will import the rclpy library.
OpenCV (Open Source Computer Vision) is a cross-platform machine vision library launched by Intel in 1999. Focuses on real-time image processing and includes patent-free implementations of the latest machine vision algorithms. In 2008, Willow Garage took over support, and OpenCV now comes with programming interfaces for languages such as C, C++, Python, and Android. OpenCV is released under the BSD license, so it can be used in both academic projects and commercial products. OpenCV now comes with a new FaceRecognizer class for face recognition, so face recognition can be easily implemented using OpenCV.
Using ROS2 to develop projects, the author summarizes the steps that need to be carried out: the first step is to create a workspace; the second step is to create a message type; the third step is to create a node; the fourth step is to realize the node task; the fifth step is to compile Function package; the sixth step, run the node. Creating a project is not done overnight. During this process, we will continue to debug and modify, so some steps can be repeated and crossed. In addition to this, the facer robot will be modified in the terminal using the parameters of ROS2.
10.2 Functional requirements
In the previous content, we have been using the turtle robot to understand the related concepts of ROS2. After understanding the concept of how ROS2 designs robots, it's time to create a robot of your own. In the development chapter, the author will teach you how to create a robot named facer. In order to simplify the process, the function of the facer robot is simple and easy to implement, and readers don't have to worry too much about the difficulty.
The function of the facer robot is to realize simple face recognition. After facer collects the video stream of the camera, it judges whether there is a human face in the lens, and finally outputs the judgment result.
10.3 Module Division
To achieve this functionality, the program is divided into three modules:
Input module: the main function is to collect images. The video stream is composed of many frames of images. After the module collects the video stream, it needs to transmit each frame of images to the processing module. In addition, in order to allow users to better understand the working details of the facer, the input module will also monitor the running status of the camera and send it to the output module in real time.
Processing module: the main function is to recognize images. Receive the image from the input module, compare it with the data model file, and draw a conclusion. In addition, in order for us to see the conclusion on the interactive interface, the processing module will also pass the face recognition result to the output module.
Output module: the main function is human-computer interaction. Receive the camera running status information from the input module, and receive the face recognition result from the processing module. Finally, the camera running status information and face recognition results are received at the terminal output.
The relationship between module functions and modules is shown in the figure below:
10.4 Function package design
Combined with the design concept of ros2, a robot is a collection of many functions, the function package is used to realize a single function, and the nodes in the function package are used to realize a single task of a single function. According to the module division of the facer robot, correspondingly, we divide the facer into three function packages: input, processing, and output, and each function package contains a node.
A function package called facer_input is responsible for functions related to the input module. The main task of the facer_input function package is to capture images, so it contains a node named capture_image.
A function package called facer_handle is responsible for the functions related to the processing module. The main task of the facer_handle function package is to identify images, so it contains a node named identify_image.
A package called facer_output is responsible for things related to the output module. The main task of the facer_output function package is human-computer interaction, so it contains a node named visual_interaction.
The relationship between function packages and nodes is shown in the figure below:
10.5 Node identity design
Image data needs to be passed between the facer_input node and the facer_handle node. The image data comes from our computer camera. Facer needs to convert the video stream into a frame-by-frame image transmission. This process is carried out periodically. As we can see from the previous article, topics are suitable for periodically published data, so we use the communication interface of topics to transfer images. The facer_input node is the publisher of the image topic, and the facer_handle is the subscriber of the image topic. Finally, we name the image topic frame_image.
Between the facer_handle node and the facer_output node, the result of face recognition needs to be passed. Face recognition is realized by calling OpenCV face recognition related functions. Machine vision is the same as human beings. Recognition takes time and can only be passed after the face recognition results come out. As we can see from the previous article, the service is suitable for data requested on demand. After the server receives the request, it will process it and return the response information. Therefore, we use the communication interface of the service to transmit the face recognition result. Finally, we named the service face_result.
Between the facer_output node and the facer_input node, what needs to be passed is the camera state. There are two types of camera status: on and off. Before the facer robot makes a face recognition result, the camera should be kept on and output the status in real time. This is an event that needs to last for a period of time. As we can see from the previous article, actions are suitable for running time-consuming tasks. After the server receives the request, it will feedback the task status in real time, so we use the communication interface of actions to transfer the camera status. Finally, we named our action camera_status.
In summary, the identity of a node is as follows:
The capture_image node is the publisher of the frame_image topic and the server of the camera_state action.
The identify_image node is a subscriber of the frame_image topic and a client of the face_result service.
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等编辑器，还可以使用终端指令直接创建多级文件夹。
Open the terminal, enter the user's home directory to create a workspace named facer_ws, and create a src folder under this workspace directory. Enter the following command:
mkdir -p ~/facer_ws/src # Create ~/facer_ws/src multi-level directory
Note: ~ represents the user's home directory, for example, the absolute path of the author's ~ directory is /home/sipeiliu; the mkdir command is used to create a directory, and the -p parameter can make it create a multi-level directory.
After the facer workspace is created, it should have the following directory hierarchy:
do_ws src
12 Create a communication interface
In ROS2, nodes use communication interfaces such as topics, services, and actions by importing their message type files. For example, Python uses import statements. Before creating topics, services, and actions, it is best to prepare their message type files. Although ROS2 officially recommends developers to use the standard message type files provided by it, these standard message type files often cannot fully meet the needs, so custom message type files are usually created in the project.
12.1 Create communication interface function package
In order for ROS2 to find the imported custom message type file, the created custom message type file needs to be placed in a Cmake function package, which is usually called a communication interface function package. Please note that the communication interface function package is a CMake function package. Currently, there is no way to use the pure Python function package to implement the communication interface function. The custom message type files created in the CMake function package can be imported and used in the Python function package. ROS2 provides terminal commands for creating function packages. Next, let's create a Cmake function package for storing custom message type files. Open the terminal and enter the following command to enter the facer_ws/src folder:
cd ~/facer_ws/src # Enter the facer_ws/src directory
After entering the facer_ws/src folder, enter the following command to create a communication interface function package for storing message types:
ros2 pkg create --build-type ament_cmake facer_interfaces # Create Cmake function package facer_interfaces
Note: pkg indicates the function related to the function package, create indicates the creation of the function package, --build-type indicates the language environment of the newly created function package, ament_cmake refers to the creation of the Cmake function package, the language environment is C++, and finally keeps up with the newly created function package Name facer_interfaces.
12.2 Create a message type file
Under the directory of the communication interface function package, create three new folders named msg, srv, and action respectively. The meanings of msg, srv, and action are message, service, and action respectively. From this, we can see that these three folders are used to store message type files (suffix .msg) of topic communication interfaces and message type files of service communication interfaces respectively. (the suffix is .srv), and the message type file of the action communication interface (the suffix is .action).
Open the terminal and enter the following command to enter the communication interface function package directory:
cd ros2_ws/src/facer_interfaces # Enter the communication interface function package directory
Enter the following command to create the msg folder:
mkdir msg # create msg folder
Enter the following command to create the srv folder:
mkdir srv # create srv folder
Enter the following command to create the action folder:
mkdir action # create action folder
Or directly enter the following command:
mkdir msg srv action # Create msg, srv, action folders
After creating the function package and folder, you can finally create a custom message type file. The facer robot has three communication interfaces: a topic named frame_image, which is used to transmit images frame by frame; a service named face_result, which is used to transmit face recognition results; an action named camera_state, which is used to transmit the camera state. Each communication interface has its own message type file, which will be explained and created one by one.
12.2.1 Topic message type file
The frame_image topic is the communication interface between the capture_image node responsible for capturing images and the identify_image node responsible for identifying images, and is used to transmit images frame by frame. The message type file of the frame_image topic uses the standard message type file Image.msg, which can be directly imported and used. Although the message type of the topic in this case does not need to be created by ourselves, we still created the folder msg to store the custom message type file of the topic class in the previous article, because one day if we want to modify the function of the robot, we may need to create a topic class message type file. Image.msg can be found in the ros/humble/share/sensor_msgs/msg folder in the ROS2 installation path, and the file is opened as follows:
# 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 Service message type file
The face_result service is the communication interface between the identify_image node responsible for identifying images and the visual_interaction node responsible for human-computer interaction, and is used to deliver face recognition results. The message type file of the face_result service uses the custom message type file FaceResult.srv. Open the terminal and enter the following command to enter the srv folder of the communication interface function package:
cd ~/facer_ws/src/facer_interfaces/srv # enter the srv folder
Enter the following command to create a custom message type file FaceResult.srv:
touch FaceResult.srv
At this point, the content of the file is empty, use an editor to open the file and fill in the following content:
bool apply_result # application result --- string get_result # get result
The apply_result variable of Boolean type above "---" is the request data of the service. When its value is True, the server-side identify_image node will accept the application of the client-side visual_interaction node and deliver the face recognition result. How to implement it with code will be explained later.
The get_result variable of string type under "---" is the feedback data of the service. The server-side identify_image node stores the face detection result in the get_result variable and returns it to the client-side visual_interaction node. How to implement it with code will be explained later.
12.3.3 Action message type file
The camera_state action is the communication interface between the visual_interaction node responsible for human-computer interaction and the capture_image node responsible for image capture, and is used to transmit the camera state. The message type file of the camera_state action uses the custom message type file CameraState.action. Open the terminal and enter the following command to enter the action folder of the communication interface function package:
cd ~/facer_ws/src/facer_interfaces/action # enter the srv folder
输入以下指令，创建自定义消息类型文件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(<dependency> 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(<dependency> 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 Add dependencies
Because the communication interface depends on rosidl_fault_generator to generate language-specific code, a dependency on it needs to be declared. The <exec_depend> tag is used to specify runtime or execution phase dependencies, while rosidl_interface_packages is the name of the dependency group to which the feature package belongs, declared using the <member_of_group> tag. Therefore, the package.xml file of the communication interface function package facer_interfaces also needs to add code generation function dependencies. Open the package.xml file and enter the following:
<depend>geometry_msgs</depend> # Add the function packages that the above message types depend on. In this example, add geometry_msgs to DemoMsg.msg. If not, do not fill in this line of code. <build_depend>rosidl_default_generators</build_depend> <exec_depend>rosidl_default_runtime</exec_depend> <member_of_group>rosidl_interface_packages</member_of_group>
12.3 Check the communication interface list
Now let's briefly learn how to view the list of communication interfaces that we just customized on the terminal to check whether we have successfully created the communication interface.
Open the terminal and enter the following command to enter the facer_ws workspace:
cd ~/facer_ws # enter the workspace
After compiling the communication interface function package facer_interfaces, ROS2 can discover the communication interface stored in it. The details of compiling the function package will be explained later, now enter the following command to compile the function package:
colcon build # compile function package
若在工作空间生成了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 创建节点功能包
Because the executable file (*.py) of Python is placed in the subfolder of the same name as the Python function package, you first need to create the Python function package. ROS2 provides terminal instructions for creating Python function packages. In order to simplify the operation and make the logic clear, we first create all three function packages of the facer in one go.
Open the terminal and enter the following command to enter the src directory of the workspace:
cd ~/facer_ws/src # The function package is placed in the src directory
Enter the following command to create the facer_input function package:
ros2 pkg create --build-type ament_python facer_input #Create function package facer_input
Enter the following command to create the facer_handle function package:
ros2 pkg create --build-type ament_python facer_handle #Create function package facer_handle
Enter the following command to create the facer_output function package:
ros2 pkg create --build-type ament_python facer_output #Create function package facer_output
Note: pkg indicates the function related to the function package, create indicates the creation of the function package, --build-type indicates the language environment of the newly created function package, ament_python refers to the creation of the Python function package, the language environment is Python, and finally keeps up with the newly created function package name.
Then open the package.xml and setup.py of each function package, and make sure to fill in the function package description, maintainer information, license and other relevant information.
facer_input function package:
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_input</name> <version>0.0.0</version> <description>采集图像</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
Similarly, open the setup.py of the facer_input function package, the name of the maintainer, the email address of the maintainer, and the description of the function package are consistent with those filled in 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功能包：
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_handle</name> <version>0.0.0</version> <description>识别图像</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
Similarly, open the setup.py of the facer_input function package, the name of the maintainer, the email address of the maintainer, and the description of the function package are consistent with those filled in 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 function package:
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_output</name> <version>0.0.0</version> <description>人机交互</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
同样的，打开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': [
],
},
)
So far, the directory hierarchy is as follows:
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
Note: The Apache License is the agreement adopted by the famous non-profit open source organization Apache. This agreement is similar to BSD, which also encourages code sharing and respects the copyright of the original author, and also allows code modification and re-release (as open source or commercial software). The conditions that need to be met are also similar to BSD: users of the code need to be given an Apache License. If you modify the code, you need to explain it in the modified file. In extended code (both modified and derived from the source code) licenses, trademarks, patent notices, and other descriptions specified by the original author are required to be included in the original code. If the redistributed product contains a Notice file, the Apache License must be included in the Notice file. You can add your own permission to the Notice, but it cannot be expressed as a change to the Apache License. If readers want to modify to other open source agreements according to the nature of their projects in the future, or learn more about open source licenses, please visit:
https://opensource.org/licenses
13.2 Creating Nodes
With the feature pack in place, it's time to create the node. The node is implemented by calling the ROS2 client rclpy library function, which is divided into steps such as creating an executable file, initializing the programming interface, creating and initializing the node, destroying the node and closing the interface, and adding dependencies and entry points. Before creating the three nodes of the facer, let's explain these steps one by one.
13.2.1 Creating executable files
Usually we only create one node in an executable file, and name the executable file name as the node name. First enter the subfolder with the same name as the function package, open the terminal and enter the following command to create an executable file named node_name:
touch node_name.py # Create an executable named node_name
Note: The touch command is used to create files of any format.
The created executable file is an empty file. We open it to write code. In order to improve the readability of the code, we can comment the relevant information of the node at the beginning of the file. As follows:
""" @node author: slightly @node identity: slightly @node task: slightly """
13.2.2 Programming Interface Initialization
First, you need to import the ROS2 client library rclpy and the node class to use the related functions provided by it.
""" @node author: slightly @node identity: slightly @node task: slightly """ '''Add code''' # ros2 library import rclpy # ros2 python interface library import rclpy.node import Node # ros2 node class
Then at the end of the file, add the following code.
"""
@node author: slightly
@node identity: slightly
@node task: slightly
"""
# ros2 library
import rclpy # ros2 python interface library
import rclpy.node import Node # ros2 node class
'''new code'''
if __name__ == '__main__':
main()
Some readers may not be clear about the meaning of these two lines of code, let me explain in detail below. Each python executable file has a built-in attribute name . If the executable file is executed as a top-level program file, the value of the built-in attribute name is main . These two lines of code can make the main() function execute; if the executable file is executed by other programs For import use, the value of name is the module name, and these two lines of code can prevent the main() function from being executed. This way of writing is very common in Python, and the advantage of writing this way is to make this executable file the main program.
The main() function is the main entry function of the node. Next, we write the main entry function and initialize the Python interface of ROS2. Since no parameters are passed in, we fill in args=None.
"""
@node author: slightly
@node identity: slightly
@node task: slightly
"""
# ros2 library
import rclpy # ros2 python interface library
import rclpy.node import Node # ros2 node class
'''new code'''
def main(args=None): # ros2 node main entry main function
rclpy.init(args=args) # ros2 python interface initialization
# Make the module run as the top-level program file, and it will not run when the module is imported
if __name__ == '__main__':
main()
13.2.3 Create node and initialize
After initializing the Python interface of ROS2, you can use the functions provided by it to create nodes. We use the object-oriented programming method, first create a node class, and then inherit the node function provided by the Python interface of ROS2, the node name node_name is used as the input parameter, the code is as follows:
"""
@node author: slightly
@node identity: slightly
@node task: slightly
"""
# ros2 library
import rclpy # ros2 python interface library
from rclpy.node import Node # ros2 python interface library node class
class NodeClass(Node):
'''New code'''
def __init__(self):
super().__init__('node_name') # ros2 node parent class initialization, node name is node_name
def main(args=None): # ros2 node main entry main Function
rclpy.init(args=args) # ros2 python interface initialization
# Make the module run as the top-level program file, but it will not run when the module is imported
if __name__ == "__main__":
main()
Objects are instances of classes, now let's create a node object:
"""
@节点作者：略
@节点身份：略
@节点任务：略
"""
# 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接口。
"""
@node author: slightly
@node identity: slightly
@node task: slightly
"""
# ros2 library
import rclpy # ros2 python interface library
from rclpy.node import Node # ros2 python interface library node class
class NodeClass(Node):
def __init__(self):
super().__init__('node_name') # ros2 node parent class initialization, node name is node_name def
main(args=None): # ros2 node main entry main function
rclpy.init(args=args) # ros2 python interface initialization
node = NodeClass() # Create ros2 node object and initialize
'''new code'''
node.destory_node() # Destroy node object
rclpy.shutdown() # Close ros2 python interface
# Make the module is The top-level program file can run, but it will not run when the module is imported
if __name__ == "__main__":
main()
13.3.5 Add dependencies and entry points
13.3.5.1 Add dependencies
Open the package.xml of the function package, and add the library rclpy that creates node dependencies:
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>demo</name> <version>0.0.0</version> <description>demo</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
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：
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_input</name> <version>0.0.0</version> <description>采集图像</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
Open the setup.py of the faer_input function package, and add the node main entry function location in the brackets of the 'console_scripts' field of entry_points.
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 Create identify_image node
Open the terminal and enter the following command to enter the ~/ros2_ws/src/facer_handle/facer_handle folder:
cd ~/ros2_ws/src/facer_handle/facer_handle # enter the facer_input folder
Enter the following command to create a python file named identify_image.py in this directory:
touch identify_image.py # create python file
Write code
""" @Author: [email protected] @Node Identity: Subscriber of frame_image topic, server of face_result service @Node Task: Recognize Image """ # ros2 library import rclpy # ros2 python interface library from rclpy.node import Node # ros2 node class """Identify image node class""" class IdentifyImage(Node): """Constructor""" def __init__(self): super().__init__('identify_image') # ros2 node parent class Initialization, the node name is identify_image '''the relevant code of the subscriber of the frame_image topic''' # temporarily omit the relevant code of the server side of the '''face_result service''' # temporarily omit def main(args=None): # ros2 node The main entry main function rclpy.init(args=args) # ros2 python interface initialization node = IdentifyImage() # Create ros2 node object and initialize rclpy.spin(node) # Loop and wait for ros2 to exit node.destory_node() # Destroy the node object rclpy.shutdown() # Close the ros2 python interface # Make the module run as the top-level program file, but it will not run when the module is imported if __name__ == "__main__": main()
Add dependencies and entry points
Open the package.xml of the facer_handle function package, and add the library rclpy that creates node dependencies:
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_handle</name> <version>0.0.0</version> <description>识别图像</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
打开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：
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_output</name> <version>0.0.0</version> <description>人机交互</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
打开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 View node list
Now let's briefly learn how to view the newly created node on the terminal to check whether we have successfully created the node.
Open the terminal and enter the following command to enter the facer_ws workspace:
cd ~/facer_ws # enter the workspace
Because we have modified the content in the function package, we need to recompile, enter the following command to compile the function package:
colcon build # compile function package
Now, we make all three nodes running, and only the running nodes can be found by the following watch list command.
Only by setting the environment variable can the terminal discover the node function package, enter the following command to set the environment variable:
source install/setup.bash # Set environment variables
Enter the following command to run the capture_image node of the facer_input function package:
ros2 run facer_input capture_image # run capture_image node
After running the capture_image node, this terminal is occupied by the capture_image node. So we need to open a new terminal and enter the following command to set the environment variable:
source install/setup.bash # Set environment variables
Enter the following command to run the identify_image node of the facer_handle function package:
ros2 run facer_handle identify_image # run identify_image node
Similarly, after running the identify_image node, this terminal is occupied by the identify_image node. So we need to open a new terminal and enter the following command to set the environment variable:
source install/setup.bash # Set environment variables
Enter the following command to run the visual_interaction node of the facer_output function package:
ros2 run facer_output visual_interaction # Run visual_interaction node
最后，打开一个新的终端，终于可以输入以下指令查看节点列表了：
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 Implement node tasks
In our case, the facer needs to use the camera of the computer. If it is operated in a virtual machine, the following settings need to be made:
-
Set the virtual machine to be compatible with USB3.1. Virtual Machine - Settings - Hardware - USB Controller - USB Compatibility
-
Connect the camera to the virtual machine in the removable device. Virtual Machine - Removable Devices - Options for Camera Devices
The task of a node is usually to process data from a communication interface. In ROS2, data is transmitted through three communication interfaces: topics, services, and actions. From the perspective of the communication interface, the node processes the data after receiving the data, then releases new data, and creates a communication interface to realize a node task, which is the process of realizing the node task. facer has three communication interfaces: the frame_image topic for transferring images, the face_image service for transferring face recognition results, and the camera_state action for transferring camera status. Next, we will create and implement corresponding node tasks in turn.
After implementing the node task, you also need to declare the dependency on the system package in the package.xml file. If the statement is missing or wrong, it may not affect the local compilation and operation, but when you publish it to the ROS community and share it with others, it may be difficult for other users to compile based on your package.xml information. So we better add these dependencies to package.xml file.
14.1 Create frame_image topic
14.1.1 Create the publisher of the frame_image topic
Open the executable capture_image.py in the facer_input function package and add the following content:
""" @Author: [email protected] @Node identity: topic publisher of frame_image, server of camera_state action @node task: image collection """ # ros2 library import rclpy # ros2 python interface library from rclpy.node import Node # ros2 node class from cv_bridge import CvBridge # ros2 and OpenCv image conversion class # Other library import cv2 # OpenCv image processing library import time # Time library # Import message type from sensor_msgs.msg import Image # Image message type """ collection Image node class """ class CaptureImage(Node): """Constructor""" def __init__(self): super().__init__('capture_image') # ros2 node parent class initialization, node name is capture_image ''' The relevant code of the publisher of the frame_image topic''' self.cap = cv2.VideoCapture(0)# Create video capture object 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，添加以下内容：
""" @Author: [email protected] @Node Identity: Subscriber of frame_image topic, server of face_result service @Node task: Recognize image """ # ros2 library import rclpy # ros2 python interface library from rclpy.node import Node # ros2 node class from cv_bridge import CvBridge # ros2 and OpenCv image conversion class # Other libraries import cv2 # OpenCv image processing library # Import message type from sensor_msgs.msg import Image # Image message type """Identify image node class"" " class IdentifyImage(Node): """Constructor""" def __init__(self): super().__init__('identify_image') # ros2 node parent class initialization, node name is identify_image '''frame_image topic subscriber Related Code''' # Create a topic subscriber object (message type, topic name, subscriber callback function, queue length) 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() # Close the ros2 python interface # Make the module run as the top-level program file, but it will not run when the module is imported if __name__ == "__main__": main()
Note: The data model file uses haarcascade_frontalface_default.xml. It can be found under the \data\ haarcascades directory in the OpenCV installation directory. The Haar feature classifier is an XML file that describes the Haar feature values of various parts of the face, including nose, eyes, lips, and so on. Data model files can also be found at the following address:
https://github.com/opencv/opencv.git
14.1.3 Add dependencies
cv_bridge can convert the image message type of ROS2 and the image type collected by OpenCv to each other. In order to realize the image processing, the machine vision library OpenCv is used, which is a dependency of image processing. The frame_image topic is the communication interface between the capture_image and identify nodes. We use the standard communication interface Image. Image.msg can be found in the ros/humble/share/sensor_msgs/msg folder in the ROS2 installation path, which is a dependency of the topic .
Open the package.xml of the function package facer_input, we will add these dependencies to the package.xml file. Add the following:
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_input</name> <version>0.0.0</version> <description>采集图像</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <exec_depend>cv_bridge</exec_depend> <exec_depend>python-opencv</exec_depend> <exec_depend>sensor_msgs</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
Open the package.xml of the function package facer_handle and fill in the following content:
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_handle</name> <version>0.0.0</version> <description>识别图像</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <exec_depend>cv_bridge</exec_depend> <exec_depend>python-opencv</exec_depend> <exec_depend>sensor_msgs</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package> <description>demo</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license>
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 # Set environment variables
Enter the following command to run the capture_image node of the facer_input function package:
ros2 run facer_input capture_image # run capture_image node
After running the capture_image node, this terminal is occupied by the capture_image node. So we need to open a new terminal and enter the following command to set the environment variable:
source install/setup.bash # Set environment variables
Enter the following command to run the identify_image node of the facer_handle function package:
ros2 run facer_handle identify_image # run identify_image node
Finally, you can finally enter the following command to view the list of topics:
ros2 topic list # View topic list
In the list output by the terminal, you will see that the topics we created are as follows:
/frame_image
14.2 Create identify_result service
14.2.1 Create a server for the identify_result service
Open the executable file identify_image.py in the facer_handle function package and add the following content:
""" @作者:[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() # Create an image conversion object, which can convert the image message type of ros2 to the image collected by OpenCv # Load the face data model, create a face classifier object, pay attention to fill in the absolute path self.face_Classifier = cv2.CascadeClassifier('/home/sipeiliu/facer_ws/src/facer/haarcascade_frontalface_default.xml') self.result = 'no Face detected' # There is no face by default '''face_result server related code''' # Create a service server object (message type, service name, server callback function) self.srv = self.create_service(FaceResult , 'face_result', self.face_result_callback) '''Topic subscriber callback function''' 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('Waiting for the server of the service face_result to run...') # Create a service message type object self.req = FaceResult.Request() ''' action camera_state client Related code''' # Temporarily omitted '''send service request function''' def send_request(self, apply_result): self.req.apply_result = apply_result # Set the application result to true, so that the server can judge that the test result can be returned self .future = self.cli.call_async(self.req) # Send service request asynchronously rclpy.spin_until_future_complete(self, self.future) # Block until self.future completes return self.future.result() # Return result def main( args=None): # ros2 node main entry main function rclpy.init(args=args) # ros2 python interface initialization node = VisualInteraction() # Create a ros2 node object and initialize it time.sleep(3) # Wait for 3 seconds before sending a service request response = node.send_request(True) # Get the detection result returned by the service print(reponse.get_result) future = node.send_goal(True) # Get the result returned by the action rclpy.spin(node) # Loop and wait for ros2 to exit node.destory_node() # Destroy the node object rclpy.shutdown() # Close the ros2 python interface # Make the module the top-level program file Can run, it will not run when importing modules if __name__ == "__main__": main()
14.2.3 Add dependencies
The identify_image node of the facer_handle function package and the visual_interaction node of the facer_output function package depend on FaceResult of facer_interfaces.srv.
Open the package.xml of the function package facer_handle, we will add the dependencies to the package.xml file. Add the following:
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_handle</name> <version>0.0.0</version> <description>识别图像</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <exec_depend>cv_bridge</exec_depend> <exec_depend>python-opencv</exec_depend> <exec_depend>sensor_msgs</exec_depend> <exec_depend>facer_interfaces.srv</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package> <description>demo</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license>
Open the package.xml of the function package facer_output, we will add the dependencies to the package.xml file. Add the following:
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_output</name> <version>0.0.0</version> <description>人机交互</description> <maintainer email="[email protected]">liusipei</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <exec_depend>facer_interfaces.srv</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
14.2.4 View service list
Now let's briefly learn how to view the face_result service just created on the terminal to check whether we have successfully created the face_result service.
Open the terminal and enter the following command to enter the facer_ws workspace:
cd ~/facer_ws # enter the workspace
Run rosdep in the root directory of the workspace facer_ws to check for missing dependencies by typing:
rosdep install -i --from-path src --rosdistro humble -y
Because we have modified the content in the function package, we need to recompile, enter the following command to compile the function package:
colcon build # compile function package
Only the services in use can be found by the view service command later, so we need to run the identify_image node and visual_interaction node first.
Set the environment variable to let the terminal discover the node function package, enter the following command to set the environment variable:
source install/setup.bash # Set environment variables
Enter the following command to run the identify_image node of the facer_handle function package:
ros2 run facer_handle identify_image # run identify_image node
After running the identify_image node, this terminal is occupied by the identify_image node. So we need to open a new terminal and enter the following command to set the environment variable:
source install/setup.bash # Set environment variables
Enter the following command to run the visual_interaction node of the facer_output function package:
ros2 run facer_output visual_interaction # Run visual_interaction node
Finally, you can finally enter the following command to view the list of services:
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 # Pass camera state goal_handle.publish_feedback(feedback_msg) # Send periodic feedback self.count = self.count + 1 time.sleep(0.5) # Feedback every 0.5 seconds goal_handle.succeed() # Indicates The target is successful, otherwise it will warn that the target state is not set result = CameraState.Result() # The result of instantiating the action result.finish = 'action completed' # Assign a value to the finish of the action result, the action is completed return result # return the action result ''' ros2 node main entry main function''' def main(args=None): rclpy.init(args=args) # ros2 python interface initialization node = CaptureImage() # Create ros2 node object and initialize rclpy.spin(node) # Loop and wait for ros2 to exit node.destory_node() # Destroy the node pair rclpy.shutdown() # Close the ros2 python interface # Make the module run as the top-level program file, but it will not run when the module is imported if __name__ == "__main__": main()
14.3.2 Create a client for the camera_state action
Open the executable file visual_interaction.py in the facer_output function package and add the following content:
""" @Author: [email protected] @Node identity: client of face_result service, client of camera_status action @Node task: human-computer interaction """ # ros2 library import rclpy # ros2 python interface library from rclpy.node import Node # ros2 node class from rclpy.action import ActionClient # Action client library # Other library import time # Time library # Import message type from facer_interfaces.srv import FaceResult # Custom detection result message type from facer_interfaces.action import CameraState # Self Define camera status message type """Human-computer interaction node class""" class VisualInteraction(Node): """Constructor""" def __init__(self): super().__init__('visual_interaction') # ros2 node parent class initialization,The node name is visual_interaction '''service face_result client related code''' # Create a service client object (message type, service name) self.cli= self.create_client(FaceResult, 'face_result') # Check whether the server is running every second, if not, wait while not self.cli.wait_for_service (timeout_sec=1.0): print('Waiting for the server of the service face_result to run...') # Create a service message type object self.req = FaceResult.Request() '''Code related to the action camera_state client''' self. _action_client = ActionClient(self, CameraState, 'camera_state') '''Send service request function''' def send_request(self, apply_result): self.req.apply_result = apply_result # Set the application result to true, so that the server can judge it as OK Return the detection result self.future = self.cli.call_async(self.req) # Send service request asynchronously 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 node main entry main function rclpy.init(args=args) # ros2 python interface initialization node = VisualInteraction() # create ros2 node object and initialize it time.sleep(3) # Wait for 3 seconds before sending a service request response = node.send_request(True) # Get the detection result returned by the service print(reponse.get_result) future = node.send_goal(True) # Get the result returned by the action rclpy .spin(node) # Loop waiting for ros2 to exit node.destory_node() # Destroy the node object rclpy.shutdown() # Close the ros2 python interface # Make the module the top-level program file can run, and it will not run when the module is imported if __name__ = = "__main__": main()
14.3.3 Add dependencies
The visual_interaction node of the facer_output function package and the capture_image node of the facer_input function package depend on the CameraState of facer_interfaces.action.
打开功能包facer_output的package.xml，我们将依赖项添加到package.xml文件中。添加以下内容：
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_output</name> <version>0.0.0</version> <description>人机交互</description> <maintainer email="[email protected]">liusipei</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <exec_depend>facer_interfaces.srv</exec_depend> <exec_depend>facer_interfaces.action</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
Open the package.xml of the function package facer_input, we will add these dependencies to the package.xml file. Add the following:
<?xml version="1.0"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="3"> <name>facer_input</name> <version>0.0.0</version> <description>采集图像</description> <maintainer email="[email protected]">sipeiliu</maintainer> <license>Apache License 2.0</license> <exec_depend>rclpy</exec_depend> <exec_depend>cv_bridge</exec_depend> <exec_depend>python-opencv</exec_depend> <exec_depend>sensor_msgs</exec_depend> <exec_depend>facer_interfaces.action</exec_depend> <test_depend>ament_copyright</test_depend> <test_depend>ament_flake8</test_depend> <test_depend>ament_pep257</test_depend> <test_depend>python3-pytest</test_depend> <export> <build_type>ament_python</build_type> </export> </package>
14.3.4 View action list
Open the terminal and enter the following command to enter the facer_ws workspace:
cd ~/facer_ws # enter the workspace
Run rosdep in the root directory of the workspace facer_ws to check for missing dependencies by typing:
rosdep install -i --from-path src --rosdistro humble -y
Because we have modified the content in the function package, we need to recompile, enter the following command to compile the function package:
colcon build # compile function package
Only the action in use can be found by the view action command later, so we need to run the visual_interaction node and capture_image node first.
Set the environment variable to let the terminal discover the node function package, enter the following command to set the environment variable:
source install/setup.bash # Set environment variables
Enter the following command to run the visual_interaction node of the facer_output function package:
ros2 run facer_output visual_interaction # Run visual_interaction node
After running the visual_interaction node, this terminal is occupied by the visual_interaction node. So we need to open a new terminal and enter the following command to set the environment variable:
source install/setup.bash # Set environment variables
输入以下指令运行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() # Instantiate the feedback of the action while goal_handle.request.start == True and feedback_count < 10 : # The action request is true and the feedback count is 10 feedback_msg.state = self.camera_state # Pass the camera state goal_handle. publish_feedback(feedback_msg) # Send periodic feedback feedback_count = feedback_count + 1 # Count the number of feedback times.sleep(0.5) # Feedback once every 0.5 seconds goal_handle.succeed() # Indicates that the goal is successful, otherwise it will warn that the goal state is not set result = CameraState .Result() # instantiate the result of the action result.finish = 'action complete' # assign a value to the finish of the action result, the action is completed return result # return the action result '''ros2 node main entry main function''' def main(args =None): rclpy.init(args=args) # ros2 python interface initialization node = CaptureImage() # Create ros2 node object and initialize rclpy.spin(node) # Loop wait for ros2 to exit node.destory_node() # Destroy the node pair rclpy.shutdown() # Close ros2 python interface # Make the module a top-level program file It can run, but it will not run when importing modules if __name__ == "__main__": main()
15.2 Add parameter description
Descriptors can also be set for parameters. Descriptors allow you to specify textual descriptions of parameters and parameter constraints, such as making them read-only, specifying ranges, etc. To achieve this, the code is modified as follows.
""" @Author: [email protected] @Node identity: topic publisher of frame_image, server of camera_state action @node task: image collection """ # ros2 library import rclpy # ros2 python interface library from rclpy.node import Node # ros2 node class from rclpy.action import ActionServer # Action client library from cv_bridge import CvBridge # ros2 and OpenCv image conversion class # Other library import cv2 # OpenCv image processing library import time # Time library # Import message type from sensor_msgs. msg import Image # Image message type from facer_interfaces.action import CameraState # Custom camera state message type """Capture image node class""" class CaptureImage(Node): """Constructor""" def __init__(self): super().__init__('capture_image') # ros2 node parent class initialization, node name is 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 # Return times feedback_msg = CameraState.Feedback() # Feedback of instantiated action while goal_handle.request.start == True and feedback_count < 10 : # Action request is true and feedback count is 10 feedback_msg.state = self.camera_state # Pass camera state goal_handle.publish_feedback(feedback_msg) # Send periodic feedback feedback_count = feedback_count + 1 # Count feedback times time.sleep(0.5) # Feedback once every 0.5 seconds goal_handle.succeed() # Indicates that the target is successful, otherwise it will warn that the target state has not been set result = CameraState.Result() # Instantiate the result of the action result.finish = 'action completed' # Assign a value to the finish of the action result, the action is completed return result # return the action result '' 'ros2 node main entry main function''' 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
The terminal output is as follows:
Parameter name: timer_period Type: double Description: this is topic publish period! Constraints:
15.3 Modify parameters
Now we know that the default value of the timer_period parameter is 0.1, and now we learn to modify its value from outside the program. There are two ways to achieve this. The first method is to use terminal commands at the console. The second method is through startup file modification.
15.3.1 Terminal command modification
Make sure the capture_image node is running:
ros2 run facer_input capture_image
Open a new terminal and set environment variables:
source ~/facer_Ws/install/setup.bash
Enter the following command to view the parameter list:
ros2 param list
The terminal will output the timer_period parameter:
/capture_image: timer_period
Now we modify the value of the timer_period parameter to 1.0, just enter the following command in the terminal:
ros2 param set /capture_image timer_period 1.0
The terminal will prompt that the setting is successful:
Set parameter successful
Due to the prolongation of the release cycle of the topic of transferring images, if you run all nodes at this time, you will find that facer execution is relatively slow.
15.3.2 Startup file modification
We can set parameters in the startup file, but first we need to add the startup folder. Open the terminal and enter the following command to enter the facer_input folder:
cd ~/make_ws/src/make_input
Enter the following command to create a folder named launch:
mkdir launch
Enter the following command to enter the launch folder:
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启动命令可用，它还确保识别所有启动文件格式：
<exec_depend>ros2launch</exec_depend>
打开终端，输入以下指令进入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 Running tests
To test the package we just compiled, run the following command:
colcon test
16.3.6 Setting Environment Variables
When colcon is successfully compiled, the functional package that can be run will be located in the installation directory install. Before any installed executable or library can be used, they need to be added to the path and library path, which we call setting environment variables. colcon will generate setup.bash and other files in the installation directory install to help set environment variables. These files will add all required elements to the PATH and LIBRARY paths and provide any bash or shell commands exported by the feature package. In short, after the function package is compiled, you need to enter the following command in each terminal to use it normally, that is, set the environment variable:
. install/setup.bash
Note: Here. has the same meaning as the source command, which means execution.
16.3.7 Try a demo
After setting the environment variables, we can run the executable compiled by colcon. Let's run the subscriber node from the example:
ros2 run examples_rclcpp_minimal_subscriber subscriber_member_function
In another terminal, let's run a publisher node (don't forget to set the environment variable):
ros2 run examples_rclcpp_minimal_publisher publisher_member_function
You should see messages from publishers and subscribers with numbers incrementing.
16.4 Create a function package
The package.xml used by colcon follows the xml specification defined in REP 149 (also supports format 2).
colcon supports compiling various types of function packages. The recommended compilation types are ament_cmake and ament_python. Pure cmake feature packages are also supported.
An example of an ament_python build is the ament_index_python package, where setup.py is the main entry point for compilation.
Functions like demo_nodes_cpp use ament_cmake to compile types and use cmake as the compilation tool.
For convenience, you can use the tool ros2 pkg create to create new feature packages based on templates.
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 技巧
If you do not want to compile a particular package, put an empty file named COLCON_IGNORE in the directory, which will not be indexed.
If you want to avoid configuring and building tests in the CMake package, you can pass: --cmakeargs-DBUILD_TESTING=0 .
If you want to compile a single function package, you can enter the following command:
colcon build --packages-select package_name # 编译package_name
In addition, to view more related command usage of colcon, enter the following command:
colcon --help
17 Add dependency tool rosdep
17.1 rosdep concept
There are three ways to use a third-party Python package in a function package: installing through rosdep, installing through a package manager, and installing through a virtual environment. The fastest of these is via rosdep, a command-line tool for installing system dependencies. rosdep can help software built from source code to install system dependencies, simplifies the problem of installing system dependencies on different platforms, and is supported on a variety of platforms/package managers.
rosdep is a dependency management tool for ROS that can be used with ROS packages and external libraries. rosdep is used to identify and install dependencies required when compiling or installing feature packages. It can be used in the following situations:
-
Compiling the workspace's feature packages requires the appropriate dependencies.
-
Check the dependencies required for execution before installing a feature package (eg sudo apt Install-ros-humble-demo-nodes-cpp).
-
It can check dependencies (such as workspaces) required by a single package or all packages in a directory.
17.2 rosdep key
The package.xml file of a feature package contains a series of dependencies. Dependencies in this file are often called rosdep keys. These rosdep keys are represented in tags <depend>, <test_depend>, <build_depend>, <build_export_depend> and <exec_depend>, which specify under what circumstances these dependencies are required:
-
<depend>: For mixing purposes, use depend. It includes build, export and execution time dependencies
-
<test_depend>: For dependencies that are used only for testing code, use test_depend.
-
<build_depend>: For dependencies that are only used to build code, use build_depend.
-
<build_export_depend>: For dependencies required by code export headers, use build_export_depend.
-
<exec_depend>: For dependencies that are only used when running code, use exec_depend.
17.3 rosdep working logic
These dependencies need to be manually populated into the package.xml file and should be an exhaustive list of all non-builtin libraries and packages required by the feature package. rosdep checks the package.xml file and looks for the rosdep key stored there. These keys are checked against a central index called rosdistro to find installable ROS packages or software repositories in various package managers. Finally, once these dependencies are found in rosdistro, rosdep will install them. Files related to rosdistro can be found at:
https://github.com/ros/rosdistro
17.4 Fill in the rosdep key
Now let's see how to correctly fill the key name into package.xml. For ROS packages (such as nav2_bt_navigator), directly fill in the name of the package. A list of all distributed ROS packages can be found in the <distribution.yaml file of 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 收录软件库
In addition, rosdistro supports developers to upload unrecorded software libraries, which is very useful for projects that need to be packaged and deployed. If the software library you want to use is not in rosdistro, you can add it yourself! Pull requests to rosdistro are usually merged within a week. Detailed instructions on how to provide a new rosdep key can be found here:
https://github.com/ros/rosdistro/blob/master/CONTRIBUTING.md#rosdep-rules-contributions
If for some reason these may not be publicly available, rosdistro can be forked and an alternate index maintained for use.
17.6 How to use
Now that we know a little about rosdep, it's time to learn how to use it. First, if this is the first time using rosdep, it must be initialized by:
sudo rosdep init
Next update the locally cached rosdistro index. It's a good idea to occasionally update rosdep to get the latest index:
rosdep update
Finally, we can install dependencies. Typically this is run on a workspace to install all dependencies. If the workspace has a directory src containing source code, enter the following directive:
rosdep install --from-paths src -y --ignore-src
Note: --from-paths, specify the path of the feature package to check for dependencies. -y, which defaults to yes to all prompts from the package manager to install without prompts. --ignore-src, means ignore installed dependencies even if rosdep key is present.
For more other available parameters and options, please use the following command to view:
rosdep -h
18 Modular visualization tool rqt
18.1 Introduction
rqt is a graphical user interface (GUI) framework that implements various tools and interfaces in the form of plugins. All existing GUI tools can be run as dockable windows in rqt! These tools can still be run in a traditional stand-alone fashion, but rqt makes it easier to manage all the individual windows in a single screen layout.
18.2 Installation and Startup
Open the terminal and enter the following command to install 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: The message indicates that the system will terminate in an attempt to protect itself from damage.
-
Error: Messages that indicate major problems that do not necessarily damage the system, but prevent it from functioning properly.
-
Warning: The message indicates unexpected activity or undesirable results that may indicate a deeper problem but does not directly impair functionality.
-
Info: Messages indicate events and status updates as visual verification that the system is functioning as expected.
-
Debug: Messages detailing the entire step-by-step process performed by the system.
rqt_console can only see messages of the default severity level and higher. The default severity level is Info and normally only Debug messages are hidden as it is the only level lower than Info level. For example, if you set the default level to Warn, you will only see messages with severities of Warn, Error, and Fatal.
Open a terminal and enter the following command to reset the default severity level:
ros2 run turtlesim turtlesim_node --ros-args --log-level WARN # Set the default severity level to Warn
19 Diagnostic problem tool ros2doctor
19.1 Background
When ROS2 is not running as expected with the settings, you can use the ros2doctor tool to check its settings. ros2doctor checks all aspects of ROS2, including platform, version, network, environment, operating system, etc., and warns you of possible errors and causes of problems.
19.2 Prerequisites
ros2doctor is part of the ros2cli package. You can use ros2doctor as long as you have ros2cli installed (as any normal installation should have).
This tutorial uses turtlesim to illustrate some examples.
19.3 Examples
19.3.1 Check Settings
First open the terminal and enter the following command to set the environment variable:
source install/setup.bash
Let's check the ROS2 setup as a whole with ros2doctor by typing:
ros2 doctor
This will run a check on all setup modules and return warnings and errors.
If the ROS2 setup is in perfect condition, you will see a message similar to the following:
All <n> checks passed
If a warning is received, it will display something like this:
<path>: <line>: UserWarning: <message>
然而，返回一些警告并不罕见，UserWarning并不意味着设置不可用，这更可能只是一种迹象，表明某些东西的配置方式并不理想。例如，如果使用不稳定的ROS2分布，ros2doctor将发现此警告：
UserWarning: Distribution <distro> 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 ...
We can cross check the information here with the warnings we get when running ros2doctor. For example, if ros2doctor returns a warning that your distribution is "not fully supported or tested", you can look at the ROS2 INFORMATION section of the report:
distribution name : <distro>
distribution type : ros2
distribution status : prerelease
release platforms : {'<platform>': ['<version>']}
Here it can be seen that the release status is pre-release, which explains why it is not fully supported.
19.4 Summary
ros2doctor will notify you of problems in the ROS2 installation and runtime system. By using the --report parameter, you can gain more insight into the information behind these warnings.
Remember, ros2doctor is not a debugging tool; it does not help with errors on the code or system implementation side.
The README of ros2doctor will tell you more about the different parameters:
https://github.com/ros2/ros2cli/tree/humble/ros2doctor
20 Record and playback tool ros2bag
20.1 Documenting packages from the command line
20.1.1 Background
ros2bag is a command-line tool for logging data published on system topics. It accumulates data passed to any number of topics and persists it in the database. You can then replay the data to reproduce the results of tests and experiments. Recording threads is also a great way to share your work and allow others to recreate it.
20.1.2 Prerequisites
You should have installed ros2bag as part of your normal ROS2 setup. If you are installing from Debian and the system does not recognize the command, install it as follows:
sudo apt-get install ros-humble-ros2bag \
ros-humble-rosbag2-storage-default-plugins
This tutorial covers concepts covered in previous tutorials, such as nodes and topics. It also uses the turtlesim package.
As always, don't forget to set the environment variable in every new terminal you open.
20.1.3 Tasks
20.1.3.1 Settings
You will be recording keystrokes in the turtlesim system to save and playback later, so first start the /turtlesim and /teolep_turtle nodes.
Open the terminal and enter the following command to run the turtle:
ros2 run turtlesim turtlesim_node # run turtlesim
Open a new terminal and enter the following command to run the turtle controller:
ros2 run turtlesim turtle_teleop_key # Run the turtle controller
Let's also create a new directory to store saved recordings in:
mkdir ~/bag_files cd bag_files
20.1.3.2 Select Topic
ros2bag can only record data published on the topic. To view a list of system topics, enter the following command:
ros2 topic list
will return:
/parameter_events /rosout /turtle1/cmd_vel /turtle1/color_sensor /turtle1/pose
In the topics tutorial, you learned that the /turtle_teleop node issues commands on the /turtle1/cmd_vel topic to make turtles move around in turtlesim.
To see what data /tuttle1/cmd_vel is publishing, run the following command:
ros2 topic echo /turtle1/cmd_vel
Nothing will appear at first because the controlling terminal is not posting any data. Go back to the terminal where the controlling turtle is running and select it, making it active. Use the arrow keys to move the turtle and you'll see the data published on the terminal running 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 Record Topic
To log data published to a topic, use the syntax command:
ros2 bag record <topic_name>
Before running this command on a topic of your choice, open a new terminal and move into the bag_files directory you created earlier, as the rosbag file will be saved in the directory you ran it from.
Run the command:
ros2 bag record /turtle1/cmd_vel
You will see the following message in the terminal (date and time vary):
[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...
Right now, ros2bag is logging data published on the /turtle1/cmd_vel topic. Go back to the terminal controlling the turtle, let the turtle move for a while, then press Ctrl+C to stop recording.
The data will be stored in a bag file in the format rosbag2_year_month_dayhour_minute_second.
20.1.3.4 Record multiple topics
You can also record multiple topics, as well as change the name of the bag file that ros2bag saves to.
Run the following command:
ros2 bag record -o subset /turtle1/cmd_vel /turtle1/pose
The -o parameter allows you to choose a specific name for the package file, in this case subset is the filename.
To log multiple threads at once, simply list each thread separated by a space. You will see the following message confirming that two sessions are being recorded.
[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...
Once you're done, you can move the turtle and press Ctrl+C to finish.
There is also an argument -a that can be added to the command, which logs all topics on the system.
20.1.3.5 Package file information
You can view detailed information about a record by running the following command:
ros2 bag info <bag_file_name>
Running this command on a subset of package files will return a list of information about the package files:
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
To see a single message, you have to open the database (in this case sqlite3) to examine it, which is outside the scope of ROS2.
20.1.3.6 Play back topic data
Enter Ctrl+C in a running controlling terminal before playing back the package file. Then make sure your turtle window is visible so you can see the package file in action.
Enter the following command:
ros2 bag play subset
Terminal will return the message:
[INFO] [rosbag2_storage]: Opened database 'subset'.
Your turtles will follow the same path you entered when recording (though not 100% accurate; turtlesim is sensitive to small changes in system timing).
因为子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
Your terminal will return a message verifying the creation of the function package bag_recorder_nodes_py and all necessary files and folders. The --dependencies parameter will automatically add the necessary dependencies to package.xml.txt. In this case the feature pack will use rosbag2_py and rclpy. Message definitions also depend on example_interfaces.
20.2.3.2 Update package.xml and setup.py
Because the --dependencies option is used during feature package creation, you don't have to manually add dependencies to package.xml. However, as always, make sure to add a description, maintainer email and name, and license information to package.xml.
<description>Python bag writing tutorial</description> <maintainer email="[email protected]">Your Name</maintainer> <license>Apache License 2.0</license>
Also, make sure to add this information to your setup.py file.
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)
Now that the writer is set up to record the data we pass to it, we create a subscription and assign it a callback. We will write data to the package in the callback.
self.subscription = self.create_subscription(
String,
'chatter',
self.topic_callback,
10)
self.subscription
The callback receives the message in unserialized form (which is standard for the rcppy API), and passes the message to the writer, specifying the topic the data is for and the timestamp to log with the message. However, the writer needs to store the serialized information in the package. This means we need to serialize the data before passing it to the writer. Therefore, instead of passing the message directly, we call serialize_message() and pass its result to the writer.
def topic_callback(self, msg):
self.writer.write(
'chatter',
serialize_message(msg),
self.get_clock().now().nanoseconds)
The file ends with the main function for creating a node instance and starting ROS to process it.
def main(args=None):
rclpy.init(args=args)
sbr = SimpleBagRecorder()
rclpy.spin(sbr)
rclpy.shutdown()
20.2.3.5 Add entry point
Open the setup.py file in the bag_recorder_nodes_py package and add the entry point for the node.
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 Compile and run
Enter the root directory ros2_ws of the workspace and compile the function package.
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
You will see the data generated and stored in the package printed to the console at a rate of one message per second.
20.4 Logging Synthetic Data from Executable Files
Now that you can create a package to store data from sources other than the topic, you'll learn how to generate and log synthetic data from a non-node executable. The advantage of this approach is that the code is simpler and large amounts of data can be created quickly.
20.4.1 Writing executables
In the ros2_ws/src/bag_recorder_nodes_py/bag_recorder_nodes.py directory, create a new file called data_generator_executable.py and paste the following code into it.
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 Check code
Comparing this sample with the previous one reveals that they are not all that different. The only notable difference is the use of a for loop to drive the data generation instead of a timer.
Note that we are now also generating timestamps for the data, rather than relying on the current system time for each sample. Timestamp can be any value you need. The data will play at the rate given by these timestamps, so this is a useful way to control the default playback speed of samples. Also note that although the interval between each sample is a full second, this executable does not need to wait a second between each sample. This allows us to generate large amounts of data covering a wide time span in much less time than would be required for playback.
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
Next, start describing startup:
def generate_launch_description(): return LaunchDescription([ ])
The first two statements in the startup descriptor start two turtle simulation windows:
Node(
package='turtlesim',
namespace='turtlesim1',
executable='turtlesim_node',
name='sim'
),
Node(
package='turtlesim',
namespace='turtlesim2',
executable='turtlesim_node',
name='sim'
),
Finally start the impersonation node with remap:
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}}"
You will see the second turtle mimicking the movements of the first turtle.
21.1.4 Summary
Startup files simplify running complex systems with many nodes and specific configuration details. You can create launch files using Python, XML or YAML and run them with the ros2 launch command.
21.2 Integrating startup files into feature packs
21.2.1 Background
In the previous we learned how to write an independent startup file and run the startup file directly. This article will now show how to add a startup file to an existing feature package, and the conventions used in general.
21.1.2 Prerequisites
You should have completed the tutorial on how to create a ROS 2 package.
As always, don't forget to set the environment variable in every new terminal you open.
21.1.3 Examples
21.1.3.1 Create a feature package
Create a workspace:
mkdir -p launch_ws/src cd launch_ws/src
Create a function package:
ros2 pkg create py_launch_example --build-type ament_python
21.1.3.2 Create a directory for storing startup files
By convention, all launch files for a feature pack are stored in the launch directory inside the feature pack. Make sure to create the startup directory at the top level of the feature package created above:
mkdir py_launch_example/launch
For a Python function package, the directory of the function package should look like this:
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添加以下内容：
<exec_depend>ros2launch</exec_depend>
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 Parent startup file
Let's create a launch file that will call the arguments and pass them to another launch file. To do this, create example_main.launch.py in the launch directory of the launch_tutorial package and add the following content:
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()
)
])
In the example_main.launch.py file, FindPackageShare replaces the path used to find the launch_tutorial feature package. Then use PathJoinSubstitution instead to join the path with the example_substitutions.launch.py filename.
PathJoinSubstitution([
FindPackageShare('launch_tutorial'),
'example_substitutions.launch.py'
])
The turtlesim_ns and use_provided_red parameters in the launch_argument dictionary are passed to IncludeLaunchDescription, and TextSubstitution is used to define the new_background_r parameter, whose value is the value corresponding to the background_r key in the colors dictionary.
launch_arguments={
'turtlesim_ns': 'turtlesim2',
'use_provided_red': 'True',
'new_background_r': TextSubstitution(text=str(colors['background_r']))
}.items()
21.3.3.3 Replacing Example Startup Files
Next, create a startup file named example_substitutions.launch.py in the launch directory, and fill in the following content:
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],
)
])
In the example_substitutions.launch.py file, launch configurations such as turtlesim_ns, use_provided_red, and new_background_r are defined. They are used to store the values of the startup parameters in the aforementioned variables and pass them to the desired action. These LaunchConfiguration overrides allow us to get the values of launch parameters in any part of the launch description.
DeclareLaunchArgument is used to define launch arguments that can be passed from the above launch file or console.
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'
)
The namespace of the turtlesim_node node is set to 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
这将执行以下操作：
-
以蓝色背景启动一个turtlesim节点。
-
生成第二只海龟。
-
将颜色更改为紫色。
-
如果提供的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 '<name>:=<value>'):
'turtlesim_ns':
no description given
(default: 'turtlesim1')
'use_provided_red':
no description given
(default: 'False')
'new_background_r':
no description given
(default: '200')
Now, the required parameters can be passed to the startup file as follows:
ros2 launch launch_tutorial example_substitutions.launch.py turtlesim_ns:='turtlesim3' use_provided_red:='True' new_background_r:=200
document
The launch documentation provides details on available overrides:
https://github.com/ros2/launch/blob/humble/launch/doc/source/architecture.rst
21.3.6 Summary
In this tutorial, you learned to use substitutions in startup files. You learned about the possibility and ability to create reusable startup files using substitution.
21.4 Using event handlers
21.4.1 Background
launch in ROS2 is a system for executing and managing user-defined processes. It is responsible for monitoring the state of the processes it starts, as well as reporting and responding to changes in the state of those processes. These changes are called events and can be handled by registering event handlers with the boot system. Event handlers can be registered for specific events and can be used to monitor the state of a process. Additionally, they can be used to define a complex set of rules that can be used to dynamically modify startup files.
This tutorial will show an example of the use of event handlers in the ROS2 startup file.
21.4.2 Prerequisites
This tutorial uses the turtlesim feature package. This tutorial also assumes that you have created a package of type ament_python compiled called launch_tutorial.
This tutorial extends the code using the superseded tutorial.
21.4.3 Using event handlers
21.4.3.1 Sample Startup File for Event Handlers
Create a python file named example_event_handlers.launch.py under the launch folder of the launch_tutorial package.
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())
)
)
),
The OnExecutionComplete event handler is used to register a callback function to be executed when the spawn_turtle action completes. It logs a message to the console and executes the change_background_r and change_background_r_conditioned actions when the spawn_turtle action completes.
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'))
]
)
),
The OnShutdown event handler is used to register a callback function that is executed when the startup file is asked to close. It will log a message to the console explaining why it asked to close the startup file. It logs a message with the reason for the shutdown, such as closing the turtlesim window or a ctrl+c signal from the user.
RegisterEventHandler(
OnShutdown(
on_shutdown=[LogInfo(
msg=['Launch was asked to shutdown: ',
LocalSubstitution('event.reason')]
)]
)
),
21.4.4 Compile function package
Go back to the workspace directory and compile the feature package:
colcon build
Set environment variables:
source install/setup.bash
21.4.5 Running the example
Enter the following command to run the startup file:
ros2 launch launch_tutorial example_event_handlers.launch.py turtlesim_ns:='turtlesim3' use_provided_red:='True' new_background_r:=200
This will do the following: 1. Start a turtlesim node with a blue background. 2. Spawn the second turtle. 3. Change the color to purple. 4. If the provided background_r parameter is 200 and the use_prided_red parameter is True, change the color to pink after two seconds. 5. Close the startup file when the turtlesim window closes.
Additionally, it will log messages to the console when: 1. The turtlesim node starts. 2. Execute the spawn action. 3. Execute the change_background_r action. 4. Execute the change_background_r_conditioned action. 5. The turtlesim node exits. 6. The startup process is asked to close.
21.4.6 Documentation
The launch documentation provides details on the available event handlers:
https://github.com/ros2/launch/blob/humble/launch/doc/source/architecture.rst
21.4.7 Summary
In this tutorial, you learned how to use event handlers in your startup files. You learned their syntax and usage examples to define a complex set of rules to dynamically modify startup files.
21.5 Managing Large Projects
21.5.1 Background
This tutorial covers some tips for writing startup files for larger projects. The point is how to structure the startup files so that they can be reused as much as possible in different situations. Additionally, it presents usage examples of the different ROS2 launch tools, such as parameters, YAML files, remaps, namespaces, default parameters, and RViz configuration.
21.5.2 Prerequisites
This tutorial uses turtlesim and the turtle_tf2_py package. This tutorial also assumes that you have created a new package called launch_tutorial of type ament_python compilation.
21.5.3 Introduction
Large-scale applications on robots often involve multiple interconnected nodes, each of which can have multiple parameters. A good example is simulating multiple turtles in the Turtle Simulator. The turtle simulation consists of multiple turtle nodes, global configuration, and TF broadcast and listener nodes. Between all nodes, there are a large number of ROS parameters that affect the behavior and appearance of these nodes. The ROS2 startup file allows us to start all nodes and set the corresponding parameters in one place. At the end of the tutorial, you will compile the launch_turtlesim.launch.py launch file in the launch_tutorial feature package. This startup file will start the nodes responsible for simulating turtles, starting TF broadcasters and listeners, loading parameters, starting RViz configuration, etc. In this tutorial, we will introduce this startup file and all related functions.
21.5.4 Writing a startup file
21.5.4.1 Top-Level Organization
编写启动文件的过程中的一个目标应该是使它们尽可能地可重用。这可以通过将相关节点和配置集群到单独的启动文件中来实现。然后，可以编写专用于特定配置的顶层启动文件。这将允许在完全相同的机器人之间移动，而完全不改变启动文件。即使是从一个真实的机器人到一个模拟的机器人这样的移动，也只需要几个改变就可以完成。
现在我们将讨论使这成为可能的顶层启动文件结构。首先，我们将创建一个启动文件，该文件将调用单独的启动文件。为此，让我们创建在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
])
This startup file includes a set of other startup files. Each of these included startup files contains nodes, parameters, and possibly nested includes that are part of the system. To be precise, we started two turtle sim worlds, TF broadcaster, TF listener, simulation, fixed coordinate system broadcaster and RViz node.
Design tip: The top-level startup file should be short, include other files corresponding to application subcomponents, and parameters that change frequently.
As we'll see later, writing a startup file the following way makes it easy to swap parts of the system. However, sometimes for performance and usage reasons certain nodes or startup files must be run separately.
Design tip: Be aware of the trade-offs when deciding how many top-level startup files your application needs.
21.5.4.2 Parameters
21.5.4.2.1 Setting parameters in the startup file
We'll start by writing a startup file to start our first turtle simulation. First, create a new file called 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'),
}]
),
])
This startup file starts the turtlesim_node node, which starts the turtlesim simulation, and defines the simulation configuration parameters passed to the node.
21.5.4.2 Parameters for loading YAML files
In the second launch, we will start a second turtle simulation with a different configuration. Now create a turtlesim_world_2.launch.py file.
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] )
However, loading the same YAML file will not affect the appearance of the third turtlesim world. The reason is that its parameters are stored under another namespace, like so:
/turtlesim3/sim: background_b background_g background_r
So instead of creating a new configuration for the same node with the same parameters, we can use the wildcard syntax. /** Will assign the same parameters to each node, albeit with different node names and namespaces.
We now update the turtlesim.yaml, saved in the /config folder as follows:
/**:
ros__parameters:
background_b: 255
background_g: 86
background_r: 150
Now include the turtlesim_world_3.launch.py launch description into the main launch file. Using this configuration file in our launch description will assign the background_b, background_g and background_r parameters to the specified values in the turtlesim3/sim and turtlesim2/sim nodes.
21.5.4.3 Namespaces
正如您可能已经注意到的，我们已经在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,
]
)
Finally, we replace turtlesim_world_2 with turtlesim_world_2_with_namespace in the returnLaunchDescription statement. Therefore, each node described in the launch description in turtlesim_world_2.launch.py will have a turtlesim2 namespace.
21.5.4.4 Multiplexing Nodes
Now create a broadcaster_listener.launch.py file.
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')}
]
),
])
In this file, we declare the target_frame launch parameter, the default value is turtle1. A default value means that the startup file can receive an argument to forward to its node, or if the argument is not provided, it will pass a default value to its node.
We then started the turtle_tf2_broadcast node twice during startup, with different names and parameters. This allows us to replicate the same nodes without conflicts.
我们还启动了一个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]
)
])
This launch file will launch RViz using the configuration file defined in the turtle_tf2_py feature package. This RViz configuration will set the world coordinate system, enable TF visualization, and start RViz with a top-down view.
21.5.4.8 Environment variables
Now, let's create one last launch file called 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
您将看到以下类似结果：
Besides that, RViz should already be started. It will display all turtle coordinates relative to the world coordinate system, whose origin is at the bottom left corner.
21.5.6 Summary
In this tutorial, you learned various tips and practices for managing large projects using ROS2 startup files.
22 Robot coordinate system management tool tf2
22.1 Introduction to tf2
22.1.1 Installation example
Let's start by installing the example feature pack and its dependencies.
sudo apt-get install ros-humble-turtle-tf2-py ros-humble-tf2-tools ros-humble-tf-transformations
22.1.2 Running Example
Now that we have the example feature pack installed, let's run the example. First, open a new terminal and set the environment variables so the commands will work properly. Then run the following command:
ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py
You will see two turtles.
Type the following command in the second terminal window:
ros2 run turtlesim turtle_teleop_key
Select a second terminal window so your keystrokes are captured to control the turtle. Once the simulation starts, you can use the keyboard arrow keys to control the turtle in the middle. As you can see, the other turtle is constantly moving, following the one you are controlling.
This demo uses the tf2 library to create three coordinate systems: the world coordinate system, the turtle1 coordinate system, and the turtle2 coordinate system. This tutorial uses the tf2 broadcaster to publish the turtle coordinates, uses the tf2 listener to calculate the difference between the turtle coordinates, and moves another turtle to follow the controlled turtle.
22.1.3 Tools for tf2
Now let's see how to use this example created by tf2. We can use tf2_tools to see what tf2 is doing behind the scenes.
22.1.3.1 view_frames
view_frames creates a schematic view of the coordinate system that tf2 broadcasts on ROS.
ros2 run tf2_tools view_frames
You will see:
Listening to tf data during 5 seconds... Generating graph in frames.pdf file...
Here, the tf2 listener is listening to coordinate systems broadcast through ROS and drawing a tree diagram of how the coordinate systems are related. To view the dendrogram, open the generated frames.pdf with a pdf viewer.
Here we can see the three coordinate systems broadcast by tf2: world, turtle1, and turtle2. The world coordinate system is the parent coordinate system of turtle1 and turtle2. view_frames also reports when the oldest and newest coordinate transformations were received, and the rate at which tf2 coordinates were posted to tf2.
22.1.3.2 tf2_echo
tf2_echo reports the transformation between any two coordinate frames broadcast via ROS.
usage:
ros2 run tf2_ros tf2_echo [reference_frame] [target_frame]
Let's look at the transformation of the turtle2 coordinate system relative to the turtle1 coordinate system, which is equivalent to:
ros2 run tf2_ros tf2_echo turtle2 turtle1
When the tf2_echo listener receives the coordinate system broadcast via ROS2, you will see the transformation displayed.
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]
When controlling the movement of the turtles, you will see the changes when the two turtles move relative to each other.
22.1.4 rvzi and tf2
rviz is a visualization tool for inspecting tf2 coordinate systems. Let's take a look at our turtle coordinate system with rviz. Let's start rvzi using the turtle_rviz.rviz configuration file:
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()
check code
Now, let's look at the code related to publishing the static turtle pose to tf2. The first line imports the required feature packages. First, we import TransformStamped from geometry_msgs , which provides the template for the messages we will post to the transform tree.
from geometry_msgs.msg import TransformStamped
Then, import rclpy so that its Node class can be used.
import rclpy from rclpy.node import Node
The tf2_ros feature package provides StaticTransformBroadcaster, which makes publishing static transformations easy. To use StaticTransformBroadcaster we need to import it from the tf2_ros module.
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
The StaticFramePublisher class constructor initializes the node with the name static_turtle_tf2_broadcast. Then, create a StaticTransformBroadcaster that will send a static transform on startup.
self.tf_static_broadcaster = StaticTransformBroadcaster(self) self.make_transforms(transformation)
在这里，我们创建一个TransformStamped对象，它将是填充后发送的消息。在传递实际的转换值之前，我们需要为其提供适当的元数据。
-
我们需要给正在发布的转换一个时间戳，我们只需要用当前时间self.get_clock().now()标记它。
-
然后，我们需要设置正在创建的父坐标系的名称，在本例中是world。
-
最后，我们需要设置正在创建的子坐标系的名称。
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]
Finally, we broadcast the static transformation using the sendTransform() function.
self.tf_static_broadcaster.sendTransform(t)
add dependencies
Enter the src/learning_tf2_py directory, which contains the created setup.py, setup.cfg, package.xml files.
Open package.xml with a text editor.
Make sure to fill out the <description>, <maintainer>, and <license> tags as described in the Creating a Feature Pack tutorial:
<description>Learning tf2 with rclpy</description> <maintainer email="[email protected]">Your Name</maintainer> <license>Apache License 2.0</license>
After the above line, add the following dependencies corresponding to node's import statements:
<exec_depend>geometry_msgs</exec_depend> <exec_depend>python3-numpy</exec_depend> <exec_depend>rclpy</exec_depend> <exec_depend>tf2_ros_py</exec_depend> <exec_depend>turtlesim</exec_depend>
This declares the required geometry_msgs, python3-numpy, rcppy, tf2_ros_py and turtlesim dependencies when executing the code. Make sure to save the file.
add entry point
To allow the ros2 run command to run the node, an entry point must be added to setup.py (located in the src/learning_tf2_py directory).
Add the following line between the "console_scripts" brackets:
'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',
compile
It's best to run rosdep in the root of your workspace to check for missing dependencies before compiling:
rosdep install -i --from-path src --rosdistro humble -y
Still in the root directory of the workspace, compile the feature package:
colcon build --packages-select learning_tf2_py
Open a new terminal, enter the root directory of the workspace, and set the environment variables:
. install/setup.bash
run
Now run the static_turtle_tf2_broadcaster node:
ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0
This sets up a turtle pose broadcast that floats mystaticturtle 1 meter off the ground.
We can now check that the static transitions were published by echoing the tf_static topic:
ros2 topic echo /tf_static
If all goes well, you should see a static transition.
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
Publish static coordinate transformations to tf2 using x/y/z offsets in meters and quaternions.
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 is designed as a command-line tool that can be used either manually or in a startup file to set up static transformations. For example:
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']
),
])
Note that all arguments except --frame-id and --child-frame-id are optional; if no specific option is specified, identity will be assumed.
22.2.4 Summary
在本教程中，您学习了静态变换如何用于定义坐标系之间的静态关系，例如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 Check code
Now, let's look at the code related to publishing the turtle pose to tf2. First, we define and get a parameter turtlename that specifies the name of the turtle, such as turtle1 or turtle2.
self.turtlename = self.declare_parameter( 'turtlename', 'turtle').get_parameter_value().string_value
The node then subscribes to the topic turtleX/pose and runs the function handle_turtle_pose on each incoming message.
self .subscription = self.create_subscription(
Pose,
f'/{self.turtlename}/pose',
self.handle_turtle_pose,
1)
Now, we create a TransformStamped object and give it the appropriate metadata.
-
We need to give a timestamp to the transition being published, we just need to stamp it with the current time self.get_clock().now(). This will return the current time used by the node.
-
We then need to set the name of the parent coordinate system we are creating, in this case world.
-
Finally, we need to set the name of the subcoordinate system we are creating.
The handler function for the turtle pose message broadcasts the translation and rotation of the turtle and publishes it as a transformation from the world coordinate system to the turtleX coordinate system.
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 Adding dependencies
Enter the src/learning_tf2_py directory, which contains the created setup.py, setup.cfg, package.xml files.
使用文本编辑器打开package.xml。添加与启动文件的导入语句相对应的以下依赖项：
<exec_depend>launch</exec_depend> <exec_depend>launch_ros</exec_depend>
这将在执行代码时声明额外所需的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]
If you run tf2_echo for the transformation between world and turtle2, you should not see the transformation because the second turtle has not yet appeared. However, when we add a second turtle in the next tutorial, the pose of turtle2 will be broadcast to tf2.
22.7 Summary
In this tutorial, you learned how to broadcast the robot's pose (position and orientation of the turtle) to tf2 and how to use the tf2_echo tool. To actually use transitions broadcasting to tf2, you should move on to the next tutorial on creating a tf2 listener.
22.4 Writing Listeners
22.4.1 Background
在之前的教程中，我们创建了一个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 Check Code
To learn how the service behind the spawning turtle works, refer to the Writing a Simple Service and Client (Python) tutorial.
Now, let's look at the code related to accessing coordinate system transformations. The tf2_ros package provides an implementation of TransformListener to help simplify the task of receiving transformations.
from tf2_ros.transform_listener import TransformListener
Here, we create a TransformListener object. Once the listener is created, it starts receiving tf2 transforms over the network and buffers them for up to 10 seconds.
self.tf_listener = TransformListener(self.tf_buffer, self)
Finally, we query the listener for a specific transition. We call the lookup_transform method with the following parameters:
-
target coordinate system
-
source coordinate system
-
time we want to change
Supplying rcpy.time.Time() will give us the latest available conversion. All of these are wrapped in a try except code block to handle possible exceptions.
t = self.tf_buffer.lookup_transform(
to_frame_rel,
from_frame_rel,
rclpy.time.Time())
22.4.3.3 Add entry point
To allow the ros2 run command to run the node, an entry point must be added to setup.py (located in the src/learning_tf2_py directory).
'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',
22.4.3.4 Update startup files
Open the launch file named turtle_tf2_demo.launch.py with a text editor, add two new nodes to the launch description, add a launch parameter, and add imports. The resulting file should look like this:
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')}
]
),
])
This will declare a target_frame start parameter, start the broadcaster for the second turtle we will spawn, and start the listener that subscribes to these transitions.
22.4.3.5 Compile
Run rosdep in the root of your workspace to check for missing dependencies.
rosdep install -i --from-path src --rosdistro humble -y
Still in the root of your workspace, compile your feature package:
colcon build --packages-select learning_tf2_py
Open a new terminal, go to the root directory of the workspace, and set environment variables:
. install/setup.bash
22.4.3.6 Running
Now you can start the full turtle example:
ros2 launch learning_tf2_py turtle_tf2_demo.launch.py
You should see the simulation window with two turtles. In the second terminal window, type the following command:
ros2 run turtlesim turtle_teleop_key
To see if things are going right, just drive the first turtle with the arrow keys (make sure the terminal window is active, not the emulator window) and you'll see the second turtle follow the first turtle!
22.4.4 Summary
In this tutorial, you learned how to access coordinate transformations with tf2. You've also finished writing the turtle example you tried for the first time in the intro to tf2 tutorial.
22.5 Adding a Coordinate System
22.5.1 Background
In a previous tutorial, we recreated the turtle demo by writing a tf2 broadcaster and a tf2 listener. This tutorial will teach you how to add additional fixed and dynamic coordinate systems to the transform tree. In fact, adding a coordinate system in tf2 is very similar to creating a tf2 broadcast, but this example will show you some additional capabilities of tf2.
For many transformation-related tasks, it's easier to think within the local coordinate system. For example, laser scanning measurements are easiest to understand in a frame at the center of the laser scanner. tf2 allows you to define a local coordinate system for each sensor, link or joint in the system. When transforming from one frame to another, tf2 will handle any hidden intermediate frame transformations that are introduced.
22.5.2 tf2 tree
tf2 establishes a tree structure of the coordinate system, so it does not allow closed loops to form in the coordinate system structure. This means that a coordinate system has only one parent coordinate system, but can have multiple child coordinate systems. Currently, our tf2 tree contains three coordinate systems: world, turtle1, and turtle2. These two turtlez coordinate systems are sub-coordinate systems of the world coordinate system. If we want to add a new coordinate system to tf2, then one of the three existing coordinate systems must be the parent coordinate system, and the new coordinate system will become its child coordinate system.
22.5.3 Examples
22.5.3.1 Writing a Fixed Coordinate System Broadcaster
In our turtle example, we will add a new coordinate system carrot1 which will be a child coordinate system of the turtle. This coordinate system will be used as the target for the second turtle.
Let's create the source files first. Go to the learning_tf2_py function package we created in the previous tutorial. Enter the following command to download the fixed coordinate system broadcaster code:
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/fixed_frame_tf2_broadcaster.py
Now open the fixed_frame_tf2_broadcaster.py file.
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',
),
])
This startup file imports the required feature packages and then creates a demo_nodes variable that will store the nodes we created in the startup file from the previous tutorial.
The last part of the code will use the fixed_frame_tf2_broadcast node to add our fixed carrot1 coordinate frame to the turtlesim world.
Node(
package='learning_tf2_py',
executable='fixed_frame_tf2_broadcaster',
name='fixed_broadcaster',
),
22.5.3.1.4 Compile
Run rosdep in the root of your workspace to check for missing dependencies.
rosdep install -i --from-path src --rosdistro humble -y
Still in the root of your workspace, compile your feature package:
colcon build --packages-select learning_tf2_py
Open a new terminal, go to the root directory of the workspace, and set environment variables:
. install/setup.bash
22.5.3.1.5 Running
Now you can run the startup file:
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py
You should notice that the new carrot1 coordinate system appears in the transformation tree.
如果你驾驶第一只乌龟四处走动，你应该注意到，尽管我们添加了一个新的坐标系，但行为与上一教程没有改变。这是因为添加额外的坐标系不会影响其他坐标系，而我们的监听器仍在使用先前定义的坐标系。
所以，如果我们希望第二只乌龟跟随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 检查代码
Instead of fixedly defining the x and y offsets, we use the sin() and cos() functions at the current time, so that the offset of carrot1 can keep changing.
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 Add entry point
To allow the ros2 run command to run the node, an entry point must be added to setup.py (located in the src/learning_tf2_py directory).
'dynamic_frame_tf2_broadcaster = learning_tf2_py.dynamic_frame_tf2_broadcaster:main',
22.5.3.2.3 Writing a startup file
To test this code, create a new launch file launch/turtle_tf2_dynamic_frame_demo.launch.py. Copy and paste the following code:
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 Compile
Run rosdep in the root of your workspace to check for missing dependencies.
rosdep install -i --from-path src --rosdistro humble -y
Still in the root of your workspace, compile your feature package:
colcon build --packages-select learning_tf2_py
Open a new terminal, go to the root directory of the workspace, and set environment variables:
. install/setup.bash
22.5.3.2.5 Running
Now you can run the startup file:
ros2 launch learning_tf2_py turtle_tf2_dynamic_frame_demo.launch.py
You should see that the second turtle is changing position following the carrot.
22.5.4 Summary
In this tutorial, you learned about the structure and properties of the tf2 transformation tree. You also learned that it is easiest to think in a local coordinate system and learned to add additional fixed and dynamic coordinate systems to that local coordinate system.
22.6 Time of use
22.6.1 Background
在之前的教程中，我们通过编写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 Repairing Listener Nodes
Now you should notice that lookup_transform() fails. It tells you that the coordinate system doesn't exist or the data is in the future. To fix this, edit the code on line 76 to look like this (returns the timeout parameter):
trans = self._tf_buffer.lookup_transform( to_frame_rel, from_frame_rel, now, timeout=rclpy.duration.Duration(seconds=1.0))
lookup_transform can accept four parameters, the last of which is an optional timeout. It will block for the duration of the wait timeout. After making this change, remove the raise line from the except() block you added above, or your code will continue to fail.
The startup file can now be run.
ros2 launch learning_tf2_py turtle_tf2_demo.launch.py
You should note that lookup_transform() actually blocks until a transformation between the two turtles is available (this usually takes a few milliseconds). Once the timeout is reached (one second in this case), an exception is only raised if the transition is still not available.
22.6.3 Summary
In this tutorial, you learned more about the lookup_transform function and its timeout feature. You also learned how to catch and handle other exceptions that tf2 may throw.
22.7 Time travel
22.7.1 Background
In the previous tutorials, we discussed the basics of tf2 and time. This tutorial will take us one step further and reveal a powerful tf2 trick: time travel. In short, a key feature of the tf2 library is its ability to transform data in time and space.
This tf2 time travel function can be used for various tasks, such as monitoring the pose of a robot over a long period of time, or building a follower robot that follows the "steps" of a leader. We'll use this time travel feature to find the shift in time, and program turtle2 to simulate the action of turtle1 5 seconds ago.
22.7.2 Time travel
First, let's go back to the end of the previous tutorial "Working with time". Go to the learning_tf2_py function package.
Now, instead of letting the second turtle go to where the carrot is now, we let the second turtle go to where the first carrot was 5 seconds ago. Edit the lookup_transform() call in the turtle_tf2_listener.py file to read:
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))
Now, if you run this, for the first 5 seconds, the second turtle won't know where to go because we don't have 5 seconds of history with the carrot pose yet. But what happens after these 5 seconds? Let's try it out:
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py
You should now notice that your turtle is driving around uncontrollably, like in this screenshot. Let's try to understand the reason behind this behavior.
-
In our code, we ask tf2 the following question: "What was the pose of carrot1 5 seconds ago, relative to turtle2 5 seconds ago?". This means we are controlling the second turtle based on where the second turtle was 5 seconds ago and where the first carrot1 was 5 seconds ago.
-
However, what we really want to ask is: "What is the pose of carrot1 relative to the position of turtle2 5 seconds ago?".
22.7.3 Lookup_transform() Advanced Interface
为了问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采用六个参数：
-
目标坐标系
-
转换到的时间
-
源坐标系
-
评估源坐标系的时间
-
不随时间变化的坐标系，在本例中为world坐标系
-
等待目标坐标系可用的时间
综上所述，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/LinearMath/Quaternion.h>
...
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());
The size of a quaternion should always be 1. ROS 2 will print a warning if a numerical error results in a quaternion size other than 1. To avoid these warnings, normalize the quaternion:
q.normalize();
22.8.4 Quaternion types in ROS2
ROS2 uses two quaternion data types, tf2::Quaternion and its equivalent geometry_msgs::msg::Quaternion. To convert quaternion data types in C++, use the tf2_geometry_msgs method.
C++
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> ... 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_geometry_msgs/tf2_geometry_msgs.hpp> ... 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>：用来描述机器人某个刚体部分的外观和物理属性，外观包括尺寸、颜色、形状，物理属性包括质量、惯性矩阵、碰撞参数等。link标签中的name表示该连杆的名称，我们可以自定义，未来joint连接link的时候，会使用到这个名称。一些标签作用如下：
-
<visual>：用来描述机器人的外观，重在描述机器人看上去的状态，也就是视觉效果。比如：
-
<geometry>：表示几何形状，里边使用<mesh>调用了一个在三维软件中提前设计好的蓝色外观，就是这个stl文件，看上去和真实机器人是一致的。
-
<origin>：表示坐标系相对初始位置的偏移，分别是x、y、z方向上的平移，和roll、pitch、raw旋转，不需要偏移的话，就全为0。
-
<collision>：描述碰撞参数。
-
-
<collision>：描述机器人运动过程中的状态，比如机器人与外界如何接触算作碰撞。
-
23.1.2 关节Joint描述
机器人模型中的刚体最终要通过关节joint连接之后，才能产生相对运动。标签中的name表示该关节的名称，type表示该关节的类型，URDF中的关节有六种运动类型。一些标签作用如下：
-
<parent>：描述父连杆。
-
<child>：描述子连杆，子连杆会相对父连杆发生运动。
-
<origin>：表示两个连杆坐标系之间的关系，也就是图中红色的向量，可以理解为这两个连杆该如何安装到一起。
-
<axis>：表示关节运动轴的单位向量，比如z等于1，就表示这个旋转运动是围绕z轴的正方向进行的。
-
<limit>：就表示运动的一些限制了，比如最小位置，最大位置，和最大速度等。
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 version="1.0"?>
<robot name="myfirst">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</visual>
</link>
</robot>
为了将XML翻译成英语，这是一个名为myfirst的机器人，它只包含一个link（连杆，也称为部分），其视觉组件只是一个0.6米长、0.2米半径的圆柱体。
要检查模型，请启动display.launch.py文件：
ros2 launch urdf_tutorial display.launch.py model:=urdf/01-myfirst.urdf
这里发生了三件事：
-
加载指定的模型并将其保存为参数
-
运行节点以发布sensor_msgs/msg/JointState和变换
-
使用配置文件启动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
All the example launch commands given in these tutorials must be changed if they are not run from the urdf_tutorial function package location. After launching display.launch.py, you should see RViz display the following:
Things to note:
-
The fixed coordinate system is the transformed coordinate system in which the center of the grid lies. Here, it is the coordinate system defined by our base_link.
-
By default, the origin of a visual element (cylinder) is at the center of its geometry. So half the cylinder is below the grid.
23.2.2 Multiple shapes
Now let's see how to add multiple shapes/links. If we just add more link elements to the urdf, the parser won't know where to put them. So, we have to add joints. Joint elements may refer to flexible joints and non-flexible joints. We'll start with joints that are not flexible or fixed.
<?xml version="1.0"?>
<robot name="multipleshapes">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</visual>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
</visual>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
</joint>
</robot>
注意我们是如何定义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，使其直立。
<?xml version="1.0"?>
<robot name="origins">
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</visual>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
</visual>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0 -0.22 0.25"/>
</joint>
</robot>
Let's start by checking the origin of the joint. It is defined in terms of the reference coordinate system of the parent object. So we are -0.22 meters in the y direction (left, but right relative to the axis) and 0.25 meters in the z direction (up). This means that the origin of the sub-link will be up and to the right, regardless of the sub-link's visual origin label. Since we didn't specify the rpy (roll pitch yaw) attribute, by default the child coordinate system will have the same orientation as the parent coordinate system. Now, looking at the leg's visual origin, it has xyz and rpy offsets. This defines where the visual element's center is relative to its joint origin. Since we want the leg to attach to the top, we offset the origin downward by setting the z offset to -0.3 meters. Since we want the long part of the leg to be parallel to the z-axis, we rotate the visible part around the y-axis by pi/2.
ros2 launch urdf_tutorial display.launch.py model:=urdf/03-origins.urdf
The function package that the startup file runs will create a TF coordinate system for each link in the model based on the URDF. Rviz uses this information to determine where to display each shape. If a given URDF link does not exist for a TF coordinate system, it will be placed at the origin in white.
23.2.4 Setting Materials
"Okay," I hear you say. "It's cute, but not everyone has a B21. My droid and R2D2 aren't red!" That's a good point. Let's look at the materials tab.
<?xml version="1.0"?>
<robot name="materials">
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0 -0.22 0.25"/>
</joint>
<link name="left_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_left_leg" type="fixed">
<parent link="base_link"/>
<child link="left_leg"/>
<origin xyz="0 0.22 0.25"/>
</joint>
</robot>
身体现在是蓝色的了。我们定义了一种新材料，称为“蓝色”，红色、绿色、蓝色和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 完成模型
现在，我们用更多的形状完成模型：脚、轮子和头。最值得注意的是，我们添加了一个球体和一些网格。我们还将添加一些稍后使用的其他部件。
<?xml version="1.0"?>
<robot name="visual">
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0 -0.22 0.25"/>
</joint>
<link name="right_base">
<visual>
<geometry>
<box size="0.4 0.1 0.1"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="right_base_joint" type="fixed">
<parent link="right_leg"/>
<child link="right_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="right_front_wheel">
<visual>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
</link>
<joint name="right_front_wheel_joint" type="fixed">
<parent link="right_base"/>
<child link="right_front_wheel"/>
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
</joint>
<link name="right_back_wheel">
<visual>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
</link>
<joint name="right_back_wheel_joint" type="fixed">
<parent link="right_base"/>
<child link="right_back_wheel"/>
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
</joint>
<link name="left_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_left_leg" type="fixed">
<parent link="base_link"/>
<child link="left_leg"/>
<origin xyz="0 0.22 0.25"/>
</joint>
<link name="left_base">
<visual>
<geometry>
<box size="0.4 0.1 0.1"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="left_base_joint" type="fixed">
<parent link="left_leg"/>
<child link="left_base"/>
<origin xyz="0 0 -0.6"/>
</joint>
<link name="left_front_wheel">
<visual>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
</link>
<joint name="left_front_wheel_joint" type="fixed">
<parent link="left_base"/>
<child link="left_front_wheel"/>
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
</joint>
<link name="left_back_wheel">
<visual>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.1" radius="0.035"/>
</geometry>
<material name="black"/>
</visual>
</link>
<joint name="left_back_wheel_joint" type="fixed">
<parent link="left_base"/>
<child link="left_back_wheel"/>
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
</joint>
<joint name="gripper_extension" type="fixed">
<parent link="base_link"/>
<child link="gripper_pole"/>
<origin rpy="0 0 0" xyz="0.19 0 0.2"/>
</joint>
<link name="gripper_pole">
<visual>
<geometry>
<cylinder length="0.2" radius="0.01"/>
</geometry>
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
</visual>
</link>
<joint name="left_gripper_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
<parent link="gripper_pole"/>
<child link="left_gripper"/>
</joint>
<link name="left_gripper">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
</geometry>
</visual>
</link>
<joint name="left_tip_joint" type="fixed">
<parent link="left_gripper"/>
<child link="left_tip"/>
</joint>
<link name="left_tip">
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
</geometry>
</visual>
</link>
<joint name="right_gripper_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
<parent link="gripper_pole"/>
<child link="right_gripper"/>
</joint>
<link name="right_gripper">
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
</geometry>
</visual>
</link>
<joint name="right_tip_joint" type="fixed">
<parent link="right_gripper"/>
<child link="right_tip"/>
</joint>
<link name="right_tip">
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
</geometry>
</visual>
</link>
<link name="head">
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="white"/>
</visual>
</link>
<joint name="head_swivel" type="fixed">
<parent link="base_link"/>
<child link="head"/>
<origin xyz="0 0 0.3"/>
</joint>
<link name="box">
<visual>
<geometry>
<box size="0.08 0.08 0.08"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<joint name="tobox" type="fixed">
<parent link="head"/>
<child link="box"/>
<origin xyz="0.1814 0 0.1414"/>
</joint>
</robot>
ros2 launch urdf_tutorial display.launch.py model:=urdf/05-visual.urdf
How to add the sphere should be self-explanatory:
<link name="head">
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<material name="white"/>
</visual>
</link>
The mesh here is borrowed from PR2. These are separate files for which you must specify a path. You should use package://NAME_OF_PACKAGE/ path notation. The mesh for this tutorial is located in a folder named mesh in the urdf_tutorial function package.
<link name="left_gripper">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
</geometry>
</visual>
</link>
可以以多种不同的格式导入网格。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 头部
<joint name="head_swivel" type="continuous"> <parent link="base_link"/> <child link="head"/> <axis xyz="0 0 1"/> <origin xyz="0 0 0.3"/> </joint>
The connection between the body and the head is a continuous joint, which means it can assume any angle from negative infinity to positive infinity. The wheels are also modeled this way so they can roll in both directions forever.
The only additional information we have to add is the axis of rotation, here specified by the xyz triplet, which specifies the vector around which the head will rotate. Since we want it to rotate around the z-axis, we assign the vector "0 0 1".
23.3.2 Fixtures
<joint name="left_gripper_joint" type="revolute"> <axis xyz="0 0 1"/> <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/> <origin rpy="0 0 0" xyz="0.2 0.01 0"/> <parent link="gripper_pole"/> <child link="left_gripper"/> </joint>
Both the left and right gripper joints are modeled as revolute joints. This means they rotate in the same way as continuous joints, but they have strict constraints. Therefore, we must include limit labels specifying the joint's upper and lower limits (in radians). We also have to specify the maximum velocity and force for this joint, but the actual values are not important for our purposes for now.
23.3.3 Gripper arm
Both the left and right gripper joints are modeled as revolute joints. This means they rotate in the same way as continuous joints, but they have strict constraints. Therefore, we must include limit tags specifying the upper and lower limits of the joint (in radians). We also have to specify the joint's maximum velocity and force, but the actual values are not important for our purposes.
<joint name="gripper_extension" type="prismatic"> <parent link="base_link"/> <child link="gripper_pole"/> <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/> <origin rpy="0 0 0" xyz="0.19 0 0.2"/> </joint>
Gripper arms are a different type of joint, the prismatic joint. That means it moves along the axis, not around it. This translational motion enables our robot model to extend and retract its gripper arm.
Prismatic arms limits are specified in the same way as revolute joints, but in meters instead of radians.
23.3.4 Other types of joints
There are two other types of joints that can move in space. While prismatic joints can only move in one dimension, planar joints can move in a plane or in two dimensions. In addition, the floating joints are unconstrained and can move arbitrarily in 3D space. These joints cannot be specified by only a number and are therefore not covered in this tutorial.
23.3.5 Designated posture
When you move the slider in the GUI, the model moves in Rviz. how did you do that? First, the GUI parses the URDF and finds all non-fixed joints and their limits. It then publishes a sensor_msgs/msg/JointState message with the value of the slider. Robot_state_publisher then uses them to calculate all transformations between different parts. The resulting transform tree is then used to display all shapes in Rviz.
23.3.6 Next step
Now that you have a visual functional model, you can add some physical properties, or start using xacro to simplify your code.
23.4 Add physical properties and collision properties
23.4.1 Collision
So far, we have only assigned a single child element visual to the links, which defines the appearance of the robot. However, in order for collision detection to work or to simulate a robot, we also need to define a collision element.
Here is our new base_link code.
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
</link>
碰撞元素是连杆对象的直接子元素，与可视标签处于同一级别。碰撞元素定义其形状的方式与可视元素相同，使用几何标签。几何标签的格式与可视标签的格式完全相同。也可以采用与可视标签相同的方式指定碰撞标签的原点。
在许多情况下，您希望碰撞几何体、原点与视觉几何体、原始完全相同。然而，有两种主要情况下您不会：
-
更快的处理。对两个网格进行碰撞检测比对两个简单几何体进行碰撞检测要复杂得多。因此，您可能希望用碰撞元素中更简单的几何体替换网格。
-
安全区。您可能希望限制靠近敏感设备的移动。例如，如果我们不想让任何东西与R2D2的头部碰撞，我们可以将碰撞几何体定义为一个包裹着它的头部的圆柱体，以防止任何东西过于靠近它的头部。
23.4.2 物理属性
为了让模型正确模拟，您需要定义机器人的几个物理属性，即Gazebo这样的物理引擎所需的属性。
23.4.3 惯性
每个被模拟的连杆元素都需要一个惯性标签。这里有一个简单的例子。
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1e-3" ixy="0.0" ixz="0.0" iyy="1e-3" iyz="0.0" izz="1e-3"/>
</inertial>
</link>
此元素也是连杆对象的子元素。质量以千克为单位。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
)
}]
)
At the top of the URDF file, a namespace must be specified in order for the file to be parsed correctly. For example, the following are the first two lines of a valid xacro file:
<?xml version="1.0"?> <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
23.5.2 Constants
Let's take a quick look at the base_link in R2D2.
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
</link>
The information here is a bit redundant. We specify the length and radius of the cylinder twice. Worse, if we want to change this, we need to do it in two different places.
Fortunately, xacro allows you to specify properties that act as constants. We can rewrite the code above as this.
<xacro:property name="width" value="0.2" />
<xacro:property name="bodylen" value="0.6" />
<link name="base_link">
<visual>
<geometry>
<cylinder radius="${width}" length="${bodylen}"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder radius="${width}" length="${bodylen}"/>
</geometry>
</collision>
</link>
These two values are specified in the first two lines. They can be defined anywhere, at any level, before or after use. Usually constants are placed at the top. Instead of specifying the actual radius in the geometry element, we use dollar signs and braces to denote the value. This code will generate the same code as shown above.
Keys in ${} will be replaced by their corresponding values, which can be combined with other text in properties.
<xacro:property name=”robotname” value=”marvin” />
<link name=”${robotname}s_leg” />
This will generate
<link name=”marvins_leg” />
However, what's inside ${} doesn't have to be just a property, which leads us to the next section...
23.5.3 Mathematics
You can build arbitrarily complex expressions within the ${} construct using the four basic operations (+, -, *, /), the unary minus sign, and parentheses. Example:
<cylinder radius="${wheeldiam/2}" length="0.1"/>
<origin xyz="${reflect*(width+.02)} 0 0.25" />
You can also use more basic math operations like sin and cos.
23.5.4 Macros
This is the largest and most useful component of the xacro feature pack.
23.5.4.1 Simple Macros
Let's look at a simple useless macro.
<xacro:macro name="default_origin">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:macro>
<xacro:default_origin />
（这是无用的，因为如果未指定原点，它的值与此值相同。）此代码将生成以下内容。
<origin rpy="0 0 0" xyz="0 0 0"/>
从技术上讲，名称不是必需的元素，但您需要指定它才能使用它。xacro:$NAME/的每个实例都被xacro:macro标签的内容替换。注意，即使它不完全相同（两个属性的顺序发生了变化），生成的XML也是等效的。如果找不到具有指定名称的xacro，则不会展开它，也不会生成错误。
23.5.4.2 参数化宏
您还可以参数化宏，使它们不会每次都生成相同的精确文本。当与数学功能相结合时，这将更加强大。
首先，让我们以R2D2中使用的一个简单宏为例。
<xacro:macro name="default_inertial" params="mass">
<inertial>
<mass value="${mass}" />
<inertia ixx="1e-3" ixy="0.0" ixz="0.0"
iyy="1e-3" iyz="0.0"
izz="1e-3" />
</inertial>
</xacro:macro>
这可以被下面的代码使用：
<xacro:default_inertial mass="10"/>
Parameters act like properties, and you can use them in expressions.
It is also possible to use entire code blocks as arguments.
<xacro:macro name="blue_shape" params="name *shape">
<link name="${name}">
<visual>
<geometry>
<xacro:insert_block name="shape" />
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<xacro:insert_block name="shape" />
</geometry>
</collision>
</link>
</xacro:macro>
<xacro:blue_shape name="base_link">
<cylinder radius=".42" length=".01" />
</xacro:blue_shape>
要指定块参数，请在其参数名称前包含星号。可以使用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的两条腿做的那样。
<xacro:macro name="leg" params="prefix reflect">
<link name="${prefix}_leg">
<visual>
<geometry>
<box size="${leglen} 0.1 0.2"/>
</geometry>
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size="${leglen} 0.1 0.2"/>
</geometry>
<origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
</collision>
<xacro:default_inertial mass="10"/>
</link>
<joint name="base_to_${prefix}_leg" type="fixed">
<parent link="base_link"/>
<child link="${prefix}_leg"/>
<origin xyz="0 ${reflect*(width+.02)} 0.25" />
</joint>
<!-- A bunch of stuff cut -->
</xacro:macro>
<xacro:leg prefix="right" reflect="1" />
<xacro:leg prefix="left" reflect="-1" />
Common trick 1: Use a name prefix to get two similarly named objects. Common trick 2: Use math to calculate joint origins. If you change the size of the robot, it will save a lot of trouble to change a property with some math to calculate the joint offset. Common trick 3: Use the reflection parameter, and set it to 1 or -1. For example use the reflect parameter to place the legs on either side of the base_to ${prefix} leg origin.
23.6 Using URDF with robot_state_publisher
23.6.1 Background
This tutorial will show you how to model a walking robot, publish the state as tf2 messages, and view the simulation in Rviz. First, we create a URDF model that describes the robot components. Next, we write a node to simulate motion and publish JointState and transforms. Then, we publish the entire robot state to /tf2 using robot_state_publisher.
23.6.2 Prerequisites
一如既往，不要忘记在您打开的每一个新终端中设置环境变量。
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 Create a startup file
Create new ~/second_ros2_ws/src/urdf_tutorial_r2d2/launch folder. Open an editor and paste the following code, save it as ~/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 Editing the startup file
You have to tell the compilation tool colcon how to install Python packages. Edit the ~/second_ros2_ws/src/urdf_tutorial_r2d2/setup.py file as follows:
Contains import statements.
import os from glob import glob from setuptools import setup from setuptools import find_packages
Add these two lines in data_files.
data_files=[
...
(os.path.join('share', package_name), glob('launch/*.py')),
(os.path.join('share', package_name), glob('urdf/*'))
],
Modify the entry_points table so that you can run "state_publisher" from the console later.
'console_scripts': [
'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
],
Save the setup.py file and its changes.
23.6.3.6 Installing Feature Packs
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一起正常使用。
Although urdfs are a useful and standardized format in ROS, they lack many features and have not been updated to deal with the evolving needs of robotics. urdf can only specify the kinematics and dynamics of a single robot alone, but cannot specify the pose of the robot itself in the world. It is also not a general description format as it cannot specify joint rings (parallel connections) and lacks friction and other properties. Also, it cannot specify non-robots, such as lights, heightmaps, etc. On the implementation side, the urdf syntax breaks proper formatting with its heavy use of XML attributes, which in turn makes urdf even more inflexible.
sdf
To solve this problem, a new format called Simulation Description Format (SDF) was created for use by GazeBo to address the shortcomings of URDF. SDF is a complete description of everything from world-level to robot-level, capable of describing all aspects of robots, static and dynamic objects, lighting, terrain, and even physics. sdf can accurately describe various properties of robots. In addition to traditional kinematics characteristics, it can also define sensors, surface properties, textures, joint friction, etc. for robots; sdf also provides methods to define various environments. Including ambient lighting, terrain, etc. sdf is also described in XML format. In summary, sdf is an evolution of urdf, which can better describe the real simulation conditions.
24.1.4.2 Format Selection
Although there are currently some conversion methods between sdf and urdf, they are often very complicated and error-prone. Therefore, it is recommended to choose the most suitable model format according to your needs at the beginning.
-
Situations where urdf must be used: to use rviz for visualization.
-
Situations where sdf must be used: research on parallel robots, or there is a closed chain structure in the robot.
-
It is recommended to use urdf: to make a simulation as soon as possible to demonstrate the effect; to use Solidworks modeling, and want to easily export the ROS 3D model.
-
It is recommended to use sdf: I want to study ROS-gazebo simulation in depth to make the dynamic characteristics of the simulation more realistic; I want to develop my own dedicated Gazebo simulation plug-in.
24.2 Setting Up Robot Simulation
24.2.1 Prerequisites
Make sure Gazebo is installed
24.2.2 Tasks
24.2.2.1 Start Simulation
在这个演示中，您将在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 Integrating ROS2 and Gazebo
To be able to communicate our simulation with ROS2, you need to use a package called ros_gz_bridge. Use this package to exchange messages between ROS2 and Gazebo Transport. The repository provides package source code for integration between ROS and Gazebo:
https://github.com/gazebosim/ros_gz/tree/ros2#from-source
-
ros_gz: Metapackage that provides all other packages.
-
ros_gz_image: A one-way bridge to transfer images from Gazebo to ROS using image_transport.
-
ros_gz_bridge: A two-way transport bridge between Gazebo Transport and ROS.
-
ros_gz_sim: Handy startup files and executables for using Gazebo Sim with ROS.
-
ros_gz_sim_demos: ROS and Gazebo integration example.
-
ros_gz_point_cloud: A plugin for publishing point clouds from Gazebo Sim simulations to ros.
Among them, ros_gz_bridge provides a network bridge that enables messages to be exchanged between ROS2 and Gazebo Transport. Its support is limited to certain message types, the following message types can bridge topics:
| 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 |
and the following services:
| 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 <name-of-pkg> --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
创客智造：www.ncnynl.com
rclpy：rclpy — rclpy 0.6.1 documentation
rosdep：ROS packages — rosdep 0.22.1 documentation
超载巴赫朋克：超载巴赫朋克 - OverloadBachPunk