古月·ROS2入门21讲——学习笔记
第一讲：ROS/ROS2是什么
1. ROS的诞生
对于越来越复杂的智能机器人系统，已经不是一个人或者一个团队可以独立完成的，如何高效开发机器人，是技术层面上非常重要的一个问题，针对这个问题，一群斯坦福大学的有志青年尝试给出一个答案，那就是机器人操作系统。
我们可以看到PR2机器人已经可以完成叠毛巾、熨烫衣服、打台球、剪头发等一系列复杂的应用功能，以叠毛巾为例，这在当时是轰动机器人圈的重要研究，因为第一次有机器人可以完成柔性物体的处理，虽然效率很低，在100分钟之内只完成了5条毛巾的整理，但是在学术层面却推动机器人向前走了一大步。
这款机器人中的软件框架就是ROS的原型，所以ROS因这款个人服务机器人而生，很快也从中独立出来，成为一款用于更多机器人的软件系统。
2. ROS的发展
ROS诞生于2007年的斯坦福大学，这是早期PR2机器人的原型，这个项目很快被一家商业公司Willow Garage看中，类似现在的风险投资一样，他们投了一大笔钱给这群年轻人，PR2机器人在资本的助推下成功诞生。
2010年，随着PR2机器人的发布，其中的软件正式确定了名称，就叫做机器人操作系统，Robot Operating System，简称为ROS。同年，ROS也肩负着让更多人使用的使命，正式开源。
PR2机器人虽好，但是成本居高不下，几百万的价格让绝大部分开发者望而却步，官方也注意到了这个问题，所以在2011年发布了一款后期成为ROS圈爆款的机器人——Turtlebot，这款机器人采用扫地机器人的底盘，加上xbox游戏机中的体感传感器Kinect，直接使用笔记本电脑就可以控制，支持ROS的所有开源功能，关键是价格便宜，随着这款机器人的普及，大大推动了ROS的应用。
从2012年开始，使用ROS的人越来越多，ROS官方也开始每年举办一届ROS开发者大会——ROS Conference，简称ROSCon，来自全球的开发者会齐聚一堂分享自己使用ROS开发的机器人应用，其中不乏亚马逊、Intel、微软等大公司的身影，近两年因为疫情原因改为线上举办，名称也变为了ROS World。
经历前几年野蛮而快速的增长，ROS逐渐迭代稳定，2014年起，ROS跟随Ubuntu系统，每两年推出一个长期支持版，每个版本支持五年时间，这标志着ROS的成熟，也让ROS加快了普及的步伐。
回到时间轴的起点，ROS的创始团队原本只想做一款个人服务机器人，万万没想到，ROS被越来越多机器人使用，受限于当初设计的局限性，ROS的问题也逐渐暴露。为了能够真正设计一款适用于所有机器人的操作系统，ROS2在2017年底正式发布，历经多年迭代，我们终于在2022年5月底，迎来了ROS2第一个长期支持版——ROS2 Humble，ROS2已经成熟，我们也进入了一个全新的ROS2时代。
从ROS发展的时间轴中，我们不仅可以了解到ROS的发展过程，更重要的是熟悉ROS和ROS2诞生的原因。
这里我们也把ROS2发展的时间轴单独提取出来，介于ROS在各种各样机器人中应用的问题，ROS2在2014年提出，2015年开始迭代，2017年推出第一个正式版，此后快速迭代，直到2022年推出了第一个长期支持版。
3. ROS的特点
ROS以“提高机器人软件复用率”为目标，这个目标简单来讲就是不要重新造轮子。
| 一个目标 |
|
| 五个特点 |
|
| 四位一体 |
|
4. ROS的社区
ROS全球社区有几个重要网站：
- answers.ros.org，这是一个ROS问答网站，大家可以在上边提出任何关于ROS的问题，全球很多开发者都很乐意回答我们的问题；
- wiki.ros.org，这是ROS的维基百科，记录了ROS教程和各种功能包的使用；
- discourse.ros.org，这是ROS论坛，关于ROS开发的新鲜事都可以在这里发表和查看，比如ROS的活动、新功能包的发布等等。
- index.ros.org，是ROS各种资源的一个索引网站；
- packages.ros.org，是ROS功能包存储的数据库。
5. 21讲的结构
课程分为4部分
第二讲：ROS2对比ROS1
1. ROS1的局限性
ROS最早的设计目标就是开发这样一款PR2家庭服务机器人，这款机器人绝大部分时间都是独立工作，为了让他具备充足的能力：
- 它搭载了工作站级别的计算平台和各种先进的通信设备，不用担忧算力不够，有足够的实力支持各种复杂的实时运算和处理；
- 由于是单兵作战，通信绝大部分都自己内部完成，那就可以用有线连接，保证了良好的网络连接，没有丢数据或者黑客入侵的风险；
- 这台机器人最终虽然小批量生产，但是由于高昂的成本和售价，也只能用于学术研究。
随着ROS的普及，应用ROS的机器人类型已经和PR2机器人有了天翻地覆的问题，也并不具备PR2这样的条件，那原本针对PR2设计的软件框架，就会出现一些问题，比如：
- 要在资源有限的嵌入式系统中运行；
- 要在有干扰的地方保证通信的可靠性；
- 要做成产品走向市场，甚至用在自动驾驶汽车和航天机器人上。
类似的问题不断涌现，致使更加适合各种机器人应用的新一代ROS诞生了，也就是我们课程的主角——ROS2。
2. 全新的ROS2
在设计之初，就考虑到要满足各种各样机器人应用的需求。
- 多机器人系统：未来机器人一定不会是独立的个体，机器人和机器人之间也需要通信和协作，ROS2为多机器人系统的应用提供了标准方法和通信机制。
- 跨平台：机器人应用场景不同，使用的控制平台也会有很大差异，比如自动驾驶汽车中的算力性能肯定比AMR机器人强很多，为了让所有机器人都可以运行ROS2，ROS2可以跨平台运行于Linux、Windows、MacOS、RTOS，甚至是没有任何系统的微控制器（MCU）上，这样我们就不用纠结自己的控制器能不能用ROS了。
- 实时性：机器人运动控制和很多行为策略要求机器人具备实时性，比如机器人要可靠得在100ms内发现前方的行人，或者稳定的在1ms周期内完成运动学、动力学的解算，ROS2为类似这样的实时性需求提供了基本保障。
- 网络连接：无论在怎样的网络环境下，ROS2都可以尽量保障机器人大量数据的完整性和安全性，比如在wifi信号不好的时候数据也要尽力发送过去，在有黑客入侵风险的场景下要对数据进行加密解密。
- 产品化：，大量机器人已经走向我们的生活，未来还会越来越多，ROS2不仅可以用于机器人研发阶段，还可以直接搭载在产品中，走向消费市场，这对ROS2的稳定性、强壮性也提除了巨大挑战。
- 项目管理：机器人开发是一个复杂的系统工程，设计、开发、调试、测试、部署等全流程的项目管理工具和机制，也会在ROS2中体现，更方便我们去开发一款机器人。
与ROS1相比，ROS2对系统架构和软件代码全部进行了重新设计和实现，体现在以下几点：
- 系统架构进行了颠覆性的变化，ROS1中所有节点都需要在节点管理器ROS Master的管理下进行工作，一旦Master出现问题，系统就面临宕机的风险，ROS2实现了真正的分布式，不再有Master这个角色，借助一种全新的通信框架DDS，为所有节点的通信提供可靠保障。
- 软件API进行了重新设计，ROS1原有的接口已经无法满足需求，ROS2结合C++最新标准和Python3语言特性，设计了更具通用性的API，虽然导致原有ROS1的代码无法直接在ROS2中运行，但是尽量保留了类似的使用方法，同时提供了大量移植的说明。
- 编译系统进行了升级，ROS1中使用的rosbuild和catkin问题诸多，尤其是针对代码较多的大项目以及Python编写的项目，编译、链接经常会出错，ROS2对这些问题也进行了优化，重新优化后的编译系统叫做ament和colcon，大家在后续的课程中就会体验到新版编译器的使用方法。
这几点只是框架层面
3. ROS2 vs ROS1
3.1 系统架构
在这张图中，左侧是ROS1，右侧是ROS2，大家注意看两者最明显的变化，那就是Master。
- 在ROS1中，应用层里Master这个节点管理器的角色至关重要，所有节点都得听它指挥，类似是一个公司的CEO，有且只有一个，如果这个CEO突然消失，公司肯定会成一团乱麻。ROS2把这个最不稳定的角色请走了，节点可以通过另外一套discovery——自发现机制，找到彼此，从而建立稳定的通信连接。
- 中间层是ROS封装好的标准通信接口，我们写程序的时候，会频繁和这些通信接口打交道，比如发布一个图像的数据，接收一个雷达的信息，客户端库会再调用底层复杂的驱动和通信协议，让我们的开发变得更加简单明了。
- 在ROS1中，ROS通信依赖底层的TCP和UDP协议，而在ROS2中，通信协议更换成了更加复杂但也更加完善的DDS系统。
- 如果是在进程内需要进行大量数据的通信，ROS1和ROS2都提供了基于共享内存的通信方法，只不过名字不太一样而已。
- 最下边是系统层，也就是可以将ROS安装在哪些操作系统上，ROS1主要安装在Linux上，ROS2的可选项就很多了，Linux、windows、MacOS、RTOS都可以。
3.2 DDS通信
ROS2相比ROS1最大的变化，除了省略了Master之外，应该就是通信系统的变化了。ROS1中基于TCP/UDP的通信系统，频繁诟病于延迟、丢数据、无法加密等问题，ROS2中的DDS在通信层面的功能就丰富多了。
DDS其实是物联网中广泛应用的一种通信协议，类似于我们常听说的5G通信一样，DDS是一个国际标准，能够实现该标准的软件系统并不是唯一的，所以我们可以选择多个厂家提供的DDS系统，比如这里的OpenSplice、FastRTPS，还有更多厂家提供的，每一家的性能不同，适用的场景也不同。
不过这就带来一个问题，每个DDS厂家的软件接口肯定是不一样的，如果我们按照某一家的接口写完了程序，想要切换其他厂家的DDS，不是要重新写代码么？这当然不符合ROS提高软件复用率的目标。
为了解决这个问题，ROS2设计了一个ROS Middleware，简称RMW，也就是指定一个标准的接口，比如如何发数据，如何收数据，数据的各种属性如何配置，都定义好了，如果厂家想要接入ROS社区，就得按照这个标准写一个适配的接口，把自家的DDS给移植过来，这样就把问题交给了最熟悉自家DDS的厂商。对于我们这些用户来讲，某一个DDS用的不爽，只要安装另一个，然后做一个简单的配置，程序一行的都不用改，轻松更换底层的通信系统。
举一个例子，比如我们在产品开发时，可以先用开源版本的DDS满足基本需求，部署交付的产品时，再更换为商业版本更稳定的DDS，这样可以减少开发成本。
总之，DDS的加入，让ROS2系统更加稳定，也更加灵活，当然复杂度也会高一些。这样，我们不用再纠结ROS的通信系统是否稳定、该如何优化等问题，更多精力都可以放在其他三个部分，专注优化我们的机器人应用功能。
3.3 核心概念
大家已经熟悉了ROS1的开发方式以及其中的很多概念，ROS2尽量保留了这些概念。
第三讲：ROS2安装方法
安装教程：
ROS2安装方法 - ROS2入门教程 (guyuehome.com)
我这里也是用虚拟机安装了ubuntu22.04来安装ros2的，我的双系统是ubuntu20.04且安装了ros1，不知道什么原因并不能安装ros2，才用了虚拟机。
虚拟机安装只有这一个问题，下面是解决办法
解决raw.githubusercontent无法访问(仅限终端获取文件) - 知乎 (zhihu.com)
第四讲：ROS2命令行操作
1. Linux的常用命令
cd
-
语法：cd <目录路径>
-
功能：改变工作目录。若没有指定“目录路径”，则回到用户的主目录
pwd
- 语法：pwd
- 功能：此命令显示出当前工作目录的绝对路径
mkdir
-
语法：mkdir [选项] <目录名称>
-
功能：创建一个目录/文件夹
ls
-
语法：ls [选项] [目录名称…]
-
功能：列出目录/文件夹中的文件列表
gedit
-
语法：gedit <文件名称>
-
功能：打开gedit编辑器编辑文件，若没有此文件则会新建
mv
-
语法：mv [选项] <源文件或目录> <目地文件或目录>
-
功能：为文件或目录改名或将文件由一个目录移入另一个目录中
cp
-
语法：cp [选项] <源文件名称或目录名称> <目的文件名称或目录名称>
-
功能：把一个文件或目录拷贝到另一文件或目录中，或者把多个源文件复制到目标目录中
rm
-
语法：rm [选项] <文件名称或目录名称…>
-
功能：该命令的功能为删除一个目录中的一个或多个文件或目录，它也可以将某个目录及其下的所有文件及子目录均删除。对于链接文件，只是删除了链接，原有文件均保持不变
sudo
-
语法：sudo [选项] [指令]
-
功能：以系统管理员权限来执行指令
2. ROS2中的命令行
ROS2命令行的操作机制与Linux相同，不过所有操作都集成在一个ros2的总命令中，后边第一个参数表示不同的操作目的，比如node表示对节点的操作，topic表示对话题的操作，具体操作干什么，还可以在后边继续跟一系列参数内容。
2.1 运行节点程序
想要运行ROS2中某个节点，我们可以使用ros2 run命令进行操作，例如我们要运行海龟仿真节点和键盘控制节点：
ros2 run turtlesim turtlesim_node
ros2 run turtlesim turtle_teleop_key
2.2 查看节点信息
当前运行的ROS系统中都有哪些节点呢？可以这样来查看：
ros2 node list
如果对某一个节点感兴趣，加上一个info子命令，就可以知道它的详细信息啦：
ros2 node info /turtlesim 
2.3 查看话题信息
当前系统中都有话题呢，使用如下命令即可查看：
ros2 topic list
还想看到某一个话题中的消息数据，加上echo子命令试一试：
ros2 topic echo /turtle1/pose 2.4 发布话题消息
想要控制海龟动起来，我们还可以直接通过命令行发布话题指令：
ros2 topic pub --rate 1 /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}}"
2.5 发送服务请求
一只海龟太孤单，仿真器还提供改了一个服务——产生海龟，我们试一试服务调用，再来一只海龟：
ros2 service call /spawn turtlesim/srv/Spawn "{x: 2, y: 2, theta: 0.2, name: ''}"
2.6 发送动作目标
想要让海龟完成一个具体动作，比如转到指定角度，仿真器中提供的这个action可以帮上忙，通过命令行这样发送动作目标：
ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "theta: 3"
2.7 录制控制命令
系统运行中的数据有很多，如果想要把某段数据录制下来，回到实验室再复现这段数据如何？ROS2中的rosbag功能还是很好用的，轻松实现数据的录制与播放：
ros2 bag record /turtle1/cmd_vel
ros2 bag play rosbag2_2022_04_11-17_35_40/rosbag2_2022_04_11-17_35_40_0.db3
第五讲：ROS2开发环境配置
Linux中安装git
sudo apt install gitgit clone https://gitee.com/guyuehome/ros2_21_tutorials.gitVSCode
官方网站：Visual Studio Code - Code Editing. Redefined
VSCode插件配置
- Chinese
- Python
- C++
- CMake
- vscode-icons
- ROS
- Msg Language Support
- Visual Studio IntelliCode
- URDF
- Markdown All in One
第六讲：工作空间与功能包
1. 工作空间是什么
类似的，在ROS机器人开发中，我们针对机器人某些功能进行代码开始时，各种编写的代码、参数、脚本等文件，也需要放置在某一个文件夹里进行管理，这个文件夹在ROS系统中就叫做工作空间。
ROS系统中一个典型的工作空间结构如图所示，这个dev_ws就是工作空间的根目录，里边会有四个子目录，或者叫做四个子空间。
- src，代码空间，未来编写的代码、脚本，都需要人为的放置到这里；
- build，编译空间，保存编译过程中产生的中间文件；
- install，安装空间，放置编译得到的可执行文件和脚本；
- log，日志空间，编译和运行过程中，保存各种警告、错误、信息等日志。
这里也要强调一点，工作空间的名称我们可以自己定义，数量也并不是唯一的。
1.1 创建工作空间
使用如下命令创建一个工作空间，并且下载教程的代码：
$ mkdir -p ~/dev_ws/src
$ cd ~/dev_ws/src
$ git clone https://gitee.com/guyuehome/ros2_21_tutorials.git
1.2 自动安装依赖
$ sudo apt install -y python3-pip
$ sudo pip3 install rosdepc
$ sudo rosdepc init
$ rosdepc update
$ cd ..
$ rosdepc install -i --from-path src --rosdistro humble -y
1.3 编译工作空间
依赖安装完成后，就可以使用如下命令编译工作空间啦，如果有缺少的依赖，或者代码有错误，编译过程中会有报错，否则编译过程应该不会出现任何错误：
$ sudo apt install python3-colcon-ros
$ cd ~/dev_ws/
$ colcon build
编译成功后，就可以在工作空间中看到自动生产的build、log、install文件夹了。
1.4 设置环境变量
$ source install/local_setup.sh # 仅在当前终端生效
$ echo " source ~/dev_ws/install/local_setup.sh" >> ~/.bashrc # 所有终端均生效
2. 功能包
功能包原理：我们把不同功能的代码划分到不同的功能包中，尽量降低他们之间的耦合关系，当需要在ROS社区中分享给别人的时候，只需要说明这个功能包该如何使用，别人很快就可以用起来了。
2.1 创建功能包
$ ros2 pkg create --build-type <build-type> <package_name>
ros2命令中：
- pkg：表示功能包相关的功能；
- create：表示创建功能包；
- build-type：表示新创建的功能包是C++还是Python的，如果使用C++或者C，那这里就跟ament_cmake，如果使用Python，就跟ament_python；
- package_name：新建功能包的名字。
比如在终端中分别创建C++和Python版本的功能包：
$ cd ~/dev_ws/src
$ ros2 pkg create --build-type ament_cmake learning_pkg_c # C++
$ ros2 pkg create --build-type ament_python learning_pkg_python # Python
2.2 编译功能包
在创建好的功能包中，我们可以继续完成代码的编写，之后需要编译和配置环境变量，才能正常运行：
$ cd ~/dev_ws
$ colcon build # 编译工作空间所有功能包
$ source install/local_setup.bash
2.3 功能包的结构
功能包并不是普通的文件夹，那如何判断一个文件夹是否是功能包呢？我们来分析下刚才新创建两个功能包的结构。
2.3.1 C++功能包
首先看下C++类型的功能包，其中必然存在两个文件：package.xml和CMakerLists.txt。
package.xml文件的主要内容，包含功能包的版权描述，和各种依赖的声明。
CMakeLists.txt文件是编译规则，C++代码需要编译才能运行，所以必须要在该文件中设置如何编译，使用CMake语法。
2.3.2 Python功能包
C++功能包需要将源码编译成可执行文件，但是Python语言是解析型的，不需要编译，所以会有一些不同，但也会有这两个文件：package.xml和setup.py。
package.xml文件的主要内容和C++版本功能包一样，包含功能包的版权描述，和各种依赖的声明。
setup.py文件里边也包含一些版权信息，除此之外，还有“entry_points”配置的程序入口，在后续编程讲解中，我们会给大家介绍如何使用。
第七讲：节点
机器人是各种功能的综合体，每一项功能就像机器人的一个工作细胞，众多细胞通过一些机制连接到一起，成为了一个机器人整体。
在ROS中，我们给这些 “细胞”取了一个名字，那就是节点。
1. 通信模型
完整的机器人系统可能并不是一个物理上的整体，比如这样一个的机器人：
在机器人身体里搭载了一台计算机A，它可以通过机器人的眼睛——摄像头，获取外界环境的信息，也可以控制机器人的腿——轮子，让机器人移动到想要去的地方。除此之外，可能还会有另外一台计算机B，放在你的桌子上，它可以远程监控机器人看到的信息，也可以远程配置机器人的速度和某些参数，还可以连接一个摇杆，人为控制机器人前后左右运动。
这些功能虽然位于不同的计算机中，但都是这款机器人的工作细胞，也就是节点，他们共同组成了一个完整的机器人系统。
- 节点在机器人系统中的职责就是执行某些具体的任务，从计算机操作系统的角度来看，也叫做进程；
- 每个节点都是一个可以独立运行的可执行文件，比如执行某一个python程序，或者执行C++编译生成的结果，都算是运行了一个节点；
- 既然每个节点都是独立的执行文件，那自然就可以想到，得到这个执行文件的编程语言可以是不同的，比如C++、Python，乃至Java、Ruby等更多语言。
- 这些节点是功能各不相同的细胞，根据系统设计的不同，可能位于计算机A，也可能位于计算机B，还有可能运行在云端，这叫做分布式，也就是可以分布在不同的硬件载体上；
- 每一个节点都需要有唯一的命名，当我们想要去找到某一个节点的时候，或者想要查询某一个节点的状态时，可以通过节点的名称来做查询。
2. 案例一：Hello World节点（面向过程）
$ ros2 run learning_node node_helloworld
代码解析
learning_node/node_helloworld.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2节点示例-发布“Hello World”日志信息, 使用面向过程的实现方式
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
import time
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = Node("node_helloworld") # 创建ROS2节点对象并进行初始化
while rclpy.ok(): # ROS2系统是否正常运行
node.get_logger().info("Hello World") # ROS2日志输出
time.sleep(0.5) # 休眠控制循环时间
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'node_helloworld = learning_node.node_helloworld:main',
],
3. 创建节点流程
代码中出现的函数大家未来会经常用到，大家先不用纠结函数的具体使用方法，更重要的是理解节点的编码流程。
总结一下，想要实现一个节点，代码的实现流程是这样做：
- 编程接口初始化
- 创建节点并初始化
- 实现节点功能
- 销毁节点并关闭接口
大家如果有学习过C++或者Pyhton的话，应该可以发现这里我们使用的是面向过程的编程方法，这种方式虽然实现简单，但是对于稍微复杂一点的机器人系统，就很难做到模块化编码。
4. 案例二：Hello World节点（面向对象）
$ ros2 run learning_node node_helloworld_class
代码解析
learning_node/node_helloworld_class.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2节点示例-发布“Hello World”日志信息, 使用面向对象的实现方式
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
import time
"""
创建一个HelloWorld节点, 初始化时输出“hello world”日志
"""
class HelloWorldNode(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
while rclpy.ok(): # ROS2系统是否正常运行
self.get_logger().info("Hello World") # ROS2日志输出
time.sleep(0.5) # 休眠控制循环时间
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = HelloWorldNode("node_helloworld_class") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'node_helloworld = learning_node.node_helloworld:main',
'node_helloworld_class = learning_node.node_helloworld_class:main',
],
5. 案例三：物体识别节点
$ sudo apt install python3-opencv
$ ros2 run learning_node node_object #注意修改图片路径后重新编译
运行前需要将learning_node/node_object.py代码中的图片路径，修改为实际路径，修改后重新编译运行即可：
image = cv2.imread('/home/hcx/dev_ws/src/ros2_21_tutorials/learning_node/learning_node/apple.jpg')
例程运行成功后，会弹出一个可视化窗口，可以看到苹果被成功识别啦，一个绿色框会把苹果的轮廓勾勒出来，中间的绿点表示中心点。
代码解析
learning_node/node_object.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2节点示例-通过颜色识别检测图片中出现的苹果
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
import cv2 # OpenCV图像处理库
import numpy as np # Python数值计算库
lower_red = np.array([0, 90, 128]) # 红色的HSV阈值下限
upper_red = np.array([180, 255, 255]) # 红色的HSV阈值上限
def object_detect(image):
hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # 图像从BGR颜色模型转换为HSV模型
mask_red = cv2.inRange(hsv_img, lower_red, upper_red) # 图像二值化
contours, hierarchy = cv2.findContours(mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测
for cnt in contours: # 去除一些轮廓面积太小的噪声
if cnt.shape[0] < 150:
continue
(x, y, w, h) = cv2.boundingRect(cnt) # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)# 将苹果的轮廓勾勒出来
cv2.circle(image, (int(x+w/2), int(y+h/2)), 5, (0, 255, 0), -1)# 将苹果的图像中心点画出来
cv2.imshow("object", image) # 使用OpenCV显示处理后的图像效果
cv2.waitKey(0)
cv2.destroyAllWindows()
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = Node("node_object") # 创建ROS2节点对象并进行初始化
node.get_logger().info("ROS2节点示例：检测图片中的苹果")
image = cv2.imread('/home/hcx/dev_ws/src/ros2_21_tutorials/learning_node/learning_node/apple.jpg') # 读取图像
object_detect(image) # 苹果检测
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'node_helloworld = learning_node.node_helloworld:main',
'node_helloworld_class = learning_node.node_helloworld_class:main',
'node_object = learning_node.node_object:main',
],
6. 案例四：机器视觉识别节点
$ ros2 run learning_node node_object_webcam #注意设置摄像头
如果是在虚拟机中操作，需要进行以下设置： 1. 把虚拟机设置为兼容USB3.1； 2. 在可移动设备中将摄像头连接至虚拟机。
运行成功后，该节点就可以驱动摄像头，并且实时识别摄像头中的红色物体啦。
代码解析
相比之前的程序，这里最大的变化是修改了图片的来源，使用OpenCV中的VideoCapture()来驱动相机，并且周期read摄像头的信息，并进行识别。
learning_node/node_object_webcam.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2节点示例-通过摄像头识别检测图片中出现的苹果
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
import cv2 # OpenCV图像处理库
import numpy as np # Python数值计算库
lower_red = np.array([0, 90, 128]) # 红色的HSV阈值下限
upper_red = np.array([180, 255, 255]) # 红色的HSV阈值上限
def object_detect(image):
hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # 图像从BGR颜色模型转换为HSV模型
mask_red = cv2.inRange(hsv_img, lower_red, upper_red) # 图像二值化
contours, hierarchy = cv2.findContours(mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测
for cnt in contours: # 去除一些轮廓面积太小的噪声
if cnt.shape[0] < 150:
continue
(x, y, w, h) = cv2.boundingRect(cnt) # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2) # 将苹果的轮廓勾勒出来
cv2.circle(image, (int(x+w/2), int(y+h/2)), 5, (0, 255, 0), -1) # 将苹果的图像中心点画出来
cv2.imshow("object", image) # 使用OpenCV显示处理后的图像效果
cv2.waitKey(50)
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = Node("node_object_webcam") # 创建ROS2节点对象并进行初始化
node.get_logger().info("ROS2节点示例：检测图片中的苹果")
cap = cv2.VideoCapture(0)
while rclpy.ok():
ret, image = cap.read() # 读取一帧图像
if ret == True:
object_detect(image) # 苹果检测
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'node_helloworld = learning_node.node_helloworld:main',
'node_helloworld_class = learning_node.node_helloworld_class:main',
'node_object = learning_node.node_object:main',
'node_object_webcam = learning_node.node_object_webcam:main',
],
第八讲：话题
1. 通信模型
-
发布/订阅模型
从话题本身的实现角度来看，使用了基于DDS的发布/订阅模型，什么叫发布和订阅呢？
话题数据传输的特性是从一个节点到另外一个节点，发送数据的对象称之为发布者，接收数据的对象称之为订阅者，每一个话题都需要有一个名字，传输的数据也需要有固定的数据类型。
-
多对多通信
因为话题是多对多的模型，发布控制指令的摇杆可以有一个，也可以有2个、3个，订阅控制指令的机器人可以有1个，也可以有2个、3个，大家可以想象一下这个画面，似乎还是挺魔性的，如果存在多个发送指令的节点，建议大家要注意区分优先级，不然机器人可能不知道该听谁的了。
-
异步通信
话题通信还有一个特性，那就是异步，这个词可能有同学是第一次听说？所谓异步，只要是指发布者发出数据后，并不知道订阅者什么时候可以收到。
异步的特性也让话题更适合用于一些周期发布的数据，比如传感器的数据，运动控制的指令等等，如果某些逻辑性较强的指令，比如修改某一个参数，用话题传输就不太合适了。
-
消息接口
最后，既然是数据传输，发布者和订阅者就得统一数据的描述格式，不能一个说英文，一个理解成了中文。在ROS中，话题通信数据的描述格式称之为消息，对应编程语言中数据结构的概念。比如这里的一个图像数据，就会包含图像的长宽像素值、每个像素的RGB等等，在ROS中都有标准定义。
消息是ROS中的一种接口定义方式，与编程语言无关，我们也可以通过.msg后缀的文件自行定义，有了这样的接口，各种节点就像积木块一样，通过各种各样的接口进行拼接，组成复杂的机器人系统。
2. 案例一：Hello World话题通信
从Hello World例程开始，我们来创建一个发布者，发布话题“chatter”，周期发送“Hello World”这个字符串，消息类型是ROS中标准定义的String，再创建一个订阅者，订阅“chatter”这个话题，从而接收到“Hello World”这个字符串。
启动第一个终端，运行话题的发布者节点：
$ ros2 run learning_topic topic_helloworld_pub
启动第二个终端，运行话题的发布者节点 2：
$ ros2 run learning_topic topic_helloLLL_pub
启动第三个终端，运行话题的订阅者节点：
$ ros2 run learning_topic topic_all_sub
可以看到发布者循环发布“Hello World” 和“Hello LLL”字符串消息，订阅者也以几乎同样的频率收到该话题的消息数据。
发布者代码解析
1. learning_topic/topic_helloworld_pub.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-发布“Hello World”话题
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from std_msgs.msg import String # 字符串消息类型
"""
创建一个发布者节点
"""
class PublisherNode(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.pub = self.create_publisher(String, "chatter", 10) # 创建发布者对象（消息类型、话题名、队列长度）
self.timer = self.create_timer(0.5, self.timer_callback) # 创建一个定时器（单位为秒的周期，定时执行的回调函数）
def timer_callback(self): # 创建定时器周期执行的回调函数
msg = String() # 创建一个String类型的消息对象
msg.data = 'Hello World' # 填充消息对象中的消息数据
self.pub.publish(msg) # 发布话题消息
self.get_logger().info('Publishing: "%s"' % msg.data) # 输出日志信息，提示已经完成话题发布
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = PublisherNode("topic_helloworld_pub") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
2. learning_topic/topic_helloLLL_pub.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-发布“Hello World”话题
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from std_msgs.msg import String # 字符串消息类型
"""
创建一个发布者节点
"""
class PublisherNode(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.pub = self.create_publisher(String, "chatter", 10) # 创建发布者对象（消息类型、话题名、队列长度）
self.timer = self.create_timer(0.5, self.timer_callback) # 创建一个定时器（单位为秒的周期，定时执行的回调函数）
def timer_callback(self): # 创建定时器周期执行的回调函数
msg = String() # 创建一个String类型的消息对象
msg.data = 'Hello LLL' # 填充消息对象中的消息数据
self.pub.publish(msg) # 发布话题消息
self.get_logger().info('Publishing: "%s"' % msg.data) # 输出日志信息，提示已经完成话题发布
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = PublisherNode("topic_helloLLL_pub") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
流程总结
对以上程序进行分析，如果我们想要实现一个发布者，流程如下：
- 编程接口初始化
- 创建节点并初始化
- 创建发布者对象
- 创建并填充话题消息
- 发布话题消息
- 销毁节点并关闭接口
订阅者代码解析
程序实现
learning_topic/topic_all_sub.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-同时订阅多个话题
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from std_msgs.msg import String # ROS2标准定义的String消息
"""
创建一个订阅者节点
"""
class SubscriberNode(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.sub_chatter = self.create_subscription(\
String, "chatter", self.listener_callback_chatter, 10) # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
self.sub_chatter1 = self.create_subscription(\
String, "chatter1", self.listener_callback_chatter1, 10) # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
def listener_callback_chatter(self, msg): # chatter话题的回调函数
self.get_logger().info('I heard chatter: "%s"' % msg.data) # 输出日志信息，提示订阅收到的话题消息
def listener_callback_chatter1(self, msg): # chatter1话题的回调函数
self.get_logger().info('I heard chatter1: "%s"' % msg.data) # 输出日志信息，提示订阅收到的话题消息
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = SubscriberNode("topic_all_sub") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'topic_helloworld_pub = learning_topic.topic_helloworld_pub:main',
'topic_helloLLL_pub = learning_topic.topic_helloLLL_pub:main',
'topic_helloworld_sub = learning_topic.topic_helloworld_sub:main',
'topic_helloLLL_sub = learning_topic.topic_helloLLL_sub:main',
'topic_all_sub = learning_topic.topic_all_sub:main',
],
},流程总结
对以上程序进行分析，如果我们想要实现一个订阅者，流程如下：
- 编程接口初始化
- 创建节点并初始化
- 创建订阅者对象
- 回调函数处理话题数据
- 销毁节点并关闭接口
3. 案例二：机器视觉识别
在节点概念的讲解过程中，我们通过一个节点驱动了相机，并且实现了对红色物体的识别。功能虽然没问题，但是对于机器人开发来讲，并没有做到程序的模块化，更好的方式是将相机驱动和视觉识别做成两个节点，节点间的联系就是这个图像数据，通过话题周期传输即可。
$ ros2 run learning_topic topic_webcam_pub
$ ros2 run learning_topic topic_webcam_sub
将红色物体放入相机范围内，即可看到识别效果。
发布者代码解析
learning_topic/topic_webcam_pub.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-发布图像话题
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from sensor_msgs.msg import Image # 图像消息类型
from cv_bridge import CvBridge # ROS与OpenCV图像转换类
import cv2 # Opencv图像处理库
"""
创建一个发布者节点
"""
class ImagePublisher(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.publisher_ = self.create_publisher(Image, 'image_raw', 10) # 创建发布者对象（消息类型、话题名、队列长度）
self.timer = self.create_timer(0.1, self.timer_callback) # 创建一个定时器（单位为秒的周期，定时执行的回调函数）
self.cap = cv2.VideoCapture(0) # 创建一个视频采集对象，驱动相机采集图像（相机设备号）
self.cv_bridge = CvBridge() # 创建一个图像转换对象，用于稍后将OpenCV的图像转换成ROS的图像消息
def timer_callback(self):
ret, frame = self.cap.read() # 一帧一帧读取图像
if ret == True: # 如果图像读取成功
self.publisher_.publish(
self.cv_bridge.cv2_to_imgmsg(frame, 'bgr8')) # 发布图像消息
self.get_logger().info('Publishing video frame') # 输出日志信息，提示已经完成图像话题发布
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = ImagePublisher("topic_webcam_pub") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
订阅者代码解析
learning_topic/topic_webcam_sub.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2话题示例-订阅图像话题
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from sensor_msgs.msg import Image # 图像消息类型
from cv_bridge import CvBridge # ROS与OpenCV图像转换类
import cv2 # Opencv图像处理库
import numpy as np # Python数值计算库
lower_red = np.array([0, 90, 128]) # 红色的HSV阈值下限
upper_red = np.array([180, 255, 255]) # 红色的HSV阈值上限
"""
创建一个订阅者节点
"""
class ImageSubscriber(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.sub = self.create_subscription(
Image, 'image_raw', self.listener_callback, 10) # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
self.cv_bridge = CvBridge() # 创建一个图像转换对象，用于OpenCV图像与ROS的图像消息的互相转换
def object_detect(self, image):
hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # 图像从BGR颜色模型转换为HSV模型
mask_red = cv2.inRange(hsv_img, lower_red, upper_red) # 图像二值化
contours, hierarchy = cv2.findContours(
mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测
for cnt in contours: # 去除一些轮廓面积太小的噪声
if cnt.shape[0] < 150:
continue
(x, y, w, h) = cv2.boundingRect(cnt) # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)# 将苹果的轮廓勾勒出来
cv2.circle(image, (int(x+w/2), int(y+h/2)), 5,
(0, 255, 0), -1) # 将苹果的图像中心点画出来
cv2.imshow("object", image) # 使用OpenCV显示处理后的图像效果
cv2.waitKey(10)
def listener_callback(self, data):
self.get_logger().info('Receiving video frame') # 输出日志信息，提示已进入回调函数
image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8') # 将ROS的图像消息转化成OpenCV图像
self.object_detect(image) # 苹果检测
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = ImageSubscriber("topic_webcam_sub") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
4. 话题命令行操作
话题命令的常用操作如下：
$ ros2 topic list # 查看话题列表
$ ros2 topic info <topic_name> # 查看话题信息
$ ros2 topic hz <topic_name> # 查看话题发布频率
$ ros2 topic bw <topic_name> # 查看话题传输带宽
$ ros2 topic echo <topic_name> # 查看话题数据
$ ros2 topic pub <topic_name> <msg_type> <msg_data> # 发布话题消息
第九讲：服务
1. 通信模型
在需要这个数据的时候，发一个查询的请求，然后尽快得到此时目标的最新位置。
这样的通信模型和话题单向传输有所不同，变成了发送一个请求，反馈一个应答的形式，好像是你问我答一样，这种通信机制在ROS中成为服务，Service。
- 客户端/服务器模型
从服务的实现机制上来看，这种你问我答的形式叫做客户端/服务器模型，简称为CS模型，客户端在需要某些数据的时候，针对某个具体的服务，发送请求信息，服务器端收到请求之后，就会进行处理并反馈应答信息。
这种通信机制在生活中也很常见，比如我们经常浏览的各种网页，此时你的电脑浏览器就是客户端，通过域名或者各种操作，向网站服务器发送请求，服务器收到之后返回需要展现的页面数据。
- 同步通信
这个过程一般要求越快越好，假设服务器半天没有反应，你的浏览器一直转圈圈，那有可能是服务器宕机了，或者是网络不好，所以相比话题通信，在服务通信中，客户端可以通过接收到的应答信息，判断服务器端的状态，我们也称之为同步通信。
- 一对多通信
所以服务通信模型中，服务器端唯一，但客户端可以不唯一。
-
服务接口
和话题通信类似，服务通信的核心还是要传递数据，数据变成了两个部分，一个请求的数据，比如请求苹果位置的命令，还有一个反馈的数据，比如反馈苹果坐标位置的数据，这些数据和话题消息一样，在ROS中也是要标准定义的，话题使用.msg文件定义，服务使用的是.srv文件定义，后续我们会给大家介绍定义的方法。
2. 案例一：加法求解器
当我们需要计算两个加数的求和结果时，就通过客户端节点，将两个加数封装成请求数据，针对服务“add_two_ints”发送出去，提供这个服务的服务器端节点，收到请求数据后，开始进行加法计算，并将求和结果封装成应答数据，反馈给客户端，之后客户端就可以得到想要的结果啦。
启动两个终端，并运行如下节点，第一个节点是服务端，等待请求数据并提供求和功能，第二个节点是客户端，发送传入的两个加数并等待求和结果。
$ ros2 run learning_service service_adder_server
$ ros2 run learning_service service_adder_client 2 3
-
客户端代码解析
learning_service/service_adder_client.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-发送两个加数，请求加法器计算
"""
import sys
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from learning_interface.srv import AddTwoInts # 自定义的服务接口
class adderClient(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.client = self.create_client(AddTwoInts, 'add_two_ints') # 创建服务客户端对象（服务接口类型，服务名）
while not self.client.wait_for_service(timeout_sec=1.0): # 循环等待服务器端成功启动
self.get_logger().info('service not available, waiting again...')
self.request = AddTwoInts.Request() # 创建服务请求的数据对象
def send_request(self): # 创建一个发送服务请求的函数
self.request.a = int(sys.argv[1])
self.request.b = int(sys.argv[2])
self.future = self.client.call_async(self.request) # 异步方式发送服务请求
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = adderClient("service_adder_client") # 创建ROS2节点对象并进行初始化
node.send_request() # 发送服务请求
while rclpy.ok(): # ROS2系统正常运行
rclpy.spin_once(node) # 循环执行一次节点
if node.future.done(): # 数据是否处理完成
try:
response = node.future.result() # 接收服务器端的反馈数据
except Exception as e:
node.get_logger().info(
'Service call failed %r' % (e,))
else:
node.get_logger().info( # 将收到的反馈信息打印输出
'Result of add_two_ints: for %d + %d = %d' %
(node.request.a, node.request.b, response.sum))
break
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'service_adder_client = learning_service.service_adder_client:main',
],
},
流程总结
对以上程序进行分析，如果我们想要实现一个客户端，流程如下：
- 编程接口初始化
- 创建节点并初始化
- 创建客户端对象
- 创建并发送请求数据
- 等待服务器端应答数据
- 销毁节点并关闭接口
-
服务端代码解析
至于服务器端的实现，有点类似话题通信中的订阅者，并不知道请求数据什么时间出现，也用到了回调函数机制。
程序实现
learning_service/service_adder_server.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-提供加法器的服务器处理功能
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from learning_interface.srv import AddTwoInts # 自定义的服务接口
class adderServer(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.adder_callback) # 创建服务器对象（接口类型、服务名、服务器回调函数）
def adder_callback(self, request, response): # 创建回调函数，执行收到请求后对数据的处理
response.sum = request.a + request.b # 完成加法求和计算，将结果放到反馈的数据中
self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b)) # 输出日志信息，提示已经完成加法求和计算
return response # 反馈应答信息
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = adderServer("service_adder_server") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'service_adder_client = learning_service.service_adder_client:main',
'service_adder_server = learning_service.service_adder_server:main',
],
},
流程总结
对以上程序进行分析，如果我们想要实现一个服务端，流程如下：
- 编程接口初始化
- 创建节点并初始化
- 创建服务器端对象
- 通过回调函数处进行服务
- 向客户端反馈应答结果
- 销毁节点并关闭接口
3. 案例二：机器视觉识别
运行效果
此时会有三个节点出现：
- 相机驱动节点，发布图像数据；
- 视觉识别节点，订阅图像数据，并且集成了一个服务器端对象，随时准备提供目标位置；
- 客户端节点，我们可以认为是一个机器人目标跟踪的节点，当需要根据目标运动时，就发送一次请求，然后拿到一个当前的目标位置。
启动三个终端，分别运行上述三个节点：
$ ros2 run usb_cam usb_cam_node_exe
$ ros2 run learning_service service_object_server
$ ros2 run learning_service service_object_client
-
客户端代码解析
learning_service/service_object_client.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-请求目标识别，等待目标位置应答
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from learning_interface.srv import GetObjectPosition # 自定义的服务接口
class objectClient(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.client = self.create_client(GetObjectPosition, 'get_target_position')
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
self.request = GetObjectPosition.Request()
def send_request(self):
self.request.get = True
self.future = self.client.call_async(self.request)
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = objectClient("service_object_client") # 创建ROS2节点对象并进行初始化
node.send_request()
while rclpy.ok():
rclpy.spin_once(node)
if node.future.done():
try:
response = node.future.result()
except Exception as e:
node.get_logger().info(
'Service call failed %r' % (e,))
else:
node.get_logger().info(
'Result of object position:\n x: %d y: %d' %
(response.x, response.y))
break
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'service_adder_client = learning_service.service_adder_client:main',
'service_adder_server = learning_service.service_adder_server:main',
'service_object_client = learning_service.service_object_client:main',
],
},
-
服务端代码解析
learning_service/service_object_server.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-提供目标识别服务
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from sensor_msgs.msg import Image # 图像消息类型
import numpy as np # Python数值计算库
from cv_bridge import CvBridge # ROS与OpenCV图像转换类
import cv2 # Opencv图像处理库
from learning_interface.srv import GetObjectPosition # 自定义的服务接口
lower_red = np.array([0, 90, 128]) # 红色的HSV阈值下限
upper_red = np.array([180, 255, 255]) # 红色的HSV阈值上限
class ImageSubscriber(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.sub = self.create_subscription(
Image, 'image_raw', self.listener_callback, 10) # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
self.cv_bridge = CvBridge() # 创建一个图像转换对象，用于OpenCV图像与ROS的图像消息的互相转换
self.srv = self.create_service(GetObjectPosition, # 创建服务器对象（接口类型、服务名、服务器回调函数）
'get_target_position',
self.object_position_callback)
self.objectX = 0
self.objectY = 0
def object_detect(self, image):
hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # 图像从BGR颜色模型转换为HSV模型
mask_red = cv2.inRange(hsv_img, lower_red, upper_red) # 图像二值化
contours, hierarchy = cv2.findContours(
mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测
for cnt in contours: # 去除一些轮廓面积太小的噪声
if cnt.shape[0] < 150:
continue
(x, y, w, h) = cv2.boundingRect(cnt) # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)# 将苹果的轮廓勾勒出来
cv2.circle(image, (int(x+w/2), int(y+h/2)), 5,
(0, 255, 0), -1) # 将苹果的图像中心点画出来
self.objectX = int(x+w/2)
self.objectY = int(y+h/2)
cv2.imshow("object", image) # 使用OpenCV显示处理后的图像效果
cv2.waitKey(50)
def listener_callback(self, data):
self.get_logger().info('Receiving video frame') # 输出日志信息，提示已进入回调函数
image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8') # 将ROS的图像消息转化成OpenCV图像
self.object_detect(image) # 苹果检测
def object_position_callback(self, request, response): # 创建回调函数，执行收到请求后对数据的处理
if request.get == True:
response.x = self.objectX # 目标物体的XY坐标
response.y = self.objectY
self.get_logger().info('Object position\nx: %d y: %d' %
(response.x, response.y)) # 输出日志信息，提示已经反馈
else:
response.x = 0
response.y = 0
self.get_logger().info('Invalid command') # 输出日志信息，提示已经反馈
return response
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = ImageSubscriber("service_object_server") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'service_adder_client = learning_service.service_adder_client:main',
'service_adder_server = learning_service.service_adder_server:main',
'service_object_client = learning_service.service_object_client:main',
'service_object_server = learning_service.service_object_server:main',
],
},
4. 服务命令行操作
$ ros2 service list # 查看服务列表
$ ros2 service type <service_name> # 查看服务数据类型
$ ros2 service call <service_name> <service_type> <service_data> # 发送服务请求
话题和服务是ROS中最为常用的两种数据通信方法，前者适合传感器、控制指令等周期性、单向传输的数据，后者适合一问一答，同步性要求更高的数据，比如获取机器视觉识别到的目标位置。
第十讲：通信接口：数据传递的标准结构
1. 接口的定义
软件开发中，接口的使用就更多了，比如我们在编写程序时，使用的函数和函数的输入输出也称之为接口，每一次调用函数的时候，就像是把主程序和调用函数通过这个接口连接到一起，系统才能正常工作。
更为形象的是图形化编程中使用的程序模块，每一个模块都有固定的结构和形状，只有两个模块相互匹配，才能在一起工作，这就很好的讲代码形象化了。
所以什么是接口，它是一种相互关系，只有彼此匹配，才能建立连接。
2. ROS通信接口
接口可以让程序之间的依赖降低，便于我们使用别人的代码，也方便别人使用我们的代码，这就是ROS的核心目标，减少重复造轮子。
ROS有三种常用的通信机制，分别是话题、服务、动作，通过每一种通信种定义的接口，各种节点才能有机的联系到一起。
3. 语言无关
为了保证每一个节点可以使用不同语言编程，ROS将这些接口的设计做成了和语言无关的，比如这里看到的int32表示32位的整型数，int64表示64位的整型数，bool表示布尔值，还可以定义数组、结构体，这些定义在编译过程中，会自动生成对应到C++、Python等语言里的数据结构。
-
话题通信接口的定义使用的是.msg文件，由于是单向传输，只需要描述传输的每一帧数据是什么就行，比如在这个定义里，会传输两个32位的整型数，x、y，我们可以用来传输二维坐标的数值。
-
服务通信接口的定义使用的是.srv文件，包含请求和应答两部分定义，通过中间的“---”区分，比如之前我们学习的加法求和功能，请求数据是两个64位整型数a和b，应答是求和的结果sum。
-
动作是另外一种通信机制，用来描述机器人的一个运动过程，使用.action文件定义，比如我们让小海龟转90度，一边转一边周期反馈当前的状态，此时接口的定义分成了三个部分，分别是动作的目标，比如是开始运动，运动的结果，最终旋转的90度是否完成，还有一个周期反馈，比如每隔1s反馈一下当前转到第10度、20度还是30度了，让我们知道运动的进度。
4. 案例一：服务接口的定义与使用
有三个节点，第一个驱动相机发布图像话题，第二个是机器视觉识别节点，封装了一个服务的服务端对象，提供目标识别位置的查询服务，第三个节点在需要目标位置的时候，就可以发送请求，收到位置进行使用了。
- 接口定义
在这个例程中，我们使用GetObjectPosition.srv定义了服务通信的接口：
learning_interface/srv/GetObjectPosition.srv
bool get # 获取目标位置的指令
---
int32 x # 目标的X坐标
int32 y # 目标的Y坐标
定义中有两个部分，上边是获取目标位置的指令，get为true的话，就表示我们需要一次位置，服务端就会反馈这个x、y坐标了。
完成定义后，还需要在功能包的CMakeLists.txt中配置编译选项，让编译器在编译过程中，根据接口定义，自动生成不同语言的代码：
...
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/GetObjectPosition.srv"
)
...
功能包的package.xml文件中也需要添加代码生成的功能依赖：
...
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
...
- 程序调用
我们在代码中再来重点看下接口的使用方法。
客户端接口调用
learning_service/service_object_client.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-请求目标识别，等待目标位置应答
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from learning_interface.srv import GetObjectPosition # 自定义的服务接口
class objectClient(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.client = self.create_client(GetObjectPosition, 'get_target_position')
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
self.request = GetObjectPosition.Request()
def send_request(self):
self.request.get = True
self.future = self.client.call_async(self.request)
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = objectClient("service_object_client") # 创建ROS2节点对象并进行初始化
node.send_request()
while rclpy.ok():
rclpy.spin_once(node)
if node.future.done():
try:
response = node.future.result()
except Exception as e:
node.get_logger().info(
'Service call failed %r' % (e,))
else:
node.get_logger().info(
'Result of object position:\n x: %d y: %d' %
(response.x, response.y))
break
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
服务端接口调用
learning_service/service_object_server.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2服务示例-提供目标识别服务
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from sensor_msgs.msg import Image # 图像消息类型
import numpy as np # Python数值计算库
from cv_bridge import CvBridge # ROS与OpenCV图像转换类
import cv2 # Opencv图像处理库
from learning_interface.srv import GetObjectPosition # 自定义的服务接口
lower_red = np.array([0, 90, 128]) # 红色的HSV阈值下限
upper_red = np.array([180, 255, 255]) # 红色的HSV阈值上限
class ImageSubscriber(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.sub = self.create_subscription(
Image, 'image_raw', self.listener_callback, 10) # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
self.cv_bridge = CvBridge() # 创建一个图像转换对象，用于OpenCV图像与ROS的图像消息的互相转换
self.srv = self.create_service(GetObjectPosition, # 创建服务器对象（接口类型、服务名、服务器回调函数）
'get_target_position',
self.object_position_callback)
self.objectX = 0
self.objectY = 0
def object_detect(self, image):
hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # 图像从BGR颜色模型转换为HSV模型
mask_red = cv2.inRange(hsv_img, lower_red, upper_red) # 图像二值化
contours, hierarchy = cv2.findContours(
mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测
for cnt in contours: # 去除一些轮廓面积太小的噪声
if cnt.shape[0] < 150:
continue
(x, y, w, h) = cv2.boundingRect(cnt) # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)# 将苹果的轮廓勾勒出来
cv2.circle(image, (int(x+w/2), int(y+h/2)), 5,
(0, 255, 0), -1) # 将苹果的图像中心点画出来
self.objectX = int(x+w/2)
self.objectY = int(y+h/2)
cv2.imshow("object", image) # 使用OpenCV显示处理后的图像效果
cv2.waitKey(50)
def listener_callback(self, data):
self.get_logger().info('Receiving video frame') # 输出日志信息，提示已进入回调函数
image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8') # 将ROS的图像消息转化成OpenCV图像
self.object_detect(image) # 苹果检测
def object_position_callback(self, request, response): # 创建回调函数，执行收到请求后对数据的处理
if request.get == True:
response.x = self.objectX # 目标物体的XY坐标
response.y = self.objectY
self.get_logger().info('Object position\nx: %d y: %d' %
(response.x, response.y)) # 输出日志信息，提示已经反馈
else:
response.x = 0
response.y = 0
self.get_logger().info('Invalid command') # 输出日志信息，提示已经反馈
return response
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = ImageSubscriber("service_object_server") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
5. 案例二：话题接口的定义与使用
话题通信接口的定义也是类似的，继续从之前的机器视觉案例中来衍生，我们想把服务换成话题，周期发布目标识别的位置，不管有没有人需要。
运行效果
现在我们会运行三个节点：
- 第一个节点，将驱动相机并发布图像话题，此时的话题数据使用的是ROS中标准定义的Image图像消息；
- 第二个节点，会运行视觉识别功能，识别目标的位置，这个位置我们希望封装成话题消息，发布出去，谁需要使用谁就来订阅；
- 第三个节点，订阅位置话题，打印到终端中。
启动三个终端，分别运行以上节点：
$ ros2 run usb_cam usb_cam_node_exe
$ ros2 run learning_topic interface_object_pub
$ ros2 run learning_topic interface_object_sub
接口定义
在这个例程中，我们使用ObjectPosition.msg定义了服务通信的接口：
learning_interface/msg/ObjectPosition.msg
int32 x # 表示目标的X坐标
int32 y # 表示目标的Y坐标
话题消息的内容是一个位置，我们使用x、y坐标值进行描述。
完成定义后，还需要在功能包的CMakeLists.txt中配置编译选项，让编译器在编译过程中，根据接口定义，自动生成不同语言的代码：
...
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/ObjectPosition.msg"
)
...
程序调用
我们在代码中再来重点看下接口的使用方法。
发布者接口调用
learning_topic/interface_object_pub.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2接口示例-发布目标位置
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from sensor_msgs.msg import Image # 图像消息类型
from cv_bridge import CvBridge # ROS与OpenCV图像转换类
import cv2 # Opencv图像处理库
import numpy as np # Python数值计算库
from learning_interface.msg import ObjectPosition # 自定义的目标位置消息
lower_red = np.array([0, 90, 128]) # 红色的HSV阈值下限
upper_red = np.array([180, 255, 255]) # 红色的HSV阈值上限
"""
创建一个订阅者节点
"""
class ImageSubscriber(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.sub = self.create_subscription(
Image, 'image_raw', self.listener_callback, 10) # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
self.pub = self.create_publisher(
ObjectPosition, "object_position", 10) # 创建发布者对象（消息类型、话题名、队列长度）
self.cv_bridge = CvBridge() # 创建一个图像转换对象，用于OpenCV图像与ROS的图像消息的互相转换
self.objectX = 0
self.objectY = 0
def object_detect(self, image):
hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # 图像从BGR颜色模型转换为HSV模型
mask_red = cv2.inRange(hsv_img, lower_red, upper_red) # 图像二值化
contours, hierarchy = cv2.findContours(
mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测
for cnt in contours: # 去除一些轮廓面积太小的噪声
if cnt.shape[0] < 150:
continue
(x, y, w, h) = cv2.boundingRect(cnt) # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2) # 将苹果的轮廓勾勒出来
cv2.circle(image, (int(x+w/2), int(y+h/2)), 5, # 将苹果的图像中心点画出来
(0, 255, 0), -1)
self.objectX = int(x+w/2)
self.objectY = int(y+h/2)
cv2.imshow("object", image) # 使用OpenCV显示处理后的图像效果
cv2.waitKey(50)
def listener_callback(self, data):
self.get_logger().info('Receiving video frame') # 输出日志信息，提示已进入回调函数
image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8') # 将ROS的图像消息转化成OpenCV图像
position = ObjectPosition()
self.object_detect(image) # 苹果检测
position.x, position.y = int(self.objectX), int(self.objectY)
self.pub.publish(position) # 发布目标位置
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = ImageSubscriber("topic_webcam_sub") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
订阅者接口调用
learning_topic/interface_object_sub.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2接口示例-订阅目标位置
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from std_msgs.msg import String # 字符串消息类型
from learning_interface.msg import ObjectPosition # 自定义的目标位置消息
"""
创建一个订阅者节点
"""
class SubscriberNode(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.sub = self.create_subscription(\
ObjectPosition, "/object_position", self.listener_callback, 10) # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度
def listener_callback(self, msg): # 创建回调函数，执行收到话题消息后对数据的处理
self.get_logger().info('Target Position: "(%d, %d)"' % (msg.x, msg.y))# 输出日志信息，提示订阅收到的话题消息
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = SubscriberNode("interface_position_sub") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
6. 接口命令行操作
$ ros2 interface list # 查看系统接口列表
$ ros2 interface show <interface_name> # 查看某个接口的详细定义
$ ros2 interface package <package_name> # 查看某个功能包中的接口定义
第十一讲：动作
1. 通信模型
举个例子，比如我们想让机器人转个圈，这肯定不是一下就可以完成的，机器人得一点一点旋转，直到360度才能结束，假设机器人并不在我们眼前，发出指令后，我们根本不知道机器人到底有没有开始转圈，转到哪里了？
现在我们需要的是一个反馈，比如每隔1s，告诉我们当前转到多少度了，10度、20度、30度，一段时间之后，到了360度，再发送一个信息，表示动作执行完成。
这样一个需要执行一段时间的行为，使用动作的通信机制就更为合适，就像装了一个进度条，我们可以随时把控进度，如果运动过程当中，我们还可以随时发送一个取消运动的命令。
客户端/服务器模型
客户端发送一个运动的目标，想让机器人动起来，服务器端收到之后，就开始控制机器人运动，一边运动，一边反馈当前的状态，如果是一个导航动作，这个反馈可能是当前所处的坐标，如果是机械臂抓取，这个反馈可能又是机械臂的实时姿态。当运动执行结束后，服务器再反馈一个动作结束的信息。整个通信过程就此结束。
一对多通信
和服务一样，动作通信中的客户端可以有多个，大家都可以发送运动命令，但是服务器端只能有一个，毕竟只有一个机器人，先执行完成一个动作，才能执行下一个动作。
同步通信
既然有反馈，那动作也是一种同步通信机制，之前我们也介绍过，动作过程中的数据通信接口，使用.action文件进行定义。
由服务和话题合成
大家再仔细看下上边的动图，是不是还会发现一个隐藏的秘密。
动作的三个通信模块，竟然有两个是服务，一个是话题，当客户端发送运动目标时，使用的是服务的请求调用，服务器端也会反馈一个应带，表示收到命令。动作的反馈过程，其实就是一个话题的周期发布，服务器端是发布者，客户端是订阅者。
没错，动作是一种应用层的通信机制，其底层就是基于话题和服务来实现的。
2. 案例一：小海龟的动作
$ ros2 run turtlesim turtlesim_node
$ ros2 run turtlesim turtle_teleop_key
$ ros2 action info /turtle1/rotate_absolute
$ ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: -1.57}"
$ ros2 action send_goal /turtle1/rotate_absolute turtlesim/action/RotateAbsolute "{theta: -1.57}" --feedback
3. 案例二：机器人画圆
动作虽然是基于话题和服务实现的，但在实际使用中，并不会直接使用话题和服务的编程方法，而是有一套针对动作特性封装好的编程接口，接下来我们就一起试一 试。
运行效果
启动两个终端，分别运行一下命令，启动动作示例的服务端和客户端：
$ ros2 run learning_action action_move_server
$ ros2 run learning_action action_move_client
终端中，我们可以看到客户端发送动作目标之后，服务器端开始模拟机器人运动，每30度发送一次反馈信息，最终完成运动，并反馈结束运动的信息。
接口定义
例程使用的动作并不是ROS中的标准定义，我们通过MoveCircle.action进行自定义：
learning_interface/action/MoveCircle.action
bool enable # 定义动作的目标，表示动作开始的指令
---
bool finish # 定义动作的结果，表示是否成功执行
---
int32 state # 定义动作的反馈，表示当前执行到的位置
包含三个部分：
- 第一块是动作的目标，enable为true时，表示开始运动；
- 第二块是动作的执行结果，finish为true，表示动作执行完成；
- 第三块是动作的周期反馈，表示当前机器人旋转到的角度。
完成定义后，还需要在功能包的CMakeLists.txt中配置编译选项，让编译器在编译过程中，根据接口定义，自动生成不同语言的代码：
...
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/MoveCircle.action"
)
...
通信模型
通信模型就是这样，客户端发送给一个动作目标，服务器控制机器人开始运动，并周期反馈，结束后反馈结束信息。
服务端代码解析
learning_action/action_move_server.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2动作示例-负责执行圆周运动动作的服务端
"""
import time
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from rclpy.action import ActionServer # ROS2 动作服务器类
from learning_interface.action import MoveCircle # 自定义的圆周运动接口
class MoveCircleActionServer(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self._action_server = ActionServer( # 创建动作服务器（接口类型、动作名、回调函数）
self,
MoveCircle,
'move_circle',
self.execute_callback)
def execute_callback(self, goal_handle): # 执行收到动作目标之后的处理函数
self.get_logger().info('Moving circle...')
feedback_msg = MoveCircle.Feedback() # 创建一个动作反馈信息的消息
for i in range(0, 360, 30): # 从0到360度，执行圆周运动，并周期反馈信息
feedback_msg.state = i # 创建反馈信息，表示当前执行到的角度
self.get_logger().info('Publishing feedback: %d' % feedback_msg.state)
goal_handle.publish_feedback(feedback_msg) # 发布反馈信息
time.sleep(0.5)
goal_handle.succeed() # 动作执行成功
result = MoveCircle.Result() # 创建结果消息
result.finish = True
return result # 反馈最终动作执行的结果
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = MoveCircleActionServer("action_move_server") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
客户端代码解析
learning_action/action_move_client.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2动作示例-请求执行圆周运动动作的客户端
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from rclpy.action import ActionClient # ROS2 动作客户端类
from learning_interface.action import MoveCircle # 自定义的圆周运动接口
class MoveCircleActionClient(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self._action_client = ActionClient( # 创建动作客户端（接口类型、动作名）
self, MoveCircle, 'move_circle')
def send_goal(self, enable): # 创建一个发送动作目标的函数
goal_msg = MoveCircle.Goal() # 创建一个动作目标的消息
goal_msg.enable = enable # 设置动作目标为使能，希望机器人开始运动
self._action_client.wait_for_server() # 等待动作的服务器端启动
self._send_goal_future = self._action_client.send_goal_async( # 异步方式发送动作的目标
goal_msg, # 动作目标
feedback_callback=self.feedback_callback) # 处理周期反馈消息的回调函数
self._send_goal_future.add_done_callback(self.goal_response_callback) # 设置一个服务器收到目标之后反馈时的回调函数
def goal_response_callback(self, future): # 创建一个服务器收到目标之后反馈时的回调函数
goal_handle = future.result() # 接收动作的结果
if not goal_handle.accepted: # 如果动作被拒绝执行
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)') # 动作被顺利执行
self._get_result_future = goal_handle.get_result_async() # 异步获取动作最终执行的结果反馈
self._get_result_future.add_done_callback(self.get_result_callback) # 设置一个收到最终结果的回调函数
def get_result_callback(self, future): # 创建一个收到最终结果的回调函数
result = future.result().result # 读取动作执行的结果
self.get_logger().info('Result: {%d}' % result.finish) # 日志输出执行结果
def feedback_callback(self, feedback_msg): # 创建处理周期反馈消息的回调函数
feedback = feedback_msg.feedback # 读取反馈的数据
self.get_logger().info('Received feedback: {%d}' % feedback.state)
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = MoveCircleActionClient("action_move_client") # 创建ROS2节点对象并进行初始化
node.send_goal(True) # 发送动作目标
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'action_move_client = learning_action.action_move_client:main',
'action_move_server = learning_action.action_move_server:main',
],
},
第十二讲：参数
1. 通信模型
在NodeA相机驱动节点中，就需要考虑很多问题，相机连接到哪个usb端口，使用的图像分辨率是多少，曝光度和编码格式分别是什么，这些都可以通过参数设置，我们可以通过配置文件或者程序进行设置。
NodeB节点中也是一样，图像识别使用的阈值是多少，整个图像面积很大，那个部分是我们关注的核心区域，识别过程是否需要美颜等等，就像我们使用美颜相机一样，我们可以通过滑动条或者输入框设置很多参数，不同参数设置后，都会改变执行功能的一些效果。
这就是参数的作用。
全局字典
在ROS系统中，参数是以全局字典的形态存在的，什么叫字典？就像真实的字典一样，由名称和数值组成，也叫做键和值，合成键值。或者我们也可以理解为，就像编程中的参数一样，有一个参数名 ，然后跟一个等号，后边就是参数值了，在使用的时候，访问这个参数名即可。
可动态监控
在ROS2中，参数的特性非常丰富，比如某一个节点共享了一个参数，其他节点都可以访问，如果某一个节点对参数进行了修改，其他节点也有办法立刻知道，从而获取最新的数值。这在参数的高级编程中，大家都可能会用到。
2. 案例一：小海龟例程中的参数
在小海龟的例程中，仿真器也提供了不少参数，我们一起来通过这个例程，熟悉下参数的含义和命令行的使用方法。
启动两个终端，分别运行小海龟仿真器和键盘控制节点：
$ ros2 run turtlesim turtlesim_node
$ ros2 run turtlesim turtle_teleop_key
查看参数列表
当前系统中有哪些参数呢？我们可以启动一个终端，并使用如下命令查询：
$ ros2 param list
参数查询与修改
如果想要查询或者修改某个参数的值，可以在param命令后边跟get或者set子命令：
$ ros2 param describe turtlesim background_b # 查看某个参数的描述信息
$ ros2 param get turtlesim background_b # 查询某个参数的值
$ ros2 param set turtlesim background_b 10 # 修改某个参数的值
参数文件保存与加载
一个一个查询/修改参数太麻烦了，不如试一试参数文件，ROS中的参数文件使用yaml格式，可以在param命令后边跟dump子命令，将某个节点的参数都保存到文件中，或者通过load命令一次性加载某个参数文件中的所有内容：
$ ros2 param dump turtlesim >> turtlesim.yaml # 将某个节点的参数保存到参数文件中
$ ros2 param load turtlesim turtlesim.yaml # 一次性加载某一个文件中的所有参数
接下来就要开始写程序了，在程序中设置参数和读取参数都比较简单，一两句函数就可以实现，我们先来体验一下这几个函数的使用方法。
运行效果
启动一个终端，先运行第一句指令，启动param_declare节点，终端中可以看到循环打印的日志信息，其中的“mbot”就是我们设置的一个参数值，参数名称是“robot_name”，通过命令行修改这个参数，看下终端中会发生什么？
$ ros2 run learning_parameter param_declare
$ ros2 param set param_declare robot_name turtle
代码解析
我们来看下在代码中，如何声明、创建、修改一个参数的值。
learning_parameter/param_declare.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2参数示例-创建、读取、修改参数
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
class ParameterNode(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.timer = self.create_timer(2, self.timer_callback) # 创建一个定时器（单位为秒的周期，定时执行的回调函数）
self.declare_parameter('robot_name', 'mbot') # 创建一个参数，并设置参数的默认值
def timer_callback(self): # 创建定时器周期执行的回调函数
robot_name_param = self.get_parameter('robot_name').get_parameter_value().string_value # 从ROS2系统中读取参数的值
self.get_logger().info('Hello %s!' % robot_name_param) # 输出日志信息，打印读取到的参数值
new_name_param = rclpy.parameter.Parameter('robot_name', # 重新将参数值设置为指定值
rclpy.Parameter.Type.STRING, 'mbot')
all_new_parameters = [new_name_param]
self.set_parameters(all_new_parameters) # 将重新创建的参数列表发送给ROS2系统
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = ParameterNode("param_declare") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'param_declare = learning_parameter.param_declare:main',
],
},
3. 案例三：机器视觉应用
参数大家已经会使用了，如何在机器人中应用呢？
继续优化机器视觉的示例，物体识别对光线比较敏感，不同的环境大家使用的阈值也是不同的，每次在代码中修改阈值还挺麻烦，不如我们就把阈值提炼成参数，运行过程中就可以动态设置，不是大大提高了程序的易用性么？
说干就干，我们先来看下效果如何，再看下代码中的变化。
运行效果
启动三个终端，分别运行：
- 相机驱动节点
- 视觉识别节点
- 修改红色阈值
$ ros2 run usb_cam usb_cam_node_exe
$ ros2 run learning_parameter param_object_detect
$ ros2 param set param_object_detect red_h_upper 180
在启动的视觉识别节点中，我们故意将视觉识别中红色阈值的上限设置为0，如果不修改参数，将无法实现目标识别。
为了便于调整阈值，我们在节点中将红色阈值的限位修改为了ROS参数，通过命令行修改该参数的值，就可以实现视觉识别啦。
代码解析
我们来看下在视觉识别的代码中，是如何通过参数来设置阈值的。
learning_parameter/param_object_detect.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 古月居(www.guyuehome.com)
@说明: ROS2参数示例-设置目标识别的颜色阈值参数
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from sensor_msgs.msg import Image # 图像消息类型
from cv_bridge import CvBridge # ROS与OpenCV图像转换类
import cv2 # Opencv图像处理库
import numpy as np # Python数值计算库
lower_red = np.array([0, 90, 128]) # 红色的HSV阈值下限
upper_red = np.array([180, 255, 255]) # 红色的HSV阈值上限
"""
创建一个订阅者节点
"""
class ImageSubscriber(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.sub = self.create_subscription(Image, # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
'image_raw', self.listener_callback, 10)
self.cv_bridge = CvBridge() # 创建一个图像转换对象，用于OpenCV图像与ROS的图像消息的互相转换
self.declare_parameter('red_h_upper', 0) # 创建一个参数，表示阈值上限
self.declare_parameter('red_h_lower', 0) # 创建一个参数，表示阈值下限
def object_detect(self, image):
upper_red[0] = self.get_parameter('red_h_upper').get_parameter_value().integer_value # 读取阈值上限的参数值
lower_red[0] = self.get_parameter('red_h_lower').get_parameter_value().integer_value # 读取阈值下限的参数值
self.get_logger().info('Get Red H Upper: %d, Lower: %d' % (upper_red[0], lower_red[0])) # 通过日志打印读取到的参数值
hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) # 图像从BGR颜色模型转换为HSV模型
mask_red = cv2.inRange(hsv_img, lower_red, upper_red) # 图像二值化
contours, hierarchy = cv2.findContours(mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测
for cnt in contours: # 去除一些轮廓面积太小的噪声
if cnt.shape[0] < 150:
continue
(x, y, w, h) = cv2.boundingRect(cnt) # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2) # 将苹果的轮廓勾勒出来
cv2.circle(image, (int(x+w/2), int(y+h/2)), 5, (0, 255, 0), -1) # 将苹果的图像中心点画出来
cv2.imshow("object", image) # 使用OpenCV显示处理后的图像效果
cv2.waitKey(50)
def listener_callback(self, data):
self.get_logger().info('Receiving video frame') # 输出日志信息，提示已进入回调函数
image = self.cv_bridge.imgmsg_to_cv2(data, "bgr8") # 将ROS的图像消息转化成OpenCV图像
self.object_detect(image) # 苹果检测
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = ImageSubscriber("param_object_detect") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'param_declare = learning_parameter.param_declare:main',
'param_object_detect = learning_parameter.param_object_detect:main',
],
},
第十三讲：分布式通信
智能机器人的功能繁多，全都放在一个计算机里，经常会遇到计算能力不够、处理出现卡顿等情况，如果可以将这些任务拆解，分配到多个计算机中运行岂不是可以减轻压力？
这就是分布式系统，可以实现多计算平台上的任务分配。
1. 分布式通信
什么叫分布式？
之前我们也讲过，在ROS系统中，机器人功能是由各种节点组成的，这些节点可能位于不同的计算机中，这种结构可以将原本资源消耗较多的任务，分配到不同的平台上，减轻计算压力，这就是分布式通信框架的典型应用之一。
比如这款机器人系统中，有两个计算平台。
机器人体积比较小，不适合放一个笔记本电脑在上边，于是采用树莓派作为控制器，主要实现传感器驱动和电机控制等功能，不过视觉处理和应用功能就不适合在树莓派里运行了，我们放在另外一个性能更强的笔记本电脑中，此外我们还需要在电脑上监控机器人的传感器信息，并且远程控制机器人运动。
两个电脑之间的通信，看上去还有点复杂，毕竟相互传输的数据还挺多的，不过ROS系统都已经为我们设计好了，我们只需要在每一个电脑上配置好ROS环境，功能开发上完全不需要做任何变化，实现非常方便。
接下来，我们就带领大家一起来感受下ROS分布式系统的魅力。
2. 分布式网络搭建
除了我使用的笔记本电脑之外，另外一个计算平台我们选择了树莓派，模拟一个放置在机器人上的控制器。
树莓派配置
在开发之前，我们需要先配置好树莓派的环境，网上也有很多资料，大家都可以参考。
装系统
我们先要给树莓派装系统，这里我们选择的是Ubuntu Mate针对树莓派的镜像，下载镜像之后，烧写到树莓派的SD卡中就可以启动系统了。
Ubuntu MATE镜像下载链接：Choose an architecture | Download
安装ROS2
在安装好的Ubuntu Mate系统中，安装ROS2，和电脑端安装的流程一样。
编译代码
将我们课程的代码也下载到树莓派中，进行编译。
远程桌面
如果大家有显示器，可以直接在树莓派上连接键盘鼠标显示器使用，如果使用不方便的话，也可以在树莓派上配置好远程桌面，就可以通过网络访问树莓派的桌面系统了。
以上步骤的整体流程和电脑端操作基本一致，大家也可以参考如下链接配置树莓派：
【树莓派4B】安装Ubuntu Mate20.04+ROS Noetic+使用电脑自带的xrdp和VNC进行PC端远程控制_ubutnu mate vnc-CSDN博客
3. 分布式数据传输
树莓派配置完成后，确保已经和你所使用的电脑连接到了同一个局域网络中。接下来我们打通两个计算平台的通信能力。具体需要做什么呢？
简而言之，什么都不需要做。我们直接用命令行测试一下话题通信的效果。
Attention
如使用虚拟机，请将虚拟机网络修改为桥接模式
在树莓派端，使用如下命令启动一个发布者节点：
$ ros2 run demo_nodes_cpp talker #树莓派端
接下来在电脑端，使用如下命令启动一个订阅者节点：
$ ros2 run demo_nodes_py listener #PC端
神奇的事情就这样发生了，只要两个计算机安装好ROS2，并且处于同一网络中，他们就可以实现之前讲过的话题、服务、动作等通信了，感觉就像在一个电脑里一样。
不过这也会带来一个问题，如果一个网络中有很多个计算机，我们并不希望他们都可以互通互联，而是可以分组通信，小组之间是无法实现通信的。
4. 分布式网络分组
没问题，ROS2提供了一个DOMAIN的机制，就类似分组一样，处于同一个DOMAIN中的计算机才能通信，我们可以在电脑和树莓派端的.bashrc中加入这样一句配置，即可将两者分配到一个小组中：
$ export ROS_DOMAIN_ID=<your_domain_id>
如果分配的ID不同，则两者无法实现通信。
原文地址：https://blog.csdn.net/weixin_45897172/article/details/136342381
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！