桑欣

 176个笔记

 前言

• 工具地址为http://github.fishros.org。

 第1章 启程——让你的第一个机器人动起来

• DDS基于实时发布订阅协议(Real-time Publish-Subscribe, RTPS)来实现数据分发。
• 用于数据记录和回放的ros2bag工具等。

 1.2 开发环境搭建

• 使用FISHROS2OS工具可以快速在移动磁盘上安装Linux与Windows共存的双系统，大家可以访问https://www.fishros.org.cn/forum/topic/1835使用。
• VirtualBox的安装包可以从其官方网站https://www.virtualbox.org/wiki/Downloads下载
• 因为Ubuntu的官方网站访问不太稳定，我们可以到国内的镜像网站进行下载，例如http://mirrors.ustc.edu.cn/ubuntu-releases/22.04。

 3.2 Python话题订阅与发布

• 代码清单3-14中的第一个命令是用apt安装Python3的包管理工具pip3，第二个命令是用apt安装语音合成引擎espeak-ng，最后一个命令是使用pip3安装espeakng的Python库，方便我们在代码中调用。安装好依赖后就可以编写节点代码了，在src/demo_python_topic/demo_python_topic下创建novel_sub_node.py文件，在该文件中编写如代码清单3-15所示的代码。

 第4章 服务和参数——深入ROS 2通信

• 代码清单4-7　查看指定节点的参数描述

 4.3 用C++服务通信做一个巡逻海龟

• 代码清单4-31　创建demo_cpp_service功能包

 第5章 ROS 2常用开发工具

• 除了四元数和欧拉角，还有其他用于表示姿态的方式，通过ROS 2中的mrpt2工具，可以方便地获取不同姿态表示之间的对应关系和可视化，在新的终端中里依次输入代码清单5-2中的命令。
• 图5-2　使用3D rotation converter进行旋转姿态转换

 5.4 常用可视化工具rqt与RViz

• 上面的命令用于安装rqt-tf-tree这一工具，该工具将安装到ROS 2的默认安装目录下，安装完成后需要删除rqt的默认配置文件，才能让rqt重新扫描和加载到这个工具，继续在终端中输入代码清单5-28中的命令，删除rqt配置文件。

 5.6 ROS 2基础之Git进阶

• 此时如果想撤销对package.xml的更改，依然可以使用reset指令，不过需要指定回退的提交id，这里我们回退到first commit时的代码状态，命令如代码清单5-49所示。

 第6章 建模与仿真——创建自己的机器人

• 随着ROS的发展，Gazebo也分出了两个不同的分支：第一个是早期的稳定分支Gazebo Classic，界面如图6-2所示；第二个则是技术更为先进和经过优化的Gazebo Harmonic，但截至本文编写时，Gazebo Harmonic的资料较少且不稳定，不适合入门使用。

 6.2 使用URDF创建机器人

• 编写完成后，便得到了一个基础的URDF文件了，此时我们可以使用ROS 2提供的urdf_to_graphviz将URDF结构进行可视化，在URDF文件同级终端使用代码清单6-4中的命令。
• 可以看到，运行该命令后，会在当前目录创建一个pdf和gv格式的文件，其中first_robot.pdf文件内容如图6-4所示。
• 虽然可以通过加载文件的方式来显示机器人模型，但可以看到会出现TF错误，这是因为RViz要显示机器人模型，除了需要URDF之外，还需要通过TF输入各个部件之间的位姿关系。在ROS 2中，使用robot_state_publisher和joint_state_publisher可以
• 将URDF文件转化为话题并将部件之间的位姿关系通过TF发布出来。使用前我们需要安装这两个依赖，安装命令如代码清单6-5所示。
• 此时打开rqt，选择Plugins→Introspection→Node Graph选项，取消对tf的隐藏显示后刷新，最终配置和机器人节点关系如图6-8所示。
• 对于关节类型为fixed的固定关节，则由robot_state_publisher通过静态广播直接发布，而对于可以移动的关节，robot_state_publisher会先订阅/joint_states话题，获取关节的实时数据，然后通过动态TF发布，joint_state_publisher就是负责发布这一话题的节点。
• 图6-8　最终配置和机器人节点关系
• Xacro(XML Macro)是基于XML的宏语言，用于简化URDF文件的创建和维护。使用它可以将部件等定义为宏，在需要的时候调用即可。接下来通过代码来进行测试，在urdf目录下创建first_robot.urdf.xacro，然后编写代码清单6-10中的代码。
• 从上面的例子就可以看出，Xacro除了可以通过定义宏来复用代码，还可以进行嵌套调用，接下来我们就尝试利用Xacro完善机器人模型。
• FishBot是鱼香ROS工作室设计的一款开源移动机器人，从本节开始，我们就来尝
• 代码清单6-17中，定义了一个imu_xacro的宏，在宏内定义了一个部件和关节，关节imu_joint将imu_link和base_link固定连接，固定的位置通过参数xyz进行传递。接着在sensor目录下新建camera.urdf.xacro，然后编写代码清单6-18中的代码。
• 代码清单6-20中，通过xacro:include标签来包含其他的Xacro文件，通过该标签的filename属性可以指定要包含的Xacro文件的文件名称，需要注意的是，通过$(findfishbot_description)可以查找功能包的安装目录。关于传感器位置值的设定，文件中的位置可作为参考，你也可以根据显示效果进行调整。完成后我们重新构建并运行display_robot.launch.py，指定模型为first_robot.urdf.xacro，最终显示效果如图6-10所示。
• 因为默认轮子是躺平状态，所以这里调整了轮子部件中rpy的值，将r部分调整成了1.57079rad，即90°，将轮子竖起来。除此之外，在关节定义部分，关节类型采用连续关节continuous，该类型关节可以绕着某个轴进行无限制的旋转。除了固定和连续关节，还有旋转、浮动和平面关节。在关节标签中添加了axis子标签，用于表示旋转轴和方向，<axis xyz="0 1 0" />则表示绕y轴正方向旋转。
• 万向轮的定义和驱动轮类似，万向轮的几何形状采用的是圆球形状，半径为0.016，关节部分采用的是固定的类型。完成后再次修改fishbot.urdf.xacro，主要添加内容如代码清单6-23所示。
• 6.2.6　贴合地面，添加虚拟部件在RViz中水平观察机器人，你会发现机器人的轮子陷入到了地面之下，通过添加虚拟关节可以解决这一问题，在src/fishbot_description/urdf/fishbot/base.urdf.xacro中添加代码清单6-24中的代码。
• 这里首先声明了名称为base_footprint的空部件，然后将base_link固定在这个部件的上方，高度则设置为机器人身体高度的一半加上轮子的半径，再稍微减去1mm，让轮子可以贴紧地面。保存好后，重新运行，修改fixed frame为base_footprint，最终效果如图6-12所示，可以看到机器人的轮子已经刚好贴着地面了。

 6.3 添加物理属性让机器人更真实

• 在URDF中，可以直接在link标签下添加collision子标签来添加碰撞属性。collision标签的内容可以和visual相同，也可以根据实际需要设置，比如对于base_link部件，collision的几何形状可以设置为visual的圆柱形，也可以设置成盒状。
• 对于一个质量为m，宽为w，高为h，长为d的长方体来说，其惯性矩阵如下所示。
• 于一个质量为m，半径为r，高度为h的圆柱体来说，其惯性矩阵如下所示。
• 对于一个质量为m，半径为r的球体来说，其惯性矩阵如下所示。
• 使用Xacro结合上面的惯性公式，可以编写一个惯性和质量专用的宏定义，
• rdf/fishbot目录下新建common_inertia.xacro文件，编写代码清单6-26中的代码。

 6.4 在Gazebo中完成机器人仿真

• 除了安装Gazebo外，我们还可以下载一些模型文件到系统的gazebo配置目录下，文件可以直接从github克隆，命令如代码清单6-29所示。代码清单6-29　下载Gazebo模型
• 着将打开建筑编辑界面，在左边选择Create Walls下的Wall，然后在右上方的空白处画墙，完成的三室一厅实际效果如图6-23所示，需要注意的是可以通过鼠标滚轮调整实际地图的比例尺。
• 保存在fishbot_description功能包目录下的world文件夹中，将文件命名为custom_room.world，然后单击Save即可。下次启动Gazebo时，直接使用gazebo命令加世界文件路径即可直接加载世界模型。用VS Code打开src/fishbot_description/world/custom_room.world文件，该文件部分内容如代码清单6-31所示。
• 可以看到，机器人的颜色变成了白色，这是因为从URDF转成SDF时部分标签并没有被处理和转换，不过我们可以通过在URDF中添加gazebo标签，来将配置传递给Gazebo。
• 使用同样的方法，你可以修改轮子和相机的颜色，除了Black，还可以使用Blue、Red和Green等几十种颜色，具体可以参考http://wiki.ros.org/simulator_gazebo/Tutorials/ListOfMaterials。
• 代码清单6-35中gazebo的四个子标签分别表示摩擦系数、刚度系数和阻尼系数。其中mu1表示切向摩擦系数，mu2表示法向摩擦系数，kp表示接触刚度系数，kd则表示阻尼系数。gazebo默认mu1和mu2为1.0, kp为1000000000000.0, kd为1.0，这里我们提高摩擦力配置，减小刚度系数配置。
• 代码清单6-37中，定义了一个gazebo_control_plugin宏，在宏内定义了一个gazebo标签。在gazebo标签内使用了plugin子标签表示插件，plugin标签有名字和对应的库名字两个属性，插件加载后会新建一个节点，而plugin设置的名字最终则成为节
• 这里我们使用的是libgazebo_ros_diff_drive.so库，该库调用后会订阅ROS 2的控制指令话题/cmd_vel，并发布机器人的里程计位置信息话题/odom和/tf。该节点的订阅和发布如图6-31所示。
• 在plugin子标签中，第一个ros标签定义了节点的命名空间和话题名称映射，这里将命名空间namespace设置为“/”​，将节点默认的控制话题cmd_vel映射为cmd_vel,odom映射为odom，相当于没有映射，这里写出来方便后续根据需求修改。由于里程计数据是测量轮子转速时计算出来的，因此需要知道轮子关节的名称、轮子的半径和轮子的安装距离等信息，这些都可以通过该插件的子标签进行设置，对应的标签及含义如表6-1所示。
• 根据提示，按w/x键可以实现速度的增加或减少，按i/j/k等键可以发送对应的速度数据到默认的/cmd_vel话题，可以自行尝试控制。除了使用键盘控制节点，也可以使用代码发送话题数据控制机器人移动，可以参考3.3.1节的海龟画圆代码，修改话题名称和速度，再次运行就可以控制仿真的fishbot画圆了。机器人走过的路径也可以通过RViz实现可视化，如图6-36所示。
• 除了plugin外，我们还设置了其他5个标签，其名称和对应含义如表6-2所示。表6-2　sensor子标签的名称及对应含义
• range标签用于设置扫描的距离和最小的分辨率，<noise>标签用于设置噪声，这里使用标准差为0.01的高斯噪声，使传感器更加真实。
• IMU(Inertial Measurement Unit)是一种集成了多个惯性传感器的设备，通过该传感器可以测量三轴角速度和线加速度数据。通过对角速度进行积分和计算，可以得到设备的姿态变化信息。
• 而深度图像信息可以通过执行rqt工具的Plugins→Visualization→Image View命令，选择对应话题进行显示，最终效果如图6-39所示。

 6.5 使用ros2_control驱动机器人

• 假如你是ros2_control的设计者，你的目的是通过它来减少重复造轮子，简化硬件集成，你该怎么做？第一步我认为应该找出重复在哪里，以Gazebo的两轮差速插件为例，该插件至少包含了两部分逻辑。第一部分是数据收发部分，即从Gazebo拿到两个轮子速度传感器的当前转速，并将目标速度发送过去。第二部分是控制器部分，控制器的组成有两个：第一个是控制器负责根据轮子反馈的转速，结合轮子直径和轮距计算里程计；第二个是根据收到的机器人控制命令，计算轮子的目标速度。Gazebo插件工作流程如图6-40所示。对于一个真实的两轮差速结构的机器人来说，控制器和数据接口两部分都不能少，只不过数据接口是用来和真实硬件进行交互的。由于数据接口要对接的硬件不同，并不算重复，所以说控制器部分是重复造轮子的部分
• 你可能会想，既然控制器重复了，那把控制器抽出来不就行了，让真实硬件和Gazebo都用同一个控制器，这确实是正确的思路，但控制器是依赖于数据接口的，所以要实现使用同一个控制器，就要先统一数据接口。有趣的是，ros2_control的设计和我们想到一块了，而且它比我们想的更强大。ros2_control的框架如图6-41所示，我们一起来看一下
• ros2_control将硬件资源分为传感器、执行器和系统三类。传感器和执行器你已经很熟悉了，比如温度测量装置就是传感器，电动机则是执行器，但有时你可能会买到一个包含传感器和执行器的设备，比如一个机械臂，不仅可以控制关节转动，还可以读取关节的角度信息，这种复杂的设备就可以用系统来表示
• 接着来看图6-41的上半部分，控制器是可以有多个的，不同控制器可能会使用不同的数据接口，所以ros2_control使用控制器管理器来实现对各个控制器的管理。
• 可以看到ROS 2提供的控制器都被放到了这个包的依赖中，其中包括了两轮差速机器人的控制器diff-drive-controller，
• 关节状态发布控制器joint-state-broadcaster，用于发布IMU数据的imu-sensor-broadcaster和用于关节位置控制的position-controllers等。
• 可以看到，上面的控制器中有一组关节的力控制、速度控制、位置控制和轨迹控制等，有了这些控制器，可以轻松实现对各个关节的精细化控制。因为没有硬件，所以下一步我们就尝试使用Gazebo中的仿真硬件接入ros2_control。
• gazebo_ros2_control对硬件资源的描述使用的也是XML格式，所以我们可以将配置写到URDF中，在src/fishbot_description/urdf/fishbot/中新建fishbot.ros2_control.xacro，在该文件中编写代码清单6-54中的代码。代码清单6-54　fishbot/fishbot.ros2_control.xacro
• 接着定义了两个关节标签，由于要采用Gazebo提供数据接口，所以这里的关节名称要和机器人的保持一致，分别设置为左、右轮的关节名称。在关节标签下使用<command_interface>指定该关节提供的命令接口，<state_interface>表示该关节提供的状态接口，这两个接口都有一个共同的属性就是name，一个关节的控制命令和状态无外乎三种形式，关节位置position、关节速度velocity和关节扭矩effort。对于命令接口，可以设置最大值和最小值，上面我们限制了速度的范围为(-1,1)，扭矩的范围为(-0.1, 0.1)。
• 此时如果通过命令行查询节点，可以看到/controller_manager节点已经启动了，使用代码清单6-59中的命令可以查询该节点对外提供的所有服务。代码清单6-59　查询控制器管理器服务
• 图6-42　轮子变为白色出现这一现象的原因是我们将两轮差速插件注释后，就没有节点提供两个轮子到base_footprint之间的TF变换了。我们可以使用ros2_control的关节状态控制器，发布/joint_states话题，然后由robot_state_publisher转成TF数据并发布。
• 在上面的配置中，给controller_manager添加了一个名称为fishbot_joint_state_broadcaster的参数，表示控制器节点的名称，接着通过type指定该控制器的类型。joint_state_broadcaster/JointStateBroadcaster控制器会自动扫描所有的状态接口，读取并通过话题/joint_states发布。重新构建和启动仿真，此时你会发现依然没有两个轮子的TF变换，检查/joint_states话题也没有被发布，这是因为没有加载并激活fishbot_joint_state_broadcaster控制器。在终端输入代码清单6-63中的命令，加载并激活fishbot_joint_state_broadcaster控制器。代码清单6-63　加载并激活fishbot_joint_state_broadcaster控制器
• 相信你一定听过“力控”这个词，在机器人中要实现更加柔顺地控制，力控是比较常见的一种策略。ros2_control就提供了这一控制器，类型为effort_controllers/JointGroup-EffortController。

 第7章 自主导航——让机器人自己动起来

• 接着转弯，你会发现在机器人前进过程中，轨迹和障碍物信息都被记录下来了，但转弯过程中障碍物信息出现了较大的偏差，如图7-2所示。
• 从图7-2的记录结果可以看出，如果只是简单地对数据进行叠加，会因为传感器速率同步和噪声等问题导致出现错误的记录。因此，SLAM（Simultaneous Localization and Mapping，同步定位与地图构建）技术被提了出来，它结合特征提取和滤波等算法，来解决机器人定位和建图问题。
• 激光SLAM主要使用激光雷达这一类传感器获取环境的深度（即距离）信息，然后标记障碍物和自由空间进行建图和定位。视觉SLAM需要先使用相机等视觉传感器获取图像信息，然后通过图像处理和特征提取来进行建图和定位。由于激光雷达可以直接获取环境深度信息，精度高且稳定，所以激光SLAM技术要相对成熟很多。
• 一条从当前位置到目标位置之间的路线，理想中这条路线应该符合某些要求，比如能避开所有障碍物且耗时最少。要实现这一效果就需要在地图上标记代价信息，比如走路况差的路线要花费更多的时间，付出更大的代价。确认路线这一步骤我们称之为全局路径规划。在按照全局路线行走时，前方可能会出现一些障碍物，比如开车时发现前方一段车道正在封闭维修，此时就需要绕开维修车道。但机器人要想绕过去，第一步要把障碍物加到地图中，然后对当前这一小段路线重新进行路径规划。因为这个障碍物是小范围和动态的，所以我们往往会针对当前小范围环境，重新创建一张局部的代价地图，然后进行路径规划。确认局部路线的这一步骤，我们就称之为局部路径规划。考虑全局路径和局部路径以后，有时我们还要面对一些特殊情况，比如行走过程中卡住或者被行人挡住，且找不到局部路径绕过去。此时就需要有一些相应的行为帮助机器人脱离困境，比如当前进卡住时尝试后退，当被人挡住时播放“请让路”语音。对于这些遇到故障问题时的脱困动作，我们称之为恢复行为。目前的机器人导航系统就是围绕着上面这三部分进行设计的。在对应的时机分别调用全局路径规划获取路径，根据当前位置和全局路径，在调用局部路径规划得到一小段路径后控制机器人行走，当出现问题时，调用恢复行为进行脱困。

 7.2 使用slam_toolbox完成建图

• lam_toolbox的输入有两个，第一个是订阅来自雷达的/scan话题，用于获取雷达数据；第二个是获取里程计坐标系odom到机器人坐标系base_footprint之间的变换。这些数据都是有时间戳的，所以在上面的命令中将use_sim_time参数的值设置为True，表示使用来自Gazebo的仿真时间，以防止因时间戳造成数据不合法。
• 打开rqt，使用rqt-tf-tree工具查看当前的TF结构，如图7-4所示。
• 一张地图可以用图片表示，为了和真实尺寸进行转换，都会设置比例尺，导航地图也不例外。保存地图可以使用nav2_map_server工具，使用apt命令可以方便地安装该工具，命令如代码清单7-3所示。代码清单7-3　安装地图服务
• trinary表示地图中的每个像素点有三种可能的状态：障碍物的占据(occupied)状态用黑色表示；无障碍的自由(free)状态用白色表示；未探索的未知(unknown)状态用灰色表示。
• resolution表示地图的分辨率，设置为0.05，表示每个像素对应的物理尺寸为0.05m。origin表示地图坐标系的原点，单位是米，默认设置在启动建图的位置。negate表示是否对地图进行取反操作，0表示不取反。
• occupied_thresh和free_thresh两个参数用于设置占据、自由和未知之间的分界线，如果把像素点的值映射到用0～1之间的数表示，free_thresh:0.25表示小于0.25就认为该像素对应的位置是自由状态，occupied_thresh:0.65表示大于或等于0.65就认为该像素对应的位置是占据状态，而它们之间的就是未知状态。

 7.3 机器人导航框架Navigation 2

• 行为树起源于游戏设计，用于控制游戏角色的行为，比如植物大战僵尸中，当僵尸出现时，豌豆射手就会开始射击。对于一个移动机器人来说，需要知道什么时候要进行路径规划，什么时候要执行脱困，和游戏中的角色行为相似，所以使用行为树来描述和管理机器人的行为再合适不过了，Navigation 2就是使用它进行机器人行为调度的。
• Navigation 2的系统框架如图7-8所示，通过它可以让你对Navigation 2的架构有一个初步了解。
• BT导航服务器下面三个服务模块中，右边的是规划器服务器(Planner Server)，它的任务就是负责全局路径规划。需要注意的是，这个模块之所以叫规划器服务器而不是具体某个规划器，是因为路径规划算法有很多，规划器服务器可以根据配置加载不同的规划器完成规划任务，这样就有了灵活性。这一点和上一章介绍的ros2_control中的控制器管理器相同，可以加载不同的控制器。中间的模块是控制器服务器(Controller Server)，该模块负责根据全局路径，结合实时障碍物和局部代价地图完成机器人的控制。需要注意的是，它同样只是一个服务器，可以加载多种不同的控制器完成这一任务。左边的模块是恢复器服务器(Recovery Server)，可以加载不同的恢复行为完成机器人的脱困。从箭头可以看出，BT导航服务器(BT Navigation Server)收到目标点后，由规划器服务器计算到目标点的路径(ComputePathToPose, CP)，然后由控制器服务器进行路径跟随(FollowPath, FP)，如果遇到卡住等困境，则调用规划器服务器完成脱困。
• Navigation 2还添加了一些辅助服务器，如平滑服务器，这些服务器协同工作，完成了整个Navigation 2的导航任务。好了，关于Navigation 2理论的介绍就到这里，接着我们来安装并尝试使用它完成机器人导航。
• 除了修改话题和坐标系名称以保证数据的正确获取以外，在进行路径规划时还需要考虑机器人的大小（即半径）这一参数，如果半径设置的比真实的大，会造成遇到窄的通道时机器人过不去；如果过小，则容易发生碰撞。由于只有在基于地图做路径规划时才会考虑这一问题，所以需要在全局代价地图global_costmap和局部代价地图local_costmap的参数中进行配置。分别修改两个代价地图节点robot_radius参数为建模时的半径，修改完成后对应的参数值如代码清单7-9所示。
• 由于全局路径规划和局部路径规划使用的地图并不相同，所以机器人周围障碍物会在局部代价地图上进行膨胀。在RViz左侧显示部分，修改Global Planner配置，取消全局代价地图Global Costmap的显示，配置如图7-11a所示，接着就可以看到如图7-11b所示的局部代价地图及其膨胀层了。
• 由于仿真的激光雷达设置的扫描频率只有5 Hz，当机器人旋转过快时就容易导致定位不稳，可以通过调整配置文件中机器人速度的相关参数来限制机器人的最大速度。
• 在介绍Navigation 2框架时我们了解到，全局路径是由规划器服务器规划出来的，然后由控制器服务器进行执行，输出最终的控制指令，所以我们要修改的速度相关参数一定是在控制器服务器节点中。打开nav2_params.yaml文件，找到controller_server，其下的FollowPath模块负责将路径转换成角速度和线速度。将该模块下最大旋转角速度参数max_vel_theta设为0.8，将角加速度参数acc_lim_theta设为2.0，除了修改角速度外，你也可以根据实际需要修改线速度参数，这里修改的对应参数如代码清单7-11所示。
• 在上面的参数文件中，我们修改了三个参数值。将FollowPath下的xy_goal_tolerance修改为0.15，表示在路径跟踪时到目标点的位置允许误差为0.15m，然后在负责检测是否到点的general_goal_checker中修改对应位置和角度精度范围，general_goal_checker会实时检测当前机器人位置和目标位置之间的差距，如果在指定的范围，则会停止FollowPath。
• 地图默认的分辨率是0.05m，如果到点范围设置得太小，就会造成机器人在目标点左右徘徊，反而不利于导航。

 7.4 导航应用开发指南

• 从该接口定义可以看出，动作消息的接口分为目标、结果和反馈三个部分，相比服务通信接口的目标和结果多出了反馈这一部分。如果在机器人导航时仔细观察RViz左下角Navigation 2部分，你会发现它会实时地显示当前导航所花费的时间、距离目标点之间的距离、花费的时间以及脱困次数，这些数据就是来自动作服务的反馈部分。

 7.6 ROS 2基础之Git仓库托管

• 图7-33　在GitHub设置中添加公钥添加完成后，再次运行推送代码命令，此时就可以成功推送了，重新访问仓库页面，便可以看到所有的代码和提交记录了，Github上的显示结果如图7-34所示。

 第8章 使用自己的规划器和控制器导航

• 起ROS 2中的插件，不得不让人想起rqt，在前面的章节，安装rqt-tf-tree插件后，就可以轻松地在rqt工具中加载并使用它。在Navigation 2中要使用自己的规划器和控制器，则需要使用插件库pluginlib来编写插件。
• 因为是测试代码，所以上面对start()和stop()方法的实现很简单，只是分别加了句输出。需要关注的是最后两行，因为PLUGINLIB_EXPORT_CLASS宏是在pluginlib/class_list_macros.hpp中的，所以第一步要包含它，接着使用PLUGINLIB_EXPORT_CLASS宏对插件进行导出，该宏有两个参数，第一个是要导出的类，第二个是导出的类的抽象基类。
• 使用add_library指令用于添加一个库文件，第一个参数是库的名字，第二个参数SHARED表示生成动态库，第三个参数是库对应的源代码文件，后面的添加以及安装指令则用于复制文件到install目录。需要额外关注的是pluginlib_export_plugin_description_file指令，用于导出插件描述文件，第一个参数是功能包的名字，第二个参数是插件描述文件的名字。接着在learn_pluginlib目录下调用colcon build进行构建，构建成功后，查看目录install/motion_control_system/lib，就可以看到动态库libspin_motion_controller.so。
• 代码清单8-8中首先包含了pluginlib和抽象基类头文件，然后在main函数中判断命令行参数的数量是否正确，接着从参数数组中获取控制器的名字放到controller_name中。接下来是加载并使用插件的步骤，第一步创建了类加载器ClassLoader的对象controller_loader，其中第一个参数motion_control_system表示功能包的名字，第二个参数是控制器基类的名字。第二步是通过控制器的名称创建类加载器实例指针，控制器的名称就是我们在spin_motion_plugins.xml中定义的名称。第三步是调用控制器的成员方法进行控制。

 8.2 自定义导航规划器

• createPlan的入口参数有两个，一个表示起始点，一个表示目标点。为了省去坐标转换操作，这里我们只接受来自在全局坐标系下的起始点和目标点，所以在代码开始第二步就是对输入点所在的坐标系进行检查，不符合则直接返回。第三步通过起始点和目标点计算路径长度，除以插值分辨率得到循环次数，根据循环次数分别计算x和y方向每次插值增加的步长。第四步则通过循环，生成一系列的点，并将点放到global_path中。有了路径后，第五步则对路径是否穿过障碍物进行检测，遍历路径，接着通过costmap_->worldToMap函数将坐标点转换成栅格坐标mx和my，然后通过costmap_->getCost获取对应栅格的代价值，最后判断栅格是否是致命障碍物，若是则输出一句异常，并通过throw抛出异常，终止规划。若没有穿过障碍物的点，则代码执行到第六步，我们将目标点作为路径的最后一个点并返回。
• 在Navigation 2中，规划器插件是由规划器服务器planner_server进行调用的，所以更换控制器也要在planner_server下进行配置。将第7章的自动巡检机器人的所有功能包复制到chapt8/chapt8_ws/src下，接着修改文件src/fishbot_navigation2/config/nav2_params.yaml，第7章中默认使用的规划器插件是"nav2_navfn_planner/NavfnPlanner"，我们修改为"nav2_custom_planner/CustomPlanner"并对参数进行设置，完成后planner_server的配置如代码清单8-22所示。

 8.3 自定义导航控制器

• 控制器根据规划器提供的路径来下发速度指令，使得机器人能够按照路径进行移动，所以路径跟踪就是控制器的主要功能。前面自定义一个规划器插件类时需要继承抽象基类nav2_core::GlobalPlanner，而创建控制器时需要继承的抽象基类是nav2_core::Controller，该类定义的所有纯虚函数如代码清单8-24所示。
• 控制器和规划器相似，都是通过生命周期进行管理的，先配置，再激活，然后使用，最后进行关闭。当需要机器人进行路径跟踪时，首先会调用setPlan方法将路径通过参数传递给控制器，然后传递当前机器人位置等参数给computeVelocityCommands方法，该方法会根据控制算法返回需要下发的速度命令。setSpeedLimit方法则用于设置机器人的最大速度，该方法即使不调用也不会影响控制器进行路径跟踪。
• 如果搜索“机器人和路径跟踪算法”关键词，你可以看到很多控制算法，例如比例-积分-微分(PID)控制、纯追踪法(Pure Pursuit)控制和模型预测控制(Model PredictiveControl)等。但本节的重点在于如何自定义控制算法，所以我们就采用最简单的原地旋转和直行策略。策略如下，当检测到目标点方向和当前机器人朝向角度差较大时，则原地旋转到目标点方向，反之则朝目标点前进。又因为要跟随路径，所以目标点不能直接选择路径终点，因此我们将距离机器人当前位置最近的点的下一个点作为目标点。依据策略，我们先来完善目标点选取方法getNearestTargetPose，完成后该方法的内容如代码清单8-30所示。
• 在calculateAngleDifference方法中，第一步是通过调用tf2::getYaw获取当前机器人的朝向，第二步则通过std::atan2获取目标点相对当前点的朝向，第三步则是计算角度差，并限定角度差的范围。
• 在代码清单8-32中，首先检查全局路径是否为空，为空则抛出异常，终止代码。因为当前点位pose默认的坐标系是里程计，所以第二步调用方法transformPoseInTargetFrame将当前位姿转换为map坐标系下，如果转换失败同样抛出异常，终止代码。第三步是获取最近的目标点和角度差。第四步则根据角度差计算速度，如果角度差的绝对值大于π/10(18°)，则机器人会停止前进，只进行原地旋转，角速度被设置为最大角速度(max_angular_speed_)；如果角度差小于等于π/10，则机器人将直行，线速度被设置为最大线速度(max_linear_speed_)，而角速度为0。
• 修改文件src/fishbot_navigation2/config/nav2_params.yaml下的controller_server参数，在之前的导航中，我们使用规划器插件"dwb_core::DWBLocalPlanner"进行路径跟踪，现在我们将插件修改为"nav2_custom_controller::CustomController"并对参数进行设置，完成后controller_server的配置如代码清单8-33所示。

 第9章 搭建一个实体移动机器人

• 在激光雷达的头部有一个激光发射头和线性CCD接收头，发射头发射出的光属于波长在1000nm左右的红外光，肉眼是不可见的，接收头收到反射的激光就可以计算出障碍物的距离。前面导航中使用的/scan话题数据在真实机器中就是由激光雷达驱动和发布的。
• 除了激光雷达以外，另一个重要的传感器就是编码器。在移动机器人中我们需要实时地获取机器人各个轮子的转速，根据机器人的运动学模型将每个轮子的转速转换成机器人的速度，通过对速度进行积分得到机器人行走的距离和角度，也就是里程计数据/odom。FishBot采用的是AB电磁编码器，如图9-2所示，编码器位于电动机的后部。
• 在图9-2中，电磁编码器是由1和2这两个霍尔传感器和圆形磁铁3共同组成的，该磁铁的磁性是间隔分布的，磁铁固定在电动机的转子上。当电动机转动时，带动磁铁转动，此时用于检测磁性的霍尔传感器就会检测到磁性的变化，从而测量出电动机在某段时间内转了多少圈，即电动机的转速。
• 电动机有很多分类，可以按照有刷无刷、直流交流来分类。FishBot采用的是一个额定电压12 V的370减速电动机，额定转速为130r/min、额定电流为0.5 A，转矩为600gf·cm(0.05884N·m)。电动机尺寸和外观如图9-3所示。
• 需要注意的是FishBot采用的是减速电动机，所谓减速电动机指的是带减速器的电动机。我们知道电动机一般由定子和转子组成，一般转速都比较快，但输出的转矩比较小，所以我们会给电动机配备减速器，让转速降低，提高转矩
• 决策系统往往需要较大的算力，所以硬件成本也是最高的，为了节省成本，FishBot采用了同时支持无线和有线连接的驱动控制板，让大家可以直接使用自己的计算机作为决策端，可以直接感知并控制系统进行通信。除了感知、决策和控制部分，一个机器人要想动起来还需要一些硬件来配合，比如电池、电源模块以及必要的支撑结构等。

 9.2 单片机开发基础

• 我们可以采用PlatformIO IDE开发Arduino，该IDE支持多种类型的单片机，可以在VSCode中直接通过插件进行安装。PlatformIO IDE主要使用Python编写，为了能够跨多个版本使用，PlatformIO IDE在Python虚拟环境运行，所以我们需要先安装虚拟环境工具，命令如代码清单9-1所示。
• 安装完成后，打开VS Code的扩展商店，如图9-4所示，搜索安装PlatformIO IDE。安装完成后，在VS Code的侧边就可以看到PlatformIO IDE的按钮，单击按钮就会执行PlatformIO IDE的首次初始化程序。如果初始化过慢可以手动进行初始化安装，命令如代码清单9-2所示。
• 为了简化开发流程，Arduino提供了setup和loop两个声明好的函数，其中setup函数在启动时调用一次，一般初始化设置都在该函数中进行；之后是loop函数，该函数会在setup函数后循环调用。在代码清单9-4中我们使用了串口Serial进行通信，串口是一种通信端口，通过专门的USB转串口芯片，我们可以在计算机上通过串口与单片机交换数据。所以我们首先在setup函数中初始化串口，并设置波特率为115200，接着在loop函数中调用串口输出数据，并延时1000ms。
• 代码清单9-6　卸载占用及添加权限
• 可以看到，​“Hello World！”已经在串行监视器中显示出来了，可以通过修改延时来改变输出速率，不过在下次下载代码前一定要记得关闭串行监视器，因为同一个串口设备同一时间只能由一个程序打开使用。
• R2是一个1kΩ的电阻，LED1是一个蓝色的LED灯，右侧是3.3 V的电压源，左侧ESP_IO2是单片机的引脚，电流由电压高的地方流向电压低的地方，如果我们将ESP_IO2的电压设成3.3 V，此时电路两端电压相同，没有电流经过，LED1不工作。如果我们将ESP_IO2电压设置成0 V，此时右侧电压高，左侧电压低，电流从右侧流过LED1到ESP_IO2, LED1开始工作。所以我们可以通过ESP_IO2的电压高低来控制LED1灯的亮灭，这个就是点灯电路的原理。
• 超声波测距传感器是一种使用超声波进行测量的传感器，可以可靠地检测部分或完全透明的物体，并进行精确的距离测量。一般的超声波传感器外观如图9-13所示。
• 超声波上有一个发射头、一个接收头，发射头负责发送超声波，超声波遇到障碍物就会反射回来，接收头就可以接收到信号，我们可以根据时间差和声速来计算障碍物的距离。FishBot采用的超声波有四个引脚，分别是VCC、GND、TRIG和ECHO。VCC和GND两个引脚负责供电。TRIG引脚是发送引脚，我们给这个引脚输出高电平时就
• 可以发射出超声波，当收到回波时TRIG引脚电平就会产生相应的电平变化，计算TRIG引脚高电平持续时间就是超声波在空中的飞行时间。我们按照这个逻辑来编写代码。
• 因为FishBot超声波接口TRIG连接在单片机27引脚，ECHO连接在21引脚，所以代码开头使用宏定义引脚编号，在setup函数中分别初始化了串口和引脚模式。在loop函数中，首先通过引脚电平设置函数digitalWrite和微秒延时函数delayMicroseconds在TRIG引脚上产生一个10μs的脉冲触发超声波，然后利用pulseIn函数计算ECHO引脚上高电平的持续的时间，接着根据声速和时间得到计算距离并输出。编译工程并下载到开发板，打开串行监视器，如代码清单9-9所示，可以看到间隔500ms左右的时间输出数据。
• IMU即惯性测量单元，FishBot采用一块MPU6050模块用于惯性测量，MPU6050为全球首例集成六轴传感器的运动处理组件，它通过I2C协议和单片机进行通信，相比上一节的超声波模块，驱动MPU6050更为复杂，不过我们不用从头编写，因为Arduino还支持通过第三方库来驱动硬件，本节我们就尝试用开源库驱动FishBot的IMU传感器。
• 代码清单9-11中，首先使用Wire来初始化MPU6050类的对象mpu，接着在setup函数中初始化串口和I2C总线Wire，然后启动MPU6050传感器并校准，最后在loop函数中不断更新MPU6050数据并输出角度信息。编译工程并下载到开发板，打开串行监视器，就可以看到如代码清单9-12所示角度信息的输出。

 9.3 机器人控制系统的实现

• 让电动机动起来只需要通电就行，电动机的转速可以通过改变通电的时间来控制，因为电动机需要的电压和电流较大，无法直接接到单片机引脚上，所以需要额外的驱动电路放大来自单片机引脚的信号。FishBot开发板采用DRV8833芯片来实现电动机的驱动，电动机驱动原理图如图9-14所示。
• 电路中AIN1(IO 23)对应AOUT1, AIN2(IO 22)对应AOUT2, DRV8833会将AIN1(IO23)和AIN2(IO 22)上的电信号放大到对应的输出引脚上，我们将电动机接在输出引脚即可。新建工程fishbot_motion_control，接着修改platformio.ini，添加依赖库 Esp32McpwmMotor，具体内容如代码清单9-13所示。
• 这里我们使用开源库Esp32McpwmMotor来驱动电动机，该库可以同时控制6个直流电动机，对于只有2个驱动轮的FishBot来说绰绰有余。
• Esp32PcntEncoder是基于ESP32的脉冲计算外设编写的脉冲计算开源库，使用起来非常简单。
• PID控制器是一种广泛应用于工业控制、自动化控制等领域的控制算法，其名称来源于“比例-积分-微分”三个控制器参数，即Proportional（比例）​、Integral（积分）​、Derivative（微分）​。PID控制器的基本原理是通过测量目标系统的反馈信号和期望输出信号之间的误差，根据一定的数学模型计算出控制信号，使目标系统能够稳定地达到期望输出。对于控制轮子速度来说，PID控制器会根据目标速度和实际速度的差值，计算出让实际速度更接近目标速度的速度值
• 左右轮速度和机器人速度的关系为：v=(v1+vr)/2ω=(vr-v1)/l也就是两轮差速运动学正解的公式。反之，当我们已知目标机器人速度，利用逆解公式可以得出左右轮子的线速度。
• v1=v-ωl/2vr=v+ωl/2有了运动学正逆解公式，我们就可以根据公式编写代码。在fishbot_motion_control/lib下新建目录Kinematics，接着在目录下新建Kinematics.h，然后在头文件中编写如代码清单9-25所示的内容。
• 电动机速度更新方法接收当前时间和左右轮编码器计数值为参数，在函数内先计算时间差和编码器计数差，最后根据脉冲距离比计算出速度，单位为mm/s。接着
• 通过运动学正解，可以获取机器人底盘实时的速度信息，通过对线速度和角速度的积分，就可以获取机器人的实时位置信息。我们来推导一下计算公式，假设在某一个时间段t中，机器人的线速度为vt，角速度为ωt，机器人在初始时刻的位置为xt, yt朝向为θt，求经过t时刻后机器人新的位置(xt+1, yt+1)和朝向θt+1。这一段时间内机器人前进的距离d=vtt，转过的角度为θ=ωtt，则机器人新的角度θt+1=θt+θ，我们将机器人前进的距离根据其朝向分解为在x和y轴上的位移量，则可得出以下公式。
• xt+1=xt+d⋅cos(θt+1)
• yt+1=yt+d⋅sin(θt+1)有了公式，我们就可以来写代码，速度积分可以在每次速度更新时计算，修改Kinematics.h代码如代码清单9-30所示。
• 该函数用于计算机器人的实时里程计数据，输入参数是时间间隔dt，单位为ms，所以在函数内首先将其转换为以s为单位，接着调用运动学正解，将左右轮速度转换为机器人的角速度和线速度，并将线速度的单位转换为m/s。然后让角速度乘上时间，得到dt时间内角度的变化量，接着将变化累积到当前角度上，并使用TransAngleInPI将角度值限制在-π～π的范围内。最后则根据前面的公式计算机器人的前进距离delta_distance，根据公式将其分解为x和y轴上的分量，并累加到里程计的x和y上。

 9.4 使用micro-ROS接入ROS 2

• 其中深色组件是专为micro-ROS开发的，浅色组件取自标准ROS 2软件。这里重点对深色部分进行介绍，最左边的ROS 2 Agent是micro-ROS在正常系统上的代理，它通过串口、蓝牙或者以太网等协议和微处理器平台的micro-ROS进行连接，并进行数据的转发。中间深色部分，最下面RTOS部分的micro-ROS arduino是基于arduino开发的代码库，中间的中间件部分是经过优化的微型DDS适配器和客户端，上面的客户端库则是提供了一套基于rclc的API接口。最右侧是用于嵌入式软件基准测试的工具。
• 要将微控制器连接到ROS 2中，需要做两部分工作，第一个是在正常系统中安装Agent，第二个是在微控制器中编写micro-ROS程序。我们先在系统中安装Agent，在主目录下新建chapt9/fishbot_ws/src目录，接着克隆micro-ROS Agent源码到src目录，指令如代码清单9-36所示。
• 接着使用colcon进行功能包构建，构建完成，就可以直接运行Agent了，构建及运行命令如代码清单9-37所示。代码清单9-37　构建并运行Agent
• 这里我们启动micro_ros_agent节点，并使用udp4作为传输协议，指定端口号为
• 8888，在接下来编写微控制器代码时就需要指定通信协议、主机地址和端口号。主机地址就是系统的IP地址，可以在系统设置的网络模块查看，如图9-18所示。
• 这里添加了micro-ROS的platformio版本依赖库，并添加了传输协议配置项board_microros_transport，指定使用Wi-Fi与ROS 2 Agent进行连接。接着修改src/main.cpp的代码，我们来创建第一个micro-ROS节点，修改后添加的代码及位置如代码清单9-39所示。
• 创建了一个名为micro_ros_task的单独任务函数，负责初始化并运行micro-ROS节点，任务在嵌入式系统中可以简单理解为线程。在任务函数中，第一步设置传输协议为无线网络Wi-Fi，这里需要设置Agent所在系统的IP地址、端口和Wi-Fi信息，当开发板连接到Wi-Fi后就会尝试发送数据到Agent所在的IP地址和端口。第二步rcl_get_default_allocator函数使用默认设置初始化allocator。第三步调用rclc_support_init初始化support，该函数的四个参数分别是rclc_support_t结构体指针、参数数量、参数数组指针和分配器指针。第四步调用rclc_node_init_default初始化节点，该函数有四个参数，分别是rcl_node_t指针、节点名称、命名空间和rclc_support_t指针。第五步调用rclc_executor_init初始化执行器，一共有四个参数，分别是rclc_executor_t指针、support中的上下文指针、可处理的句柄数量和分配器的指针。第六步调用rclc_executor_spin，对执行器的事件进行不断的循环处理。最后在setup函数中，通过xTaskCreate函数来创建任务，将其添加到系统的任务队列中运行。
• 代码清单9-39中的代码就是一个完整的micro-ROS节点的编写方法，要和Agent建立通信，需要确保Wi-Fi账户信息、Agent地址和端口号正确，另外需要注意的是ESP32仅支持2.4 GHz的Wi-Fi信号。
• linear表示线速度，angular表示角速度，x、y和z表示速度在三个轴上的分量，在ROS 2中，定义机器人的正前方为x轴，z轴垂直于地面向上，符合右手坐标系，所以线速度在x轴上的分量就是我们需要的机器人线速度，角速度在z轴的分量就是机器人的角速度。接着在代码清单9-39的基础上编写代码，添加速度话题的订阅者和回调函数，在src/main.cpp中添加的代码如代码清单9-43所示。
• 我们在micro_ros_task中首先修改了rclc_executor_init函数的句柄数量参数num_handles为1，这是因为执行器需要一个句柄来处理订阅事件，接着调用函数rclc_subscription_init_best_effort初始化订阅者，该函数的四个参数分别是订阅者指针、节点指针、消息接口类型和话题名称，初始化订阅者后，调用rclc_executor_add_subscription函数将订阅者添加到执行器中，当执行器收到新的消息时就会调用twist_callback函数进行处理，该函数的五个参数分别是执行器指针、订阅者指针、订阅的消息接口指针、回调函数指针和调用情形。需要注意这里使用的是best_effort即最大努力订阅数据，发布者不用确保消息的到达，10.1节有对Qos的详细介绍。
• 在回调函数中我们首先将其转换成geometry_msgs__msg__Twist类型的指针，接着调用运动学逆解函数，将目标的线速度和角速度转换成左右轮的目标速度，然后调用PID控制器更新目标速度。需要注意的是ROS 2订阅来的线速度单位为m/s，这里乘上1000将其转换成mm/s。
• 从注释看可以将该消息接口分为四个部分，第一部分是header，包含时间戳和当前
• 的frame_id；第二部分是子坐标系名称child_frame_id，通过frame_id和child_frame_id来确定里程计是哪两个坐标系之间的关系；第三部分是姿态pose；第四部分是速度twist。
• 了解了接口，我们来编写代码，修改src/main.cpp，添加发布者、进行时间同步和创建定时器，修改的内容如代码清单9-47所示。代码清单9-47　fishbot_motion_control/src/main.cpp
• 在micro_ros_task中，第6步调用micro_ros_string_utilities_set对里程计消息的frame_id和child_frame_id进行初始化赋值，并调用rclc_publisher_init_best_effort初始化里程计发布者，该函数的四个参数分别是里程计发布者指针、节点指针、消息接口和话题名称，需要注意这里使用的是best_effort即最大努力发布数据，不用确保消息的到达。第7步，进行微控制器和主机之间的时间同步，因为里程计消息中需要写明当前的时间，使用rmw_uros_epoch_synchronized函数用于检查时间同步状态，如果返回false，表示时间尚未同步，则使用rmw_uros_sync_session尝试进行时间同步。第8步，为了定时发布里程计话题，我们创建了一个定时器timer，它将每50ms调用一次callback_publisher函数进行里程计话题的发布。

 9.5 移动机器人建图与导航实现

• 图9-23　建图时需要维护的坐标系关系如果只使用单地图，则不需要维护earth到map之间的变换，而base_link到机器人传感器sensor和执行器actuator之间的变换一般使用URDF进行描述，然后使用robot_state_publisher节点进行广播。odom到base_link之间的坐标变换通过测距模块发布，这个测距模块可能是轮式里程计，也有可能是视觉里程计或其他模块。在FishBot中，我们需要根据里程计数据发布坐标变换。 map到odom之间的坐标变换则是由定位组件基于传感器观测并不断重新计算机器人在map坐标系中的姿态，但定位组件并不会广播从map到base_link的变换。它首先接收从odom到base_link的变换，然后计算map到odom之间的坐标变换并发布，在Navigation 2中，由AMCL模块来完成这个工作，但使用slam_toobox建图时，则由slam_toolbox内部组件完成这部分工作。
• URDF用于描述机器人模型，通过URDF文件和robot_state_publisher节点就可以发布基坐标系和各个组件之间的变换，上一节我们了解了移动机器人所需的坐标变换，本节我们就来编写URDF并广播变换。
• 接着新建fishbot_bringup功能包，同样采用默认构建类型，我们将加载URDF和启动相关的命令放到该功能包中，在src/fishbot_bringup下新建launch目录，接着新建urdf2tf.launch.py，该launch文件和6.2.2节的代码类似，编写的文件内容如代码清单9-57所示。代码清单9-57　src/fishbot_bringup/launch/urdf2tf.launch.py
• 关于是否发布TF变换，我们可以通过rqt-tf-tree插件进行验证，打开插件，就可以
• 里程计到机器人的坐标变换，表示的是机器人在里程计坐标系中的位置和姿态，这和里程计话题所表示的内容一致，我们只需要将里程计话题转换成里程计和机器人的坐标变换。
• 代码清单9-62中，将多个节点启动放到同一个launch文件。因为雷达驱动依赖串口转Wi-Fi驱动，所以使用TimerAction，延时5s后启动雷达驱动节点。保存并重新构建功能包，运行该节点，接着给机器人重新上电，在各个节点正常运行后，检查各话题和TF结构是否正常，然后就可以进行建图了。我们依然使用slam_toolbox进行建图，如果没有安装可以根据章节7.2.1的介绍进行安装，在新的终端运行slam_toolbox并设置不使用仿真时间，命令及运行结果如代码清单9-63所示。

 第10章 ROS 2使用进阶

• 因为发布者和订阅者都可以配置自己的QoS策略，所以两者之间可能会因策略不同导致不兼容，不兼容的QoS则无法建立正确的连接。由于DDS只提供了发布订阅通信，所以ROS 2中的服务、参数和动作通信都是基于发布订阅来实现的，兼容性策略对除话题外的通信都适用。

 10.2 执行器与回调组

• 调用executor.spin时，会不断查询来自客户端支持库(ROS Client Support Library)和中间层传入的消息和其他事件，然后调用相应的回调函数进行处理，直到节点关闭，执行流程如图10-2所示。
• 图10-2　Executor执行流程
• ROS 2允许将节点的回调组织成组，回调组配合多线程执行器就可以让回调函数在单独的线程里执行。在代码中可以轻松创建和使用回调组，在rclcpp下的示例代码如代码清单10-14所示。
• 可以看到，使用回调组后，此时服务处理和话题发布就可以并行进行了。因为使用的是互斥回调组，所以此时服务依然无法并行处理，你可以开启多个终端同时发送请求进行测试。
• 修改MutuallyExclusiveCallbackGroup为ReentrantCallbackGroup，再次构建测试，此时你会发现服务已经可以并行处理了。需要注意的是，对于依赖上下文，或者需要处理顺序的场景，不要使用可重入回调组。
• 当有回调函数需要执行回调时，多线程执行器就会查看该回调函数所在的回调组是互斥的还是可重入的。如果是互斥回调组，则检查当前回调组是否有回调在执行，如果没有，则从线程池获取一个线程资源来执行回调；否则会等待上一个执行完成后再执行。如果是可重入回调组，则直接从线程池获取一个线程资源来执行回调，但并不是一定可以获取到的。对于线程池，收到获取线程资源的请求后会检查是否有空闲的线程，有则返回该线程；没有则查看当前激活的线程数量是否到达了代码中配置的最大数量，没有超过则激活一个新的线程执行回调。以上就是ROS 2中回调组和线程之间的关系，你可以通过上面的示例测试验证。
• 可以看到，使用回调组后，此时服务处理和话题发布就可以并行进行了。因为使用的是互斥回调组，所以此时服务依然无法并行处理，你可以开启多个终端同时发送请求进行测试。修改MutuallyExclusiveCallbackGroup为ReentrantCallbackGroup，再次构建测试，此时你会发现服务已经可以并行处理了。需要注意的是，对于依赖上下文，或者需要处理顺序的场景，不要使用可重入回调组。C++回调组和线程池之间的关系和Python中基本相同，可以参考10.2.2节的介绍，这里不再赘述。

 10.3 生命周期节点

• 代码清单10-31中的命令太长，ROS 2提供了命令行工具来设置和获取状态，比如使用如代码清单10-32所示的命令获取状态。
• 返回结果unconfigured表示待配置状态，生命周期节点提供四种可以持续保持的状态，包括Unconfigured（待配置）​、Inactive（待激活）​、Active（已激活）和Finalized（已结束）​，除了这四种稳定状态外，它们之间在切换时会产生中间的转换状态，包括Configuring（配置中）​、CleaningUp（清理中）​、ShuttingDown（关闭中）​、Activating（激活中）​、Deactivating（失活中）和ErrorProcessing（错误处理中）​。
• 我们可以使用ID为1的configure指令，将节点从Unconfigured状态转换成Configuring状态，也可使用ID为5的shutdown指令，将节点从Unconfigured状态转换成ShuttingDown状态。除了configure和shutdown，生命周期节点还支持activate（激活）​、deactivate（失活）​、cleanup（清理）和destory（销毁）指令。使用set命令或调用change_state服务就可以改变节点状态，使用set命令对节点进行配置，指令如代码清单10-34所示。
• 查看/lc_talker/transition_event话题订阅终端，可以看到节点首先从待配置状态转换成配置中，然后再转换成待激活状态。查看所有话题列表，可以看到/lifecycle_chatter话题，但使用命令行工具订阅该话题时就会发现并没有任何输出，这是因为节点处于待激活状态，再次使用命令，来激活节点，命令如代码清单10-36所示。
• 此时再次订阅/lifecycle_chatter就可以看到数据输出了，节点状态也变成了激活状态。在激活状态下可以使用deactivate进行失活，也可以使用shutdown进行关闭。生命周期节点的状态转换流程如图10-4所示。

 10.4 同一进程组织多个节点

• 经过前面的学习，你一定好奇节点、可执行文件以及进程之间的关系。我们知道，可执行文件运行起来一般就是一个进程，而在前面的学习中，一个可执行文件里我们就放置一个节点，运行可执行文件产生的进程里也仅有一个节点。使用独立进程运行单个节点具备进程及故障隔离的优点，更方便进行调试。但如果将多个节点放到同一进程，就可以实现更低的资源消耗和更高效的进程内通信。本节我们来学习将多个节点组织到同一进程的方式，以及实现更高效的进程内通信的方法。
• ROS 2提供多种方式将节点组织到同一进程内，最简单的方式就是直接使用执行器添加多个节点。在使用C++单线程执行器时，示例代码如代码清单10-41所示。代码清单10-41　C++使用单线程执行器加载两个节点
• 该可执行文件会启动两个节点，一个进行话题订阅，另一个进行发布。从结果可以看出，订阅者接收到数据的内存地址和发布者相同，这种直接传递数据地址，没有进行数据复制的传输模式，我们称为零复制传输。相比之下，显然零复制的传输效率更高，尤其是传输大量数据时，但这种传输模式只适用于在同一主机中进行数据传输时。
• 在构造函数里，首先创建了发布者，接着创建了一个匿名函数进行话题数据的定时发布，为了保证每次数据的指针都在同一个地址，发布者使用unique_ptr来指向msg的真实地址。在前面章节中，我们学习并使用了共享式智能指针shared_ptr，共享指针可以将指向的资源分享给其他智能指针，从而使得多个指针指向同一个对象，而独占式智能指针unique_ptr则是独占资源，同一时刻只能有一个独占式智能指针指向同一个对象。因为同一个节点中只需要维护一个buffer，所以这里采用独占式指针进行定义。接着我们来创建Listener类，在目录src/learn_compose/include/learn_compose下新建listener.hpp，然后编写如代码清单10-46所示的内容。
• 10.4.2　使用组件运行组合节点使用组件(Component)可以动态地将不同节点加载到一个进程，也可以动态卸载，这样做的好处显而易见。当我们需要调试节点时，可以单独运行；当需要节省资源开销时就可以组合在一起。在安装ROS 2时，组件就已经安装了，下面我们通过具体的示例来学习如何使用组件。查看当前上下文已经注册的可用组件，命令及部分结果如代码清单10-51所示。
• composition是ROS 2提供的一个组件示例功能包，该功能包下有五个组件例子，我们尝试将Talker和Listener这两个节点组织在一起。首先我们来启动节点容器，其实也是组件管理器，这个节点会提供一些用于加载和卸载节点的服务，不过这些服务是隐藏的服务，需要在service list后追加--include-hidden-services，启动组件节点，命令如代码清单10-52所示。
• 此时观察component_container所运行的终端，就可以看到话题已经开始发布了，在使用load命令时，可以指定被加载节点的名字和命名空间，比如修改节点的名字为talker3，命名空间为/ns，追加指令为--node-name talker3 --node-namespace /ns，除此之外还可以给节点传递参数值，追加-p name:=value即可。在默认情况下被加载的节点并不会使用进程内通信，需要通过附加参数指定，追加指令-euse_intra_process_comms:=true即可开启进程内通信。同样的方法，可以将Listener加载进/component_test容器中去，命令如代码清单10-55所示。

 10.5 使用消息过滤器同步数据

• 在机器人系统中，通常会使用多个传感器来获取环境的信息，比如相机、激光雷达、惯性测量单元(IMU)等。为了正确地将不同传感器的数据关联起来以进行进一步的数据融合或处理，需要确保它们对应同一时间点的信息，因此时间同步是必不可少的。否则，数据可能会被错误地关联，导致严重的系统错误。ROS 2提供了message_filters功能包来进行传感器的数据过滤与同步，本节我们就来学习它的使用方法。
• 第一个概念是Subscriber（订阅者）​，消息过滤器中的订阅者是基于ROS 2的订阅者封装而来的，主要用于配合时间同步器，在使用时可以为其注册单独的回调函数，当收到订阅的消息时就会调用该回调函数。第二个概念是Sync Policies（同步策略）​，ROS 2提供了多个同步策略实现，主要有严格时间对齐(ExactTime)、大约时间对齐(ApproximateTime)和最新时间对齐(LatestTime)，关于三者的区别，在下面的测试示例中会进行演示。第三个概念是Synchronizer（时间同步器）​，我们将同步策略和订阅者传递给时间同步器，由它完成时间同步，并调用相应的回调函数传递同步结果。
• 对节点进行注册，然后构建运行，最后启动第6章的仿真或者播放数据包，可以看到结果如代码清单10-67所示。
• 可以看到，两个数据的时间戳已经对齐输出了。
• 在代码清单10-68中，首先引入了message_filters的订阅者、同步策略和同步器的相关头文件，接着引入了消息接口和客户端库头文件。我们创建了一个节点类TimeSyncTestNode，在构造函数中，首先创建了两个订阅者，并为其注册了回调函数，在回调函数中输出了收到数据的时间戳，然后创建了一个同步器，采用严格时间对齐(ExactTime)策略，MySyncPolicy(10)中的10表示缓存队列的深度，并为其注册了回调函数，在回调函数中输出了imu和odom的时间戳。
• 可以看到，此时只有imu和odom订阅者的回调在工作，并没有任何同步结果输出，这是因为我们采用了严格时间对齐策略，只有两者的数据时间戳完全对齐才会被输出。修改同步策略为大约时间对齐，修改代码如代码清单10-70所示。
• LatestTime策略的工作方式是将较慢的消息重复对齐到最快的消息上进行输出，通过上面的输出可以看到这一点。

 10.6 DDS中间件进阶

• ROS 2使用DDS/RTPS作为中间件，提供了发现、序列化和传输功能。在实际使用中，选择DDS/RTPS时不能“一刀切”​，需要考虑许可证、资源消耗等多种因素。...

 来自微信读书

更多参考：

GitHub - cartographer-project/cartographer_ros: Provides ROS integration for Cartographer. · GitHub

GitHub - cartographer-project/cartographer: Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations. · GitHub

GitHub - slam-xiay/cartographer: Modify by herochiyou · GitHub

https://github.com/ros2/cartographer_ros.git

GitHub - ros-navigation/navigation2: ROS 2 Navigation Framework and System · GitHub

GitHub - figurerobotics/navigation2: ROS2 Navigation Framework and System · GitHub