通过学习ROS2与Navigation2入门教程，了解动态路径规划、计算电机速度、避开障碍物和结构恢复行为
目录： ROS2与Navigation2入门教程-Navigation 2概述 ROS2与Navigation2入门教程-开始使用Nav2 ROS2与Navigation2入门教程-构建和安装Nav2 ROS2与Navigation2入门教程-移动机器人导航概念 ROS2与Navigation2入门教程-设置TF ROS2与Navigation2入门教程-设置URDF ROS2与Navigation2入门教程-设置里程计（Odometry） ROS2与Navigation2入门教程-使用ROS2进行相机标定 ROS2与Navigation2入门教程-在ROS 2/Nav2中获取错误回溯信息 ROS2与Navigation2入门教程-使用Turtlebot3物理机器人进行导航 ROS2与Navigation2入门教程-使用SLAM制图的同时进行导航 ROS2与Navigation2入门教程-使用外部的STVL成本地图插件进行导航 ROS2与Navigation2入门教程-动态物体跟随 ROS2与Navigation2入门教程-使用禁区进行导航 ROS2与Navigation2入门教程-使用速度限制进行导航 ROS2与Navigation2入门教程-编写新的Costmap2D插件 ROS2与Navigation2入门教程-编写新的规划器（Planner）插件 ROS2与Navigation2入门教程-编写新的控制器（Controller）插件 ROS2与Navigation2入门教程-编写新的行为树（Behavior Tree）插件 ROS2与Navigation2入门教程-编写新的恢复器（Recovery）插件 ROS2与Navigation2入门教程-配置航点跟随者（Waypoint Follower） ROS2与Navigation2入门教程-配置行为树导航仪（Behavior-Tree Navigator） ROS2与Navigation2入门教程-配置行为树XML节点（Behavior Tree XML Nodes） ROS2与Navigation2入门教程-配置Costmap 2D ROS2与Navigation2入门教程-配置生命周期管理器（Lifecycle Manager） ROS2与Navigation2入门教程-配置规划器服务器（Planner Server） ROS2与Navigation2入门教程-配置NavFn规划器 ROS2与Navigation2入门教程-配置Smac规划器 ROS2与Navigation2入门教程-配置Theta*规划器（Theta Star Planner） ROS2与Navigation2入门教程-配置控制器服务器（Controller Server） ROS2与Navigation2入门教程-配置地图服务器/保存器（Map Server/Saver） ROS2与Navigation2入门教程-配置DWB控制器（DWB Controller） ROS2与Navigation2入门教程-配置AMCL ROS2与Navigation2入门教程-配置恢复器服务器（Recovery Server） ROS2与Navigation2入门教程-配置受管制纯追踪（Regulated Pure Pursuit）控制器 ROS2与Navigation2入门教程-行为树XMLs概述 ROS2与Navigation2入门教程-导航至某个位姿（Navigate To Pose）行为树 ROS2与Navigation2入门教程-通过多个位姿导航（Navigate Through Poses）行为树 ROS2与Navigation2入门教程-跟随动态点（Follow Dynamic Point）行为树 ROS2与Navigation2入门教程-导航插件（Navigation Plugins） ROS2与Navigation2入门教程-python3简单命令（Simple Commander）API 参考: https://navigation.ros.org/
说明： 介绍Navigation 2 Navigation 2概述 Navigation 2（以下简称“Nav2”）项目是ROS导航（Navigation）软件堆栈的精神继任者。该项目旨在寻找一种安全的方式让移动机器人从A点移动到B点。Nav2也可以应用于其他涉及机器人导航的应用，例如跟踪动态点。为此，Nav2将会完成动态路径规划、计算电机速度、避开障碍物和结构恢复行为。要了解Nav2项目相关项目、使用Nav2的机器人、ROS 1对比、维护者等的更多信息，请参见其“关于和联系方式”网页。 Nav2使用行为树（BT，Behavior Trees）调用模块化服务器来完成一个动作。动作可以是计算路径、控制工作（control efforts）、恢复或任何其他与导航相关的动作。这些都是通过ROS动作服务器与行为树（BT）进行通信的独立节点。下图将让您对Nav2的结构有一个很好的初步了解。请注意：在每个控制器、规划器和恢复器服务器中可以有多个插件，并具有与之匹配的BT插件。这可以用来创建基于具体环境（上下文）的导航行为。若想查看Nav2项目与 ROS (1) Navigation之间的对比，请参阅“ROS到ROS 2 的导航（Navigation）”。 Nav2的预期输入是符合REP-105的TF坐标变换、使用静态成本地图图层的地图源、BT XML文件和任何相关的传感器数据源。这样Nav2就可以为完整性约束机器人或非完整性约束机器人的电机提供有效的速度指令以进行跟随。目前Nav2支持所有的主要机器人类型：完整性约束机器人、差分驱动机器人、腿式机器人和阿克曼（类汽车）机器人等基本类型！Nav2同时使用用于SE2碰撞检测的圆形和任意形状机器人为这些基本类型的机器人提供独特的支持。 Nav2具有下列工具： 加载、提供和存储地图的工具（地图服务器Map Server） 在地图上定位机器人的工具 (AMCL) 避开障碍物从A点移动到B点的路径规划工具（Nav2 Planner） 跟随路径过程中控制机器人的工具（Nav2 Controller） 将传感器数据转换为机器人世界中的成本地图表达的工具（Nav2 Costmap 2D） 使用行为树构建复杂机器人行为的工具（Nav2 行为树和BT Navigator） 发生故障时计算恢复行为的工具（Nav2 Recoveries） 跟随顺序航点的工具（Nav2 Waypoint Follower） 管理服务器生命周期的工具和看门狗(Nav2 Lifecycle Manager) 启用用户自定义算法和行为的插件（Nav2 Core） Nav2还提供了一组启动插件来帮助您进行机器人导航。NavFn使用A*或Dijkstra算法计算从一个位置到达目标位置的最短路径。DWB会使用DWA算法来计算跟踪路径的控制工作，并通过其自身的几个插件计算轨迹评分（critics）。恢复行为包括：等待、旋转、清除成本地图以及后退。有一组行为树（BT）插件用于调用这些服务器和计算条件。最后，还有一组用于与Nav2软件堆栈交互和控制生命周期的Rviz插件。可以在“导航插件（Navigation Plugins）”网页上找到用户报告的全部插件列表。 本Nav2系列教程包括如何安装Nav2、以Turtlebot 3（TB3）为示例机器人使用Nav2的方法、如何为其他机器人进行Nav2自定义、调整行为以获得更佳性能以及自定义内部结构以获得高级软件堆栈等内容。 相关视频 IROS 2020关于Nav2马拉松实验的演讲。查看视频： 这个视频是Turtlebot3（TB3）机器人在小型休息室中进行导航的示例。查看视频
说明： 介绍如何完成Nav2二进制软件包的安装以及在Gazebo仿真软件中进行Turtelbot3仿真机器人导航等过程 安装Nav2 按照ROS官方文档的安装说明进行ROS 2二进制软件包安装。 使用Linux系统的软件包管理器安装Nav2软件包，命令为： sudo apt install ros-<ros2-distro>-navigation2 sudo apt install ros-<ros2-distro>-nav2-bringup 安装Turtlebot3软件包，命令为： sudo apt install ros-<ros2-distro>-turtlebot3* 运行示例 在Linux系统的GUI中打开一个终端。 设置关键环境变量，命令为： source /opt/ros/<ros2-distro>/setup.bash export TURTLEBOT3_MODEL=waffle export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/<ros2-distro>/share/turtlebot3_gazebo/models 如果是从源代码编译安装ROS 2的，或者ROS 2和Gazebo安装目录不是默认目录，则要改成相应的正确目录。 在该终端中运行以下命令： ros2 launch nav2_bringup tb3_simulation_launch.py 对于ROS 2 Dashing Diademata或者更早的发行版，请使用nav2_simulation_launch.py启动文件。但是，建议使用最新的ROS 2长期支持（LST）发行版（即Foxy）以获得更好的稳定性和功能完整性。 这个启动文件会在turtlebot3_world仿真世界中启动带有AMCL定位程序的Nav2软件包。同时还会启动机器人状态发布者节点以提供坐标变换、一个含有Turtlebot3 URDF模型的Gazebo实例和RVIZ。 如果一切正常，就可以看到RViz和Gazebo GUI，如下所示： 单击RViz左下角的“启动（Startup）”按钮。这会让Nav2更改为活动（Active）状态。这样RViz应该会改变外观以显示该地图 导航 启动后，机器人最初并不知道自己在哪里。默认情况下，Nav2会等待用户给出机器人的一个大致起始位置。查看机器人在Gazebo仿真世界中的位置，并在地图上找到该位置。通过单击RViz中的“2D位置估计（2D Pose Estimate）”按钮然后在地图中机器人估计的位置处单击来设置机器人的初始位置。可以通过从刚才单击的位置向前拖动来设置机器人起始的移动方向。 目前为止如果使用的都是默认值，则该机器人应大致如下图所示： 如果设置的机器人位置不完全正确，那也没关系。Nav2在导航时会对位置进行优化。如果愿意，也可以再次单击“2D Pose Estimate”按钮并重新尝试设置机器人的初始位置。 一旦设置好机器人初始位置后，就会完成坐标变换树，且Nav2会完全激活并准备就绪。现在应该可以看到机器人和点云（见下图）。 接下来，单击“导航目标（Navigaton2 Goal）”按钮并选择一个目的地。这会调用BT导航仪（navigator）通过动作服务器到达该目标位置。可以通过在RViz左下角显示的Navigation 2 rviz插件暂停（取消）或重置该动作。 现在就可以看到机器人在移动了（如下图所示）。 参考： https://navigation.ros.org/getting_started/index.html
说明： 介绍如何构建和安装Nav2 安装 Nav2软件包及其依赖包都发布了二进制文件。可以通过以下命令安装它以获得最新的稳定发行版： sudo apt install ros-<distro>-navigation2 ros-<distro>-nav2-bringup ros-<distro>-turtlebot3* 对于Ubuntu 20.04，请使用下面这个命令，因为其通配符的解析已更改： sudo apt install ros-<distro>-navigation2 ros-<distro>-nav2-bringup '~ros-<distro>-turtlebot3-.*' 构建 有3种构建Nav2的方式: 为特定的ROS 2发行版（例如eloquent、foxy）构建Nav2 使用快速启动安装脚本在主分支上构建Nav2 在主分支上手动构建Nav2。 为特定ROS 2发行版构建Nav2 安装ROS,请按照您所需发行版的常用构建说明安装ROS 2。 构建Nav2 首先要新建一个工作空间nav2_ws，将NAV2分支克隆到该工作空间中并进行构建。 可以使用rosdep工具获取ROS 2特定发行版中Nav2依赖包的二进制文件。 具体命令如下： mkdir -p ~/nav2_ws/src cd ~/nav2_ws/src git clone https://github.com/ros-planning/navigation2.git --branch <ros2-distro-branch> cd ~/nav2_ws rosdep install -y -r -q --from-paths src --ignore-src --rosdistro <ros2-distro> colcon build --symlink-install 需要将--branch后的<ros2-distro-branch>更改为所选的分支（例如 eloquent-devel、foxy-devel、galactic、humble） 需要将--rosdistro后的<ros2-distro>更改为所选的ROS 2发行版名称（例如 eloquent、foxy、galactic、humble）。 在主分支上快速启动构建Nav2 构建步骤 从ROS 2安装页面安装ROS 2的所有依赖包。 在执行以下步骤之前，请确保终端或.bashrc文件中没有设置ROS环境变量。 mkdir <directory_for_workspaces> cd <directory_for_workspaces> wget https://raw.githubusercontent.com/ros-planning/navigation2/main/tools/initial_ros_setup.sh chmod a+x initial_ros_setup.sh ./initial_ros_setup.sh 主分支上快速启动构建小结： 脚本initial_ros_setup.sh会下载三个ROS 工作空间，然后按照正确的顺序构建它们。 这三个工作空间分别是： ROS 2发行版：为最新的ROS 2发行版，由此处的存储库文件定义； ROS 2依赖包：为一组未包含在ROS 2发行版中的ROS 2软件包。但是需要这些软件包才能构建Nav2。还包括组成ROS 2发行版的一些软件包，因为Nav2要使用这些软件包的不同版本； Nav2：即本存储库。 下载所有工作空间后，运行navigation2/tools/build_all.sh脚本。 脚本build_all.sh使用colcon build --symlink-install命令按照上面列出的顺序构建每个存储库。 脚本initial_ros_setup.sh可以接受以下选项： --no-ros2：这会跳过下载和构建ROS 2发行版。相反，它会使用安装在/opt/ros/<ros2-distro>目录中的二进制软件包和setup.sh脚本； --download-only：这会跳过构建步骤。 在主分支上手动构建Nav2 构建ROS 2主分支 ！警告信息：在从源代码编译构建ROS2时，请确保ros2.repos文件来自主分支。 按照ROS 2文档中提供的构建说明对ROS 2主分支进行编译构建。 构建Nav2依赖项 由于不是为已发布的发行版构建Nav2，因此必须自己构建依赖包而不是使用这些依赖包的二进制文件。 首先，在ROS 2构建工作空间中对setup.bash 脚本文件进行source，命令为： source ~/ros2_ws/install/setup.bash 接着要从Nav2获取underlay.repos文件。 然后，使用vcs工具将其中的存储库和版本克隆到工作空间中。 具体命令如下： source ros2_ws/install/setup.bash mkdir -p ~/nav2_depend_ws/src cd ~/nav2_depend_ws wget https://raw.githubusercontent.com/ros-planning/navigation2/main/tools/underlay.repos vcs import src < underlay.repos rosdep install -y -r -q --from-paths src --ignore-src --rosdistro <ros2-distro> colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release 构建Nav2主分支 终于有了ROS 2 主分支和必要的依赖包，现在可以构建Nav2主分支本身了。 下面会对nav2_depend_ws工作空间进行source，这也会同时对ROS 2主分支构建工作空间的软件包进行source，以使用依赖包进行Nav2构建。 具体命令为： source ~/nav2_depend_ws/install/setup.bash mkdir -p ~/nav2_ws/src cd ~/nav2_ws/src git clone https://github.com/ros-planning/navigation2.git --branch main cd ~/nav2_ws rosdep install -y -r -q --from-paths src --ignore-src --rosdistro <ros2-distro> colcon build --symlink-install 用Docker构建Nav2 官方Dockerhub条目主要用于Nav2 CI（持续集成），但也可用于开发。 拥有一个跟踪Nav2主（main）分支的docker 镜像很有用。 建议将存储库根目录中的Dockerfile用于生产用途，设置成您选择的发行版。 如果想使用发布的二进制文件，通常建议从容器内的apt存储库安装Nav2发行版。 构建Docker容器 为从Nav2文件夹中的Dockerfile构建映像，需要执行以下操作：首先，将存储库克隆到本地系统（或参阅上面的从源代码构建部分），命令为： sudo docker build -t nav2/latest . 如果需要代理，则命令为： sudo docker build -t nav2/latest --build-arg http_proxy=http://proxy.my.com:### --build-arg https_proxy=http://proxy.my.com:### . 可能还需要配置您的docker以使DNS工作。有关详细信息，请参阅下面这篇文章 如果想从dockerhub缓存构建以加快构建速度，则其命令为： sudo docker pull rosplanning/navigation2:main sudo docker build -t nav2/latest --cache-from rosplanning/navigation2:main . 使用DockerHub容器 允许用户随时从主分支拉取最新的docker镜像。 随着新版本和标签（tag）的发布，docker hub上的docker容器也将被版本化以供选择。 该docker镜像不会包含构建好的叠加层，必须自己构建Nav2叠加层 工作空间（请参阅上面的构建Nav2主分支部分内容）。 命令为： sudo docker pull rosplanning/navigation2:main 生成Doxygen 在Nav2存储库的根目录中运行doxygen。 这会生成一个包含文档的/doc/*目录。浏览器中该文档的入口点是index.html。 实际上，如果采用默认选项，用二进制软件包或者从源代码编译构建安装ROS 2 Foxy发行版时，都会自动安装Navigation 2软件包。 上述教程可以用于安装ROS 2 Foxy发行版时没有安装Navigation 2的情况下参考使用。 参考: https://navigation.ros.org/build_instructions/index.html
说明： 介绍移动机器人导航的概念，了解和使用Nav2项目所需的概念 ROS 2 ROS2是用于Nav2的核心中间件。 如果您对ROS 2不熟悉，请在继续本教程之前先访问ROS 2文档。 动作服务器（Action Server） 就像在ROS中一样，动作服务器是控制导航等长时间运行任务的常用方式。Nav2软件堆栈更广泛地使用动作，并且在某些情况下会缺乏简单的话题接口。作为ROS 2的开发人员，了解动作服务器更为重要。一些简单的命令行界面CLI示例可以在ROS 2文档中找到。 动作服务器类似于规范的服务（service）服务器。客户端会请求完成某个任务，除非该任务可能需要很长时间。例如，将铲子从推土机向上移开或者让机器人向右移动10米就是这类任务的示例。 在这种情况下，动作服务器和客户端允许用户在另一个进程或线程中调用长时间运行的任务，并为其结果返回一个future变量。此时允许阻塞线程直到动作完成，但是，用户可能希望偶尔检查动作是否完成并继续在客户端线程中处理工作。由于是长时间运行的，动作服务器也会向其客户端提供反馈。这个反馈可以是任何事物，并且在ROS中使用具有请求和结果类型的.action文件定义。在推土机示例中，请求可能是一个角度，反馈可能是要移动的剩余角度，而结果则是以结束角度表示的成功或失败布尔值。在导航示例中，请求可能是一个位姿，反馈可能是已导航的时间和到达目标位姿的距离，而结果则是以成功或失败表示的布尔值。 通过向动作客户端注册回调函数，可以同步收集反馈和结果消息。也可以通过从共享的future对象异步请求信息来收集反馈和结果消息。两者都需要旋转（spinning）客户端节点来处理回调函数组。 在Nav2软件堆栈中使用动作服务器通过NavigateToPose动作消息与最高级别的行为树BT导航仪（Navigator）进行通信。它们还用于行为树BT导航仪与后续较小的动作服务器通信，以计算路径规划、控制工作和恢复。每个动作服务器都有自己独特的nav2_msgs格式的.action类型，用于与服务器进行交互。 生命周期节点和绑定（bond） 生命周期（或更准确地说是托管）节点是ROS 2独有的。更多相关信息可以在这里找到。它们是包含用于启动和关停（teardown） ROS 2服务器的状态机过渡状态的节点。这有助于确定ROS系统在启动和关闭时的行为。它还有助于用户以合理的方式构建他们的程序以用于商业用途和调试。 当一个节点启动时，它处于未配置（unconfigured）状态，只处理该节点的构造函数，不应包含任何ROS网络设置或参数读取。通过启动系统或提供的生命周期管理器，需要通过配置将这些节点转换过渡到非活动或非激活（inactive）状态。之后，可以通过过渡到激活阶段来激活节点。 激活状态允许节点处理信息并完全设置为运行。配置阶段触发的on_configure()方法会设置所有参数、ROS网络接口以及安全系统的所有动态内存分配。激活阶段触发的on_activate()方法会激活ROS网络接口并设置程序中的任何状态以开始处理信息。 为关闭这些节点，需要过渡到停用（deactivating）、清理、关闭并以最终状态结束。ROS网络接口分别在这些阶段被停用和停止处理、释放内存、干净地退出。 整个Nav2项目中广泛地使用生命周期节点框架，所有服务器都使用它。如果可能，所有ROS系统最好都使用生命周期节点。 Nav2中使用生命周期节点（LifecycleNodes）的封装器nav2_util LifecycleNode。该封装器为典型的应用程序封装了LifecycleNodes的大部分复杂性。该封装器还包括生命周期管理器的绑定（bond）连接，以确保在服务器过渡到激活状态后，它也保持激活状态。如果服务器崩溃，它会让生命周期管理器知道并关闭系统以防止发生严重故障。有关详细信息，请参阅“从Eloquent到Foxy”。 行为树（Behavior Trees） 行为树（BT）在复杂机器人任务中变得越来越普遍。它们是待完成任务的树形结构。行为树为定义多步或多状态应用程序创建了一个更具可扩展性和人类可理解性的框架。行为树与有限状态机（FSM） 相对，后者可能有几十个状态和数百个状态过渡。一个例子是踢足球机器人。将足球比赛的逻辑嵌入到FSM中将非常具有挑战性，并且容易出错，因为有许多可能的状态和规则。此外，诸如从左侧、右侧或中间射门之类的建模选择尤其不清晰。使用行为树则可以为许多行为创建和重用基本原语，如“踢（kick）”、“走（walk）”、“找球（go to ball）”等。更多相关信息可以在这本书中找到。强烈建议阅读该书的第1-3章以更好地理解术语和工作流程。这应该只会花费大约30分钟时间。 Nav2项目使用BehaviorTree CPP V3作为行为树库。在BT Navigator中创建了可以构建为行为树的节点插件。节点插件被加载到BT 中，而且在解析该行为树的XML文件时，注册的名称会被关联起来。至此，就可以通过该行为树进行导航。 使用BehaviorTree CPP V3库的一个原因是它能够加载子树。这意味着可以将Nav2行为树加载到另一个更高层次的行为树中，以使用此项目作为节点插件。一个例子就是在足球比赛中，使用Nav2行为树作为“找球”节点，将足球检测（detection）作为更大任务的一个部分。此外，为BT提供了一个NavigateToPoseAction插件，这样可以从客户端应用程序通过通常的动作接口调用Nav2软件堆栈。 导航服务器（Navigation Servers） 规划器（planner）和控制器（controller）是导航任务的核心。恢复器（recoveries）用于使机器人摆脱不良状况或者尝试处理各种形式的问题以使系统具有容错能力。在本节中，将分析有关它们的一般概念及其在Nav2项目中的使用。 规划器、控制器和恢复器服务器 Nav2项目中的三个动作服务器是规划器、控制器和恢复器服务器。这些动作服务器用于托管一个地图算法插件以完成各种任务。它们还托管由该算法插件使用的环境表达，以计算其输出。 规划器和控制器服务器会在运行时使用名称（别名）和要使用的算法类型进行配置。这些类型是已注册的插件库名称，而这些名称则是任务的别名。一个示例就是使用名称FollowPath的DWB控制器，因为该控制器会跟随参考路径。在这个示例中，DWB的所有参数都会被放置在该命名空间中，例如FollowPath.。 这两个服务器会公开与其任务对应的动作接口。行为树在勾选相应的BT节点时，会调用动作服务器来处理其任务。该服务器内的动作服务器回调函数会通过映射到特定算法的名称（如FollowPath）调用所选算法。这允许用户将行为树中使用的算法抽象为算法类。例如，用户可以使用N个插件控制器来跟踪路径、与充电器对接、避开动态障碍物，或者与某个工具进行交互。在同一服务器中拥有所有这些插件允许用户使用单个环境表达对象，因为该对象的复制成本很高。 对于恢复器服务器，每个恢复器也会包含它们自己的名称，但是，每个插件也会公开它自己的具体动作服务器。这样做是因为可以创建的各种恢复动作无法共享单个简单的接口。恢复器服务器还包含一个订阅本地成本地图的成本地图订阅者节点，该节点会从控制器服务器接收实时更新以计算其任务。这样做是为了避免为本地成本地图创建多个实例，因为复制本地成本地图在计算上是昂贵的。 或者，既然BT节点是调用某个动作的简单插件，因此可以创建新的BT节点来调用具有其他动作类型的其他动作服务器。如果可能，建议始终使用提供的服务器。如果由于插件或动作接口的原因需要一个新的服务器，则可以通过该框架来维持。与提供的服务器类似，新服务器也应该使用新的类型和插件接口。需要创建一个新的BT节点插件来调用新的动作服务器——但是，通过广泛使用服务器和插件，不需要在Nav2存储库本身中进行分叉（forking）或修改。 如果您发现需要连接至插件库定义或动作类型的新接口，则请提交工单，并查看我们是否可以在相同的接口中纠正该问题。 规划器 规划器的任务是计算完成某个目标函数（objective function）的路径。路径也可以称为路线（route），具体取决于所选的命名法和算法。两个典型的例子就是计算一个到达目标位姿的规划（例如从当前位置到达一个目标位姿）或者完全覆盖（例如覆盖所有空闲空间的规划）。规划器可以访问全局环境表达和缓存在其中的传感器数据。 规划器可以被编写为具有以下功能的工具： 计算最短路径 计算完全覆盖的路径 沿着稀疏或预定义的路线计算路径 Nav2中规划器的一般任务是计算从当前位置到达目标位姿的一个有效且可能是最佳的路径。 但是，有许多受支持的规划类和路线类。 控制器 控制器，在ROS 1中也称为局部规划器，是跟随全局计算路径或完成局部任务的方法。控制器有权访问局部环境表达，以尝试计算要跟随的基准路径的可行控制工作。许多控制器会将机器人向前投射到空间中，并在每次更新迭代时计算局部可行路径。 控制器可以被编写为具有以下功能的工具： 跟随路径 使用里程计坐标系中的检测器与充电站（桩）对接 登上升降机（电梯） 与某个工具的接口 Nav2中控制器的一般任务是计算一个有效的控制工作以跟随全局规划路径。但是，有多个控制器类和局部规划器类。Nav2项目的目标就是所有控制器算法都可以作为此服务器中的插件，以用于一般研究和产业任务中 恢复器 恢复器是容错系统的支柱。恢复器的目标是处理系统的未知状况或故障状况并自主处理这些状况。例子包括感知系统中会导致环境表达充满假障碍物的故障。这样就会触发清除成本地图恢复以允许机器人移动。 另一个例子就是机器人由于动态障碍物或控制不佳而卡住。在允许的情况下，倒退或原地旋转会允许机器人从卡住的位置移动到可以成功进行导航的自由空间中。 最后，在完全故障的情况下，为寻求帮助，可以实施恢复以引起操作人员的注意。这可以通过电子邮件、短信、Slack、Matrix等来完成。 航点跟随（Waypoint Following） 航点跟随是导航系统的基本功能之一。它会告知系统如何使用导航程序到达多个目的地。 软件包nav2_waypoint_follower包含一个航点跟随程序，该程序具有用于特定任务执行器的插件接口。如果需要让机器人前往给定位姿并完成像拍照、捡起盒子或等待用户输入之类的特定任务，这会非常有用。这是一个用于展示如何在示例应用程序中使用Nav2的不错的演示应用程序。 但是，该软件包并不仅仅可以用于示例应用程序。关于机器人队管理器/调度器（fleet managers/dispatchers）有两种思想流派：①哑机器人+智能集中式调度器；②智能机器人+哑集中式调度器。 在第一种思想流派路中，软件包nav2_waypoint_follower完全足以创建产品级的机器人解决方案。由于自主系统/调度器在分配任务时会考虑机器人的位姿、电池电量、当前任务等各种因素，因此机器人上的应用程序只需要关心手头上的任务，而不用关心完成系统要求任务的其他复杂因素。在这种情况下，应该将发送至航点跟随者的请求视为1个工作单元（例如，仓库中的1次拣货、1个安全巡逻循环、1个过道等）来执行任务，然后返回给调度器以进行下一个任务或者要求充电。在这种思想流派中，航点跟随应用程序只是导航软件堆栈之上和系统自主应用程序之下的一个步骤。 在第二种思想流派中，软件包nav2_waypoint_follower是一个不错的示例应用程序/概念证明（proof of concept），但确实需要机器人上的航点跟踪/自主系统来承担更多任务以制定健壮的解决方案。在这种情况下，应该使用nav2_behavior_tree软件包创建自定义应用程序级别的行为树，以使用导航来完成任务。这可以包含子树，例如在任务中检查充电状态以返回停泊坞（dock），或者在更复杂的任务中处理1个以上的工作单元。很快就将会有一个nav2_bt_waypoint_follower软件包（名称可能会调整），让用户可以更轻松地创建此应用程序。在这种思想流派中，航点跟随应用程序与自主系统的联系更加紧密，或者在很多情况下，航点跟随应用程序就是自主系统本身。 这两种思想流派并不能简单地说谁比谁更好，谁更好很大程度上取决于机器人正在完成何种任务、处于何种类型的环境中以及有何种可用的云资源。通常，对于既定的业务案例，这种区别非常明显。 状态估计（State Estimation） 根据ROS社区标准，在导航项目中，要求提供2个主要的坐标变换。由map到odom的坐标变换由定位系统（定位localization, 制图mapping, 即时定位与制图SLAM）提供，而odom到base_link的坐标变换则由里程计系统提供。 注：无需在机器人上使用LIDAR即可使用导航系统。不需要使用基于激光雷达的防撞、定位或SLAM系统。但是，Nav2确实可以提供说明和支持使用激光雷达对这些系统进行尝试和真实实现。使用基于视觉或深度传感器的定位系统和使用其他传感器来避免碰撞可以同样成功。唯一的要求就是在选择具体实现方式时遵循以下标准。 标准 REP 105定义了导航和更大的ROS生态系统所需的坐标系及其约定。应始终遵循这些约定，以利用社区中可用的丰富的定位、里程计和 SLAM项目资源。 简而言之，REP-105告知您必须至少为您的机器人构建一个包含map -> odom -> base_link -> [sensor frames]的完整TF树。TF2是 ROS 2中的时变（time-variant）坐标变换库，Nav2使用TF2来表达和获取时间同步的坐标变换。全局定位系统（GPS、SLAM、动作捕捉Motion Capture）的工作至少要提供map -> odom坐标变换。然后里程计系统的作用就是提供odom -> base_link坐标变换。与base_link相关的其余坐标变换应该都是静态的，并且应该在URDF中定义。 全局定位：定位（Localization）和SLAM 全局定位系统（GPS、SLAM、动作捕捉Motion Capture）的工作至少要提供map -> odom坐标变换。Nav2提供了amcl工具，这是一种基于粒子过滤器的自适应蒙特卡罗定位（Adaptive Monte-Carlo Localization，AMCL）技术，用于静态地图的定位。Nav2还提供了SLAM工具箱（SLAM Toolbox）作为SLAM默认算法，用于定位和生成静态地图。 这些方法也可能产生其他输出，包括位姿话题、地图或其他元数据，但它们必须提供坐标变换才能有效。可以使用机器人定位（robot localization）将多种定位方法融合在一起，下面将会对其详细讨论。 里程计（Odometry） 里程计系统的作用就是提供odom -> base_link坐标变换。里程计可以来自许多数据源，包括激光雷达LIDAR、雷达RADAR、车轮编码器、VIO和IMU。里程计的目标是提供一个基于机器人运动的平滑、连续的局部坐标系。全局定位系统会更新相对于全局坐标系（global frame）的坐标变换以解决里程计漂移问题。 机器人定位（Robot Localization）通常被用于这种融合。它会采用N个各种类型的传感器，并为TF和一个话题提供连续且平滑的里程计。一个典型的移动机器人装置可能具有来自车轮编码器、IMU的里程计以及融合在该范围内的视觉。 这样平滑输出就可用于精确运动的航行位置推算和在全局位置更新之间准确地更新机器人的位置。 环境表达（Environmental Representation） 环境表达是机器人感知环境的方式。它还充当各种算法和数据源的中心定位工具，以将它们的信息组合到单个空间中。这样，控制器、规划器和恢复器就可以使用该空间来安全有效地计算它们的任务。 成本地图（Costmaps）和图层（Layers） 当前的环境表达是一个成本地图。成本地图是一个规则的2D单元格网格，这些单元格中包含来自未知、空闲、占用或膨胀成本的成本。然后会搜索此成本地图以计算全局路径规划或进行采样以计算局部控制工作。 各种成本地图图层被实现为pluginlib插件，以将信息缓冲到成本地图中。这包括来自LIDAR、RADAR、声纳、深度传感器、图像等的信息。在将传感器数据输入到成本地图图层之前对其进行处理可能是明智的，但这取决于开发人员。 可以创建成本地图图层来检测和跟踪场景中的障碍物，以便使用相机或深度传感器来避免发生碰撞。此外，可以创建图层以基于某些规则或启发式算法更改底层成本地图。最后，它们可用于将实时数据缓冲到2D或3D世界中，以进行障碍物的二值化标记。 成本地图过滤器 想象一下，您正在注释地图文件（或任何图像文件），以便根据注释后地图中的位置执行特定动作。标记/注释的示例可以是将某些区域排除在外以避免在这些区域内进行路径规划，或者让某些像素属于标记区域中的最大速度。这种带注释的地图被称为“过滤器掩码（filter mask）”。就像覆盖在某个表面上的掩码一样，过滤器掩码的大小、位置和比例尺可以与主地图相同，也可以不同。过滤器掩码的主要目标是提供在地图上进行标记的能力，以标记具有某些附加功能或行为变化的区域。 成本地图过滤器是基于成本地图图层的方法，可以将过滤器掩码中注释的空间相关行为变化应用到Nav2软件堆栈中。成本地图过滤器是作为成本地图插件实现的。这些插件之所以被称为“过滤器”，是因为它们会通过过滤器掩码上标记的空间注释对成本地图进行过滤。为了制作过滤后的成本地图并改变机器人在注释区域的行为，过滤器插件会读取来自过滤器掩码的数据。该数据在过滤器空间中被线性变换为特征图。有了这个变换后的特征图以及地图/成本地图、任何传感器数据和机器人当前坐标，过滤器就可以更新底层成本地图，并根据机器人的位置改变机器人的行为。 例如，通过使用成本地图过滤器可以实现以下功能： 机器人永远不会进入的禁区/安全区。 限速区：进入这些区域的机器人最大速度将会受到限制。 机器人在工业环境和仓库中移动的首选通道。 其他形式 存在各种其他形式的环境表达，包括： 梯度图，类似于成本地图，但梯度图会表达表面梯度以检查可穿越性（traversibility）； 3D成本地图，以3D形式表示空间，但这样就也需要3D规划和碰撞检测； 网格（mesh）图，类似于梯度图，但具有多个角度的表面网格； “矢量空间”，接收传感器信息并使用机器学习算法来检测要跟踪的单个物品（item）和位置，而不是对离散点进行缓冲区计算。 Nav2学术概述（Academic Overview） 有关Nav2的学术概述，请观看视频 参考： https://navigation.ros.org/concepts/index.html
说明： 介绍ROS中的坐标变换，制作TF2静态发布者的简单命令行演示，以查看其运行情况 坐标变换简介 许多ROS软件包需要使用ROS软件包TF2发布机器人的坐标变换树。坐标变换树定义了不同坐标系之间的关系，包括平移、旋转和相对运动。为了更具体地说明这一点，下面以一个简单机器人为例，该机器人具有一个可移动底座，并在其顶部安装了一个激光传感器。 该机器人有两个定义好的坐标系：一个对应于机器人可移动底座的中心点，而另一个对应于安装在该底座顶部的激光器的中心点。可以将依附于可移动底座的坐标系称为base_link，而将依附于激光的坐标系称为base_laser。请注意，下一节将详细讨论这些坐标系的命名和约定。 此时，假设有一些来自激光器的数据，这些数据是来自激光器中心点的距离测量值。换句话说，在base_laser坐标系中有一些数据。 现在，假设想要获取这些数据并用它来帮助可移动底座避开机器人世界中的障碍物。为了成功做到这一点，需要一种方法将 base_laser坐标系中接收到的激光扫描数据变换到base_link坐标系。从本质上说，就是需要定义base_laser和base_link坐标系之间的关系。 在定义这种关系时，假设我们拥有的唯一数据就是激光器安装在可移动底座中心点前方10厘米和上方20厘米处。这为我们提供了对 base_link坐标系与base_laser坐标系进行关联的平移偏移量。具体来说，我们知道要将base_link坐标系中获取的数据变换到base_laser坐标系，必须应用(x: 0.1m, y: 0.0m, z: 0.2m)的平移量，反之，将base_laser坐标系中获取的数据变换到base_link坐标系，则必须应用相反的平移量，即（x：-0.1m，y：0.0m，z：-0.20m）。 可以选择让用户自己管理这种关系，这意味着在必要时存储和应用坐标系之间的适当变换，但是随着坐标系数量的增加，这会变得非常痛苦。幸运的是，我们可以不必自己做这项工作。相反地，可以使用TF2一次性定义base_link和base_laser坐标系之间的关系，并让TF2来管理这两个坐标系之间的坐标变换。这在处理非静态变换时特别有用，例如一组相对于彼此正在移动的坐标系，如在地图（map）坐标系中的机器人底座坐标系。 为了使用TF2定义和存储base_link和base_laser坐标系之间的关系，需要将它们添加到坐标变换树中。从概念上讲，变换树中的每个节点对应一个坐标系，每条边对应于从当前节点移至其子节点所需执行的坐标变换。TF2使用树结构来保证只需一次遍历即可将任何两个坐标系链接起来，并假设树中的所有边都是从父节点指向子节点的。 要为上面的简单示例创建一个坐标变换树，就会创建两个节点：一个用于base_link坐标系，一个用于base_laser坐标系。要在这两个节点之间创建边，首先需要决定哪个节点是父节点，哪个节点是子节点。请记住，这种区别很重要，因为TF2假设所有坐标变换都是从父节点到子节点的变换。 这里选择base_link坐标系作为父级坐标系，因为当其他部件/传感器添加到机器人上时，通过遍历base_link坐标系将它们与base_laser坐标系关联起来最有意义。这意味着与连接base_link和 base_laser的边相关联的坐标变换应该是 (x: 0.1m, y: 0.0m, z: 0.2m)。 设置好这个变换树后，将base_laser坐标系中接收到的激光扫描变换至base_link坐标系就如同调用TF2库一样简单。这样，机器人现在就可以使用这些变换树信息来对base_link坐标系中的激光扫描进行推理，并安全地规划路径绕过机器人环境中的障碍物。 静态坐标变换发布者节点运行演示 现在来尝试使用TF2提供的static_transform_publisher工具发布一个非常简单的坐标变换。 这里将会发布从链接base_link到链接base_laser的变换，平移量为 (x: 0.1m, y: 0.0m, z: 0.2m)。 请注意，这里会构建来自本教程前面图表中的坐标变换。 打开命令行窗口并执行以下命令： ros2 run tf2_ros static_transform_publisher 0.1 0 0.2 0 0 0 base_link base_laser 这样就成功地在TF2中发布了base_link到base_laser的坐标变换。 现在通过tf2_echo来检查它是否正常工作。 打开另一个单独的命令行窗口并执行以下命令： ros2 run tf2_ros tf2_echo base_link base_laser 这样就应该能够看到类似于下面的重复输出： At time 0.0 - Translation: [0.100, 0.000, 0.200] - Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000] 这个简短的演示就是这样——可以使用TF2库成功地发布从 base_link到base_laser的坐标变换。请注意，不建议在为您的真实机器人项目发布坐标变换时使用上述演示，这只是为了查看TF2实际运行情况的快速演示。对于真实的机器人系统，会创建一个URDF文件，该文件会嵌入这个坐标变换信息以及机器人相关的更多信息，以便使用robots_state_publisher而不是static_transform_publisher。 关于这个问题有更多合适和实用的方法，这将会在“设置URDF”教程中进行讨论。 Navigation2中的坐标变换 强烈建议查看两个重要的ROS REP（ROS Enhancement Proposals）。 这些REP文档详细说明了ROS社区制定的一些标准，以确保在不同的软件包之间正确地进行操作。 Nav2也遵守这些标准和约定。 REP 105——移动机器人平台的坐标系 REP 103——标准度量单位和坐标约定 REP 105的内容简要概括如下：此文档规定了ROS中使用的不同坐标系的命名约定和语义含义。本教程关心的是base_link、odom和map坐标系。坐标系base_link是依附于机器人上的某个固定位置的，通常该位置是在机器人的主机架或底盘（chassis）中心和旋转中心。坐标系odom是相对于机器人起始位置的一个固定坐标系，主要用于距离的局部一致性表示。最后，map坐标系是一个机器人世界固定坐标系，用于距离的全局一致性表示。 另一方面，REP 103则讨论了一些标准度量单位和其他相关约定，以将ROS的不同软件包之间的集成问题降至最低。基本概述就是坐标系是使用右手法则定义的，Z轴向上而X轴向前，单位应为标准的SI（国际单位制）单位。 现在来继续讨论使Navigation2软件包正常运行的一些具体规定。 Nav2要求在ROS中发布下面三个坐标变换： map => odom odom => base_link base_link => base_laser (sensor base frames) *注：base_laser坐标系不包含在REP 105标准中。本指南中使用此名称来指代机器人平台上激光传感器的坐标系。如果有多个基于传感器的坐标系（例如camera_link、base_laser2、lidar_link等），则需要将每个坐标系变换回base_link坐标系。 第一个坐标变换map => odom通常由处理定位和制图的不同 ROS软件包提供，例如AMCL。这个变换会更新使用中的实时数据，因此不会在机器人的TF树中为这个变换设置静态值。有关如何设置的更多详细信息可能非常复杂，因此强烈建议阅读您的平台所使用的制图或定位软件包的文档。所有合规的ROS SLAM软件包和定位软件包都会在启动时自动提供这个坐标变换。 坐标变换odom => base_link通常由里程计系统使用传感器（如车轮编码器）发布。 这通常是使用机器人定位软件包robot_localization通过里程计传感器（IMU、车轮编码器、VIO 等）的传感器融合来计算的。 所有其他静态定义的坐标变换（例如base_link => base_laser、base_link => wheels、wheels => IMU等）都是本指南其余章节将要讨论的内容。Nav2使用此坐标变换树将来自传感器或其他感兴趣坐标系的信息与机器人的其余部件进行正确地关联。这两个坐标系之间的坐标变换通常是通过机器人状态发布者（Robot State Publisher）软件包和统一机器人描述格式URDF（Unified Robot Description Format）文件提供给Nav2的。如果您的平台上有更多传感器坐标系，则需要发布从base_link坐标系到每个传感器坐标系的坐标变换树。 结束语 本教程讨论了坐标变换的概念以及如何在Nav2中使用坐标变换。 在上一节中还探索了使用TF2的static_transform_publisher节点来发布坐标变换。可以使用该节点来设置Nav2的坐标变换，但通常来说这并不是最好的方法。大多数机器人项目中会使用机器人状态发布者（Robot State Publisher）软件包，因为它更容易使用，并且在机器人变得更加复杂时也易于扩展。在下一个教程“设置URDF”中将会讨论Robot State Publisher软件包、URDF以及如何设置URDF。 最后，还讨论了Nav2的三个坐标变换的发布要求以及在设置它们时要记住的必要REP（REP 105和REP 103）。 参考： https://navigation.ros.org/setup_guides/transformation/setup_transforms.html
说明： 介绍如何使用URDF，为该机器人的URDF添加运动学属性 本教程中的完整源代码可以在navigation2_tutorials存储库中的sam_bot_description软件包下找到。 请注意，该存储库包含完成本指南中所有教程后的完整源代码。 URDF和机器人状态发布者（Robot State Publisher）软件包 正如上一教程中所述，Navigation2的要求之一就是从base_link坐标系到各种传感器坐标系和参考坐标系的坐标变换。这个坐标变换树的范围可以是一棵简单的树，从base_link到laser_link只有一个链接，也可以是由位于不同位置上的多个传感器组成的树，每个传感器都有其自己的坐标系。创建多个发布者节点来处理所有这些坐标系变换可能会变得乏味。因此可以使用Robot State Publisher软件包来发布这些坐标变换。 Robot State Publisher是一个ROS 2软件包，该软件包会与tf2软件包进行交互以发布所有必要的坐标变换，这些坐标变换可以直接从机器人的几何形状和结构中推导出来。需要为该软件包提供正确的URDF文件，这样就会自动处理这些坐标变换的发布。这对于复杂的坐标变换非常有用，但仍然建议使用更简单的坐标变换树。 URDF是一种表示机器人模型的XML文件。本教程中URDF主要用于构建与机器人几何体相关的坐标变换树，但URDF还有很多其他用途。一个示例就是URFD如何通过定义材质和网格等视觉组件，在RVIZ（一款用于ROS的3D可视化工具软件）中可视化机器人模型。另一个示例就是如何使用URDF来定义机器人的物理属性。然后将这些物理属性用于物理仿真程序（例如Gazebo），以仿真机器人如何在环境中进行交互。 URDF的另一个主要特性就是它还支持Xacro（XML宏），以帮助用户创建更短且可读的XML文件来帮助定义复杂机器人。可以使用这些宏来消除在URDF中重复一些XML块的需求。Xacro还可用于定义能在整个URDF中重复使用的配置常量。 设置环境 本指南假设您已经熟悉ROS 2以及如何设置您的开发环境，因此可以轻松地完成本节中的各个步骤。 首先来安装本教程中将会使用的一些其它ROS 2软件包。命令为： sudo apt install ros-<ros2-distro>-joint-state-publisher-gui sudo apt install ros-<ros2-distro>-xacro 接下来，为您的项目创建一个目录，初始化一个ROS2工作空间并为您的机器人命名。 对于我们的机器人，我们把它叫做sam_bot。 ros2 pkg create --build-type ament_cmake sam_bot_description 编写URDF 现在已经设置好了项目的工作空间，下面可以直接开始编写URDF。后面将要尝试构建的机器人其图像如下图所示： 首先在src/description目录创建一个名为sam_bot_description.urdf的文件，并输入以下内容作为该文件的初始内容： <?xml version="1.0"?> <robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro"> </robot> *注：下面的代码段应放在标签内。建议按照本教程中介绍的相同顺序添加它们。 这里还包含了一些行号，以让您大致了解在何处输入该代码。 这可能与您正在编写的实际文件不同，具体取决于您对空格的使用。 另请注意，行号假定您正在输入的代码与本指南中显示的代码相同。 接下来，使用XAcro属性定义一些将在整个URDF中重复使用的常量。 代码如下所示： <xacro:property name="base_width" value="0.31"/> <xacro:property name="base_length" value="0.42"/> <xacro:property name="base_height" value="0.18"/> <xacro:property name="wheel_radius" value="0.10"/> <xacro:property name="wheel_width" value="0.04"/> <xacro:property name="wheel_ygap" value="0.025"/> <xacro:property name="wheel_zoff" value="0.05"/> <xacro:property name="wheel_xoff" value="0.12"/> <xacro:property name="caster_xoff" value="0.14"/> 这里简要说明一下这些属性在urdf中代表什么。属性base_*都是用来定义机器人主机架或底盘尺寸的。属性wheel_radius和wheel_width定义机器人两个后轮的形状（半径和厚度）。属性wheel_ygap定义沿y轴调整车轮和底盘之间的间距，而属性wheel_zoff和wheel_xoff则定义沿z轴和x 轴正确地定位后轮。最后，属性caster_xoff定义沿x轴定位前脚轮。 然后来定义base_link，这个链接是一个大的方盒，将作为机器人的主底盘。在URDF中，link元素用于描述机器人的刚性部件或组件。这样机器人状态发布者软件包就可以利用这些定义来确定每个链接的坐标系并发布它们之间的坐标变换。 这里还会定义该链接的一些视觉属性，Gazebo和Rviz等工具可以使用这些视觉属性向我们展示机器人的3D模型。在这些属性中，用于描述该链接的形状，而则描述其颜色。 下面的代码块中使用${property}语法从之前定义的机器人常量代码块访问base属性。此外，还将主底盘的材质颜色设置为Cyan（青色）。请注意，在标签下设置了以下这些参数，这样它们将仅用作视觉参数，而不会影响任何碰撞或物理属性。 <visual> <geometry> <box size="${base_length} ${base_width} ${base_height}"/> </geometry> <material name="Cyan"> <color rgba="0 1.0 1.0 1.0"/> </material> </visual> 接下来会定义一个base_footprint链接。链接base_footprint是一个没有尺寸或碰撞区域的虚拟（非物理）链接。其主要目的是使各种软件包确定机器人投影到地面的中心。例如，Navigation2使用此链接来确定其避障算法中使用的圆形足迹的中心。同样，这里将该链接设置为没有尺寸，并设置机器人投影到地平面时机器人中心所在的位置。 在定义base_link后，又会添加一个连接到base_link的关节。在 URDF中，joint元素可用于描述坐标系之间的运动学和动力学属性。本示例中会定义一个具有适当偏移量的fixed（固定类型）关节，以根据上述描述将base_footprint链接放置在正确的位置。请记住，当从主底盘中心投影时，我们希望将base_footprint设置在地平面上，这样会得到wheel_radius和wheel_zoff的总和，以获得z轴上的正确位置。 <joint name="base_joint" type="fixed"> <parent link="base_link"/> <child link="base_footprint"/> <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/> </joint> 现在要为机器人添加两个大的驱动轮。为了使代码更加清晰并避免重复，这里会使用宏来定义一个代码块，该代码块将使用不同的参数进行重复。这里宏会有3个参数：prefix只是为链接和关节名称添加前缀，而x_reflect和y_reflect允许分别相对于x和y轴翻转车轮的位置。在这个宏中，还可以定义单个车轮的视觉属性。最后，还会定义一个continuous（连续类型）的关节，以允许车轮绕某个轴自由旋转。 这个关节还会将车轮在适当位置连接到base_link上。 在此代码块的末尾，会使用刚刚通过xacro:wheel标签创建的宏实例化两个车轮。 请注意，还会定义一些参数以使机器人尾部两侧各有一个车轮。 <xacro:macro name="wheel" params="prefix x_reflect y_reflect"> <visual> <origin xyz="0 0 0" rpy="${pi/2} 0 0"/> <geometry> <cylinder radius="${wheel_radius}" length="${wheel_width}"/> </geometry> <material name="Gray"> <color rgba="0.5 0.5 0.5 1.0"/> </material> </visual> <joint name="${prefix}_joint" type="continuous"> <parent link="base_link"/> <child link="${prefix}_link"/> <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/> <axis xyz="0 1 0"/> </joint> </xacro:macro> <xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" /> <xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" /> 接下来，会在机器人前面添加一个脚轮。这里会把这个车轮建模为一个球体以保持简单。同样也会定义该脚轮的几何形状、材质和关节以将其在适当位置连接到base_link。 <visual> <geometry> <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/> </geometry> <material name="Cyan"> <color rgba="0 1.0 1.0 1.0"/> </material> </visual> <joint name="caster_joint" type="fixed"> <parent link="base_link"/> <child link="front_caster"/> <origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/> 就是这样！为一个简单差分驱动机器人构建好了一个URDF。 下一节中会重点关注构建包含该URDF的ROS软件包、启动机器人状态发布者以及在RVIz中可视化该机器人。 构建和启动 本节首先会添加构建这个项目时所需的一些依赖包。 打开本项目的根目录，并将以下行添加到package.xml文件中（最好添加在 <buildtool_depend>标签之后）： <exec_depend>joint_state_publisher</exec_depend> <exec_depend>joint_state_publisher_gui</exec_depend> <exec_depend>robot_state_publisher</exec_depend> <exec_depend>rviz</exec_depend> <exec_depend>xacro</exec_depend> 然后来创建启动文件。ROS 2使用启动文件来启动软件包中的必要节点。在本项目的根目录下，创建一个名为launch的子目录，并在该子目录中创建一个display.launch.py文件。下面这个启动文件会启动ROS 2中的一个机器人发布者节点，该节点使用上面编写的URDF发布机器人的坐标变换。此外，该启动文件还会自动启动RVIZ，这样就可以按照URDF的定义可视化该机器人。 将下面的代码段复制并粘贴到display.launch.py文件中： import launch from launch.substitutions import Command, LaunchConfiguration import launch_ros import os def generate_launch_description(): pkg_share = launch_ros.substitutions.FindPackageShare(package='sam_bot_description').find('sam_bot_description') default_model_path = os.path.join(pkg_share, 'src/description/sam_bot_description.urdf') default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz') robot_state_publisher_node = launch_ros.actions.Node( package='robot_state_publisher', executable='robot_state_publisher', parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}] ) joint_state_publisher_node = launch_ros.actions.Node( package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui')) ) joint_state_publisher_gui_node = launch_ros.actions.Node( package='joint_state_publisher_gui', executable='joint_state_publisher_gui', name='joint_state_publisher_gui', condition=launch.conditions.IfCondition(LaunchConfiguration('gui')) ) rviz_node = launch_ros.actions.Node( package='rviz2', executable='rviz2', name='rviz2', output='screen', arguments=['-d', LaunchConfiguration('rvizconfig')], ) return launch.LaunchDescription([ launch.actions.DeclareLaunchArgument(name='gui', default_value='True', description='Flag to enable joint_state_publisher_gui'), launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path, description='Absolute path to robot urdf file'), launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path, description='Absolute path to rviz config file'), joint_state_publisher_node, joint_state_publisher_gui_node, robot_state_publisher_node, rviz_node ]) 为了在进行可视化时让事情变得更加简单，这里会提供一个RVIz 配置文件，在启动软件包时会加载该配置文件。这个配置文件使用适当的设置初始化RVIz，以便可以在机器人启动后立即查看该机器人。在本项目根目录下创建一个名为rviz的子目录，并在该子目录中创建一个名为urdf_config.rviz的文件。 将以下代码段复制粘贴到urdf_config.rviz文件中： Panels: - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /RobotModel1/Links1 - /TF1 Splitter Ratio: 0.5 Tree Height: 557 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Name: Grid - Alpha: 0.6 Class: rviz_default_plugins/RobotModel Description Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /robot_description Enabled: true Name: RobotModel Visual Enabled: true - Class: rviz_default_plugins/TF Enabled: true Name: TF Marker Scale: 0.3 Show Arrows: true Show Axes: true Show Names: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: base_link Frame Rate: 30 Name: root Tools: - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select - Class: rviz_default_plugins/FocusCamera - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 Transformation: Current: Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit Name: Current View Target Frame: <Fixed Frame> Value: Orbit (rviz) Saved: ~ 最后来修改项目根目录中的CMakeLists.txt文件，以包含刚刚在软件包安装过程中创建的文件。 将以下代码段添加到CMakeLists.txt文件中，最好放在if(BUILD_TESTING) 这一行上面： install( DIRECTORY src launch rviz DESTINATION share/${PROJECT_NAME} ) 现在可以用colcon构建该项目。进入项目根目录并执行以下命令： colcon build . install/setup.bash 构建成功后，执行以下命令来安装该ROS 2软件包并启动该项目： ros2 launch sam_bot_description display.launch.py 现在ROS 2应该会启动一个机器人发布者节点并使用上面编写的URDF启动RVIZ。 下一节中将会使用RVIZ来查看该机器人 使用RVIZ进行可视化 RVIZ是一款机器人可视化工具软件，允许用户使用URDF查看机器人的3D模型。使用上一节中的命令成功启动后，RVIZ现在应该在您的计算机屏幕上可见，并且看起来应该如下图所示。可能需要四处移动并操纵视图以仔细查看您的机器人 如您所见，已经成功创建了一个简单的差分驱动机器人并在RVIz中对其进行了可视化。并非必须在RVIz中可视化您的机器人，但这是一个好的步骤以查看是否正确定义了机器人的URDF。这有助于确保机器人状态发布者正在发布正确的坐标变换。 您可能已经注意到还启动了另一个窗口——这是关节状态发布者的GUI。关节状态发布者是另一个ROS 2软件包，用于发布非固定类型关节的状态。可以通过该小的GUI操作该发布者，而且关节的新位姿将会反映在RVIz中。滑动两个车轮中任何一个的滑动条都会旋转这些关节。当在关节状态发布者（Joint State Publisher）GUI中滑动滑块时，可以通过浏览RVIZ来看到关节正在旋转。 至此，由于已经实现了为简单的差分驱动机器人创建URDF这一目标，您可能已决定停止继续学习本教程。机器人状态发布者现在正在发布派生自URDF的坐标变换。现在，其他软件包（例如Nav2）可以使用这些坐标变换来获取有关机器人形状和结构的信息。但是，为了在仿真中正确使用这个URDF，还需要一些物理属性以便机器人像真实机器人一样对物理环境做出反应。可视化字段只能用于可视化，而不能用于碰撞，因此您的机器人会直接穿过障碍物。下一节中会将这些物理属性添加到该URDF中。 添加物理属性 作为本指南的附加部分，这一节中会修改当前的URDF以包含机器人的一些运动学属性。这些信息可能会被诸如Gazebo等物理仿真程序用于对机器人在虚拟环境中的行为方式进行建模和仿真。 首先来定义一些宏，这些宏包含本项目中要使用的几何图元（geometric primitives）的惯性属性。 将下面的代码段放置在URDF中的常量部分之后： <xacro:macro name="box_inertia" params="m w h d"> <inertial> <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/> <mass value="${m}"/> <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/> </inertial> </xacro:macro> <xacro:macro name="cylinder_inertia" params="m r h"> <inertial> <origin xyz="0 0 0" rpy="${pi/2} 0 0" /> <mass value="${m}"/> <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/> </inertial> </xacro:macro> <xacro:macro name="sphere_inertia" params="m r"> <inertial> <mass value="${m}"/> <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/> </inertial> </xacro:macro> 首先会使用标签向base_link添加碰撞区域。还会使用前面定义的box_inertia宏为base_link添加一些惯性属性。 将以下代码段复制放入URDF中base_link的标签内： <collision> <geometry> <box size="${base_length} ${base_width} ${base_height}"/> </geometry> </collision> <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/> 接着对车轮宏执行相同的操作。将以下代码段复制放入URDF中车轮宏的标签内： <collision> <origin xyz="0 0 0" rpy="${pi/2} 0 0"/> <geometry> <cylinder radius="${wheel_radius}" length="${wheel_width}"/> </geometry> </collision> <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/> 最后来为球形脚轮添加类似的属性。 将以下代码段复制放入URDF中脚轮的标签内： <collision> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/> </geometry> </collision> <xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/> *注：这里没有向base_footprint链接添加任何惯性或碰撞属性，这是因为它是一个虚拟和非物理的链接。 构建该项目，然后使用上一节中的相同命令启动RViz。 命令为： colcon build . install/setup.bash ros2 launch sam_bot_description display.launch.py 可以通过在RVIZ左面板中的RobotModel下启用Collision Enabled来验证是否正确设置了碰撞区域（如果还将Visual Enabled关闭掉可能会更容易查看）。 本教程中定义了一个与视觉属性相似的碰撞区域。 请注意，情况可能并非总是如此，因为可能会根据机器人的外观选择更简单的碰撞区域。 现在将不得不在这里停住，因为需要设置更多组件才能在Gazebo中实际开始对该机器人进行仿真。后面会在这些软件包的设置指南课程中回到这个项目，而且一旦进入仿真章节，就会最终看到该机器人在虚拟环境中移动。这项工作中缺少的主要组件是仿真机器人控制器所需的仿真插件。在后续相关章节中会介绍这些插件并将它们添加到这个URDF中。 结束语 本教程中已成功为简单的差分驱动机器人创建了URDF。 还设置了一个ROS 2项目，用于启动机器人发布者节点，然后使用该URDF发布了机器人的坐标变换。 本教程还使用RViz来可视化该机器人，以验证该URDF是否正确。 最后，在URDF中添加了一些物理属性，以便为机器人仿真做好准备。 请随意使用本教程作为您自己的机器人模板。请记住，您的主要目标是发布从base_link到sensor_frames的正确坐标变换。完成这些设置后，可以继续阅读其他设置指南。 参考： https://navigation.ros.org/setup_guides/urdf/setup_urdf.html
说明： 本教程将会研究如何将机器人的里程计系统与Nav2集成 里程计简介 里程计系统基于机器人的运动提供对机器人位姿和速度的局部准确估计。里程计信息可以从各种来源获得，例如IMU、LIDAR、RADAR、VIO和车轮编码器等。需要注意的一件事情是，IMU会随时间漂移，而车轮编码器会随航行距离漂移，因此它们经常一起使用以抵消各自的负面特性。 坐标系odom及与之相关的坐标变换会使用机器人的里程计系统来发布连续的定位信息，但会随着时间或距离（取决于传感器模式和漂移）变得不那么准确。尽管如此，机器人仍然可以使用该信息在其附近进行导航（例如避免发生碰撞）。为了随时间推移仍获得一致准确的里程计信息，坐标系map会提供用于校正odom坐标系的全局准确信息。 正如在之前的指南和REP 105中所讨论的那样，odom坐标系通过 odom => base_link的坐标变换连接到系统的其余部分和Nav2。这个坐标变换由tf2软件包的广播者节点或者robot_localization等框架发布，这些框架还提供了其他功能。在后面的章节中将会更详细地讨论robot_localization框架。 除了要求的odom => base_link坐标变换之外，Nav2还要求发布nav_msgs/Odometry消息，因为这个消息提供了机器人的速度信息。 详细地说，nav_msgs/Odometry消息包含以下信息： # This represents estimates of position and velocity in free space. # The pose in this message should be specified in the coordinate frame given by header.frame_id # The twist in this message should be specified in the coordinate frame given by the child_frame_id # Includes the frame id of the pose parent. std_msgs/Header header # Frame id the pose is pointing at. The twist is in this coordinate frame. string child_frame_id # Estimated pose that is typically relative to a fixed world frame. geometry_msgs/PoseWithCovariance pose # Estimated linear and angular velocity relative to child_frame_id. geometry_msgs/TwistWithCovariance twist 这个消息会告知机器人的位姿和速度的估计值。header消息会提供给定坐标系中的带时间戳数据。pose消息会提供机器人相对于 header.frame_id中指定的坐标系的位置和方向。twist消息会给出相对于child_frame_id中定义的坐标系的线速度和角速度。 在您的机器人上设置里程计 为您的物理机器人设置Nav2的里程计系统很大程度上取决于机器人上可用的里程计传感器有哪些。归因于您的机器人可能需要大量的配置工作，具体的设置说明超出了本教程的范围。相反地，这里会提供一些基本示例和有用的资源来帮助您在机器人上配置Nav2。 首先会使用一个带有车轮编码器作为其里程计来源的机器人示例。请注意，Nav2并不需要车轮编码器，但它在大多数机器人装置中很常见。设置里程计的目标是计算里程计信息并在ROS 2上发布 nav_msgs/Odometry消息和odom => base_link坐标变换。要计算里程计信息，需要设置一些代码，这些代码会将车轮编码器信息转换为里程计信息. 这些代码类似于以下代码段： linear = (right_wheel_est_vel + left_wheel_est_vel) / 2 angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation; 其中right_wheel_est_vel和left_wheel_est_vel分别是右车轮和左车轮的估计速度，而wheel_separation是这两个车轮之间的距离。 right_wheel_est_vel和left_wheel_est_vel的值可以通过简单地获取车轮关节位置随时间的变化来获得。然后可以使用此信息来发布Nav2的要求。有关如何执行此操作的基本示例可以在位于此处的里程计导航文档中找到。 手动发布此信息的替代方法推荐通过ros2_control框架来实现。 ros2_control框架包含了ROS 2中用于实时控制机器人的各个软件包。针对车轮编码器，ros2_control在ros2_controller软件包下有一个 diff_drive_controller（差分驱动控制器）节点。节点diff_drive_controller 会接收在cmd_vel话题上发布的geometry_msgs/Twist消息，计算里程计信息，并在odom话题上发布nav_msgs/Odometry消息。 ros2_control框架中还提供了处理不同类型传感器的其他软件包。 对于其他类型的传感器，如IMU、VIO等，它们各自的ROS驱动程序应该有关于如何发布里程计信息的文档。 请记住，Nav2要求发布nav_msgs/Odometry消息和odom => base_link坐标变换，这应该就是您设置里程计系统时的目标。 使用Gazebo对里程计系统进行仿真 本节中将会使用Gazebo来仿真sam_bot的里程计系统，该机器人是本教程系列中前面两节中构建的机器人。 可以先通读该指南或者在此处获取完整的源代码。 *注：如果您正在使用自己的物理机器人并且已经设置好了里程计传感器，则可以选择跳过本节并进入下一节，在下一节中将会融合IMU和里程计消息以提供一个平滑的odom => base_link坐标变换。 本节内容概述如下：首先会安装Gazebo及使其能与ROS 2协同工作所需的必要软件包。接着会添加Gazebo插件，这些插件会仿真IMU传感器和差分驱动里程计系统，以便分别发布sensor_msgs/Imu和nav_msgs/Odometry消息。最后会在Gazebo环境中生成sam_bot并验证在ROS 2上发布的sensor_msgs/Imu和nav_msgs/Odometry消息。 安装与前提条件 Gazebo是一款3D仿真程序软件，允许用户观察虚拟机器人在仿真环境中的功能。要开始在ROS 2中使用Gazebo，请按照Gazebo安装文档中的安装说明进行Gazebo软件安装。 还需要安装gazebo_ros_pkgs软件包来仿真里程计并在Gazebo中使用ROS 2控制该机器人 其安装命令为： sudo apt install ros-<ros2-distro>-gazebo-ros-pkgs 可以按照此处的说明测试是否已成功设置ROS 2和Gazebo环境。 请注意，这里使用URDF描述sam_bot。但是Gazebo使用仿真描述格式（SDF）描述仿真环境中的机器人。幸运的是，Gazebo会自动将兼容的URDF文件转换为SDF。URDF与Gazebo兼容的主要要求是在每个元素中都要有一个元素。这个要求在sam_bot的URDF文件中已经满足，所以已经可以在Gazebo中使用了。 向URDF中添加Gazebo插件 现在会将Gazebo的IMU传感器和差分驱动插件添加到URDF中。 有关Gazebo中可用的不同插件的概述，请参阅教程“在ROS中使用 Gazebo插件”。 针对该机器人，将会使用GazeboRosImuSensor，它是一个传感器插件（SensorPlugin）。传感器插件必须依附于一个链接，因此会创建一个imu_link，IMU传感器会依附于该链接。该链接将会在元素下被引用。接着会将/demo/imu设置为IMU发布信息的话题，且会通过将initalOrientationAsReference设置为false来遵守REP 145。还会使用Gazebo的传感器噪声模型向传感器配置添加一些噪声。 现在，会根据上面的描述通过在URDF中的行之前添加以下行来设置IMU传感器插件： <visual> <geometry> <box size="0.1 0.1 0.1"/> </geometry> </visual> <collision> <geometry> <box size="0.1 0.1 0.1"/> </geometry> </collision> <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/> <joint name="imu_joint" type="fixed"> <parent link="base_link"/> <child link="imu_link"/> <origin xyz="0 0 0.01"/> </joint> <gazebo reference="imu_link"> <sensor name="imu_sensor" type="imu"> <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin"> <ros> <namespace>/demo</namespace> <remapping>~/out:=imu</remapping> </ros> <initial_orientation_as_reference>false</initial_orientation_as_reference> </plugin> <always_on>true</always_on> <update_rate>100</update_rate> <visualize>true</visualize> <imu> <angular_velocity> <x> <noise type="gaussian"> <mean>0.0</mean> <stddev>2e-4</stddev> <bias_mean>0.0000075</bias_mean> <bias_stddev>0.0000008</bias_stddev> </noise> </x> <y> <noise type="gaussian"> <mean>0.0</mean> <stddev>2e-4</stddev> <bias_mean>0.0000075</bias_mean> <bias_stddev>0.0000008</bias_stddev> </noise> </y> <z> <noise type="gaussian"> <mean>0.0</mean> <stddev>2e-4</stddev> <bias_mean>0.0000075</bias_mean> <bias_stddev>0.0000008</bias_stddev> </noise> </z> </angular_velocity> <linear_acceleration> <x> <noise type="gaussian"> <mean>0.0</mean> <stddev>1.7e-2</stddev> <bias_mean>0.1</bias_mean> <bias_stddev>0.001</bias_stddev> </noise> </x> <y> <noise type="gaussian"> <mean>0.0</mean> <stddev>1.7e-2</stddev> <bias_mean>0.1</bias_mean> <bias_stddev>0.001</bias_stddev> </noise> </y> <z> <noise type="gaussian"> <mean>0.0</mean> <stddev>1.7e-2</stddev> <bias_mean>0.1</bias_mean> <bias_stddev>0.001</bias_stddev> </noise> </z> </linear_acceleration> </imu> </sensor> </gazebo> 现在来添加差分驱动模型插件（ModelPlugin）。下面会配置该插件以便在/demo/odom话题上发布nav_msgs/Odometry消息。左右两轮的关节将会设置为sam_bot的车轮关节。车轮间距和车轮直径会分别根据wheel_ygap和wheel_radius定义的值进行设置。 要将此插件包含在URDF中，请在IMU插件的标签后添加以下几行： <gazebo> <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'> <ros> <namespace>/demo</namespace> </ros> <left_joint>drivewhl_l_joint</left_joint> <right_joint>drivewhl_r_joint</right_joint> <wheel_separation>0.025</wheel_separation> <wheel_diameter>0.2</wheel_diameter> <max_wheel_torque>20</max_wheel_torque> <max_wheel_acceleration>1.0</max_wheel_acceleration> <publish_odom>true</publish_odom> <publish_odom_tf>true</publish_odom_tf> <publish_wheel_tf>true</publish_wheel_tf> <odometry_frame>odom</odometry_frame> <robot_base_frame>base_link</robot_base_frame> </plugin> </gazebo> 启动和构建文件 现在会编辑启动文件launch/display.launch.py以在Gazebo中生成sam_bot机器人。 由于这里要仿真该机器人，可以删除关节状态发布者的 GUI，方法是通过删除generate_launch_description()内的以下几行代码来实现的： joint_state_publisher_gui_node = launch_ros.actions.Node( package='joint_state_publisher_gui', executable='joint_state_publisher_gui', name='joint_state_publisher_gui', condition=launch.conditions.IfCondition(LaunchConfiguration('gui')) ) 删除在返回launch.LaunchDescription中的下面这一行代码： joint_state_publisher_gui_node, 然后打开package.xml文件并删除下面这一行代码: <exec_depend>joint_state_publisher_gui</exec_depend> 要启动Gazebo，请在joint_state_publisher_node之前添加下面这一行代码： launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'], output='screen'), 现在将会添加一个在Gazebo中生成sam_bot的节点。再次打开 launch/display.launch.py并在return launch.LaunchDescription([这一行之前粘贴下面这几行代码： spawn_entity = launch_ros.actions.Node( package='gazebo_ros', executable='spawn_entity.py', arguments=['-entity', 'sam_bot', '-topic', 'robot_description'], output='screen' ) 然后在rviz_node这一行之前添加一行spawn_entity，如下所示： robot_state_publisher_node, spawn_entity, rviz_node ]) 构建、运行和验证 现在来运行该软件包，以检查/demo/imu和/demo/odom话题在该系统中是否处于活动状态。 进入到该项目的根目录并执行以下命令： colcon build . install/setup.bash ros2 launch sam_bot_description display.launch.py 这样就会启动Gazebo并且应该可以看见sam_bot机器人的3D模型，如下图所示： 要查看该系统中的活动话题，请打开一个新的终端并执行命令： ros2 topic list 这样就应该会在话题列表中看到/demo/imu和/demo/odom这两个话题。 要查看有关这两个话题的更多信息，请执行以下命令： ros2 topic info /demo/imu ros2 topic info /demo/odom 这样就应该会看见如下所示的输出消息： Type: sensor_msgs/msg/Imu Publisher count: 1 Subscription count: 0 Type: nav_msgs/msg/Odometry Publisher count: 1 Subscription count: 0 可以看到，/demo/imu话题会发布sensor_msgs/Imu类型的消息，而/demo/odom话题则会发布nav_msgs/Odometry类型的消息。 在这两个话题上发布的信息分别来自IMU传感器和差分驱动的Gazebo仿真。 另外还可以注意到，这两个话题目前都没有订阅者。 在下一节中会创建一个robot_localization节点来订阅这两个话题。 然后会使用这两个话题上发布的消息为Nav2提供一个融合好的、局部准确且平滑的里程计信息。 机器人定位演示 软件包robots_localization被用于从N个里程计传感器输入提供的数据中提供融合的、局部准确的、平滑的里程计信息。这些信息可通过geometry_msgs/PoseWithCovarianceStamped、nav_msgs/Odometry、sensor_msgs/Imu和geometry_msgs/TwistWithCovarianceStamped消息类型提供给该软件包。 普通的机器人装置至少包括车轮编码器和IMU作为其里程计传感器数据源。当向robot_localization软件包提供多个里程计传感器数据源时，可以通过使用状态估计节点来融合这些传感器给出的里程计信息。这些节点使用扩展卡尔曼滤波器 (ekf_node) 或无迹卡尔曼滤波器 (ukf_node) 来实现这种融合。此外，该软件包还实现了一个 navsat_transform_node节点，在使用 GPS 时，该节点可以将地理坐标转换为机器人的世界坐标系坐标。 如果在其配置中启用了相应的功能，则robot_localization 软件包会通过odometry/filtered和accel/filtered话题发布融合的传感器数据。此外，该软件包还可以在/tf话题上发布odom => base_link坐标变换。 如果您的机器人只能提供一个里程计来源，那么除了平滑之外，使用robot_localization软件包的影响很小。在这种情况下，另一种方法是通过tf2广播者（broadcaster）节点在您的单一里程计节点来源中发布坐标变换。尽管如此，您仍然可以选择使用robots_localization软件包来发布这些坐标变换，并且在输出中仍然可能会观察到一些平滑特性。 本节的其余部分将会展示如何使用robot_localization软件包来融合sam_bot机器人的传感器数据。该软件包会使用/demo/Imu话题上发布的sensor_msgs/Imu消息和在/demo/odom话题上发布的nav_msgs/Odometry消息，然后会在odometry/filtered、accel/filtered和/tf话题上发布数据。 配置机器人定位（Robot Localization）软件包 现在来配置robot_localization软件包以使用扩展卡尔曼滤波器（ekf_node）来融合里程计信息并发布odom => base_link坐标变换。 首先，使用您机器上的软件包管理器或者通过执行以下命令来安装robot_localization软件包： sudo apt install ros-<ros2-distro>-robot-localization 然后会使用YAML文件指定ekf_node的参数。 在项目根目录下创建一个名为config的子目录，并在该子目录中创建一个名为ekf.yaml的文件。 将以下代码行复制到ekf.yaml文件中： ### ekf config file ### ekf_filter_node: ros__parameters: # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin # computation until it receives at least one message from one of theinputs. It will then run continuously at the # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. frequency: 30.0 # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected # by, for example, an IMU. Defaults to false if unspecified. two_d_mode: false # Whether to publish the acceleration state. Defaults to false if unspecified. publish_acceleration: true # Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified. publish_tf: true # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame. # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame" # to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark # observations) then: # 3a. Set your "world_frame" to your map_frame value # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node # from robot_localization! However, that instance should *not* fuse the global data. map_frame: map # Defaults to "map" if unspecified odom_frame: odom # Defaults to "odom" if unspecified base_link_frame: base_link # Defaults to "base_link" ifunspecified world_frame: odom # Defaults to the value ofodom_frame if unspecified odom0: demo/odom odom0_config: [true, true, true, false, false, false, false, false, false, false, false, true, false, false, false] imu0: demo/imu imu0_config: [false, false, false, true, true, true, false, false, false, false, false, false, false, false, false] 在此配置中，定义了frequency、two_d_mode、publish_acceleration、publish_tf、map_frame、odom_frame、base_link_frame和world_frame 的参数值。有关可以修改的其他参数的更多信息，请参阅“状态估计节点的参数”API文档，而在此处可以找到示例efk.yaml文件。 要将传感器输入添加到ekf_filter_node，请将序列中的下一个数字添加到其基本名称上（odom、imu、pose、twist）。本示例中有一个 nav_msgs/Odometry和一个sensor_msgs/Imu作为过滤器的输入，因此使用odom0和imu0。这里将dom0的值设置为demo/odom，也就是发布nav_msgs/Odometry消息的话题。同样地，将imu0的值设置为发布sensor_msgs/Imu的话题即demo/imu。 可以使用_config参数指定过滤器要使用一个传感器中的哪些值。这个参数值的顺序是x、y、z、roll、pitch、yaw、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。 在本示例中将odom0_config中的所有内容都设置为false，除了第1、2、3和12个条目，这意味着过滤器仅会使用odom0的 x、y、z和vyaw值。 在imu0_config矩阵中，可以注意到仅使用了roll、pitch和yaw。典型的移动机器人级IMU还会提供角速度和线性加速度。为了使robot_localization软件包正常工作，不应该在相互衍生的多个字段中进行融合。由于角速度在内部融合到IMU以提供横滚角（roll）、俯仰角（pitch）和偏航角（yaw）估计，因此不应该融合用于导出该信息的角速度。 考虑到在不使用特别高质量（和昂贵）的IMU时角速度具有噪声的特性，也不会融合角速度。 启动和构建文件 现在来把ekf_node添加到启动文件中。 打开display.launch.py文件并将以下代码行粘贴到return launch.LaunchDescription([行之前： robot_localization_node = launch_ros.actions.Node( package='robot_localization', executable='ekf_node', name='ekf_filter_node', output='screen', parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}] ) 然后在return launch.LaunchDescription([代码块中添加下列启动参数选项： launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True', description='Flag to enable use_sim_time'), 最后在rviz_node 行上面添加robot_localization一行，以启动robot localization节点 如下所示： robot_state_publisher_node, spawn_entity, robot_localization_node, rviz_node ]) 接下来需要向软件包定义中添加robot_localization的依赖项。 打开package.xml文件并在最后的<exec_depend>标签后面添加下面这一行代码： <exec_depend>robot_localization</exec_depend> 最后打开 CMakeLists.txt文件并在install(DIRECTORY...)内添加配置目录 如下代码段所示： install( DIRECTORY src launch rviz config DESTINATION share/${PROJECT_NAME} ) 构建、运行和验证 现在来构建和运行该软件包。 进入到项目根目录并执行以下命令： colcon build . install/setup.bash ros2 launch sam_bot_description display.launch.py 这样Gazebo和RVIZ应该就会启动。在RVIZ窗口中，应该可以看见sam_bot机器人模型和TF坐标系 如下图所示： 接着来验证在该系统中odometry/filtered、accel/filtered和/tf话题是否处于活动状态。 打开一个新的终端并执行以下命令： ros2 topic list 这样就应该可以在话题列表中看见odometry/filtered、accel/filtered和/tf话题。 通过再次执行以下命令还可以查看这些话题的订阅者个数： ros2 topic info /demo/imu ros2 topic info /demo/odom 现在应该会看到/demo/imu和/demo/odom话题都各有1个订阅者。 要验证ekf_filter_node是这两个话题的订阅者，需要执行命令： ros2 node info /ekf_filter_node 这样应该就会看见如下所示的输出消息： /ekf_filter_node Subscribers: /demo/imu: sensor_msgs/msg/Imu /demo/odom: nav_msgs/msg/Odometry /parameter_events: rcl_interfaces/msg/ParameterEvent /set_pose: geometry_msgs/msg/PoseWithCovarianceStamped Publishers: /accel/filtered: geometry_msgs/msg/AccelWithCovarianceStamped /diagnostics: diagnostic_msgs/msg/DiagnosticArray /odometry/filtered: nav_msgs/msg/Odometry /parameter_events: rcl_interfaces/msg/ParameterEvent /rosout: rcl_interfaces/msg/Log /tf: tf2_msgs/msg/TFMessage Service Servers: 从上面的输出中可以看到ekf_filter_node订阅了/demo/imu和 /demo/odom话题。 还可以看到ekf_filter_node在odometry/filtered、accel/filtered和/tf话题进行了消息发布。 还可以用tf2_echo实用程序验证robots_localization是否正在发布odom => base_link坐标变换。 在单独的命令行终端中运行以下命令： ros2 run tf2_ros tf2_echo odom base_link 这样就应该会看见一条如下所示的连续输出： At time 8.842000000 - Translation: [0.003, -0.000, 0.127] - Rotation: in Quaternion [-0.000, 0.092, 0.003, 0.996] At time 9.842000000 - Translation: [0.002, -0.000, 0.127] - Rotation: in Quaternion [-0.000, 0.092, 0.003, 0.996] 结束语 本指南中讨论了Nav2期望从里程计系统中获得的消息和坐标变换。 已经看到了如何设置里程计系统以及如何验证发布的消息。 还讨论了robot_localization软件包如何使用多个里程计传感器来提供一个过滤好的和平滑的里程计。 还检查了robot_localization软件包是否可以正确发布odom => base_link坐标变换。 参考： https://navigation.ros.org/setup_guides/odom/setup_odom.html
说明： 介绍如何使用ROS2进行相机标定 概述 相机（摄像头）是一种非常精密的光学仪器，对外界环境的感知非常敏感。 由于摄像头内部和外部的一些原因，摄像头采集的图像常常会发生一定的畸变。 如果直接将采集到的图像拿来进行图像处理，会产生很大的问题。 为了避免图像数据源造成的误差，需要对摄像头的相关参数进行标定。 本教程将会说明如何标定单目（monocular）相机，从而获取单目相机的标定参数。 要求 使用操作系统的软件包管理器apt安装相机标定需要用到的以下软件包，命令如下： sudo apt install ros-<ros2-distro>-camera-calibration-parsers sudo apt install ros-<ros2-distro>-camera-info-manager sudo apt install ros-<ros2-distro>-launch-testing-ament-cmake 需要在您的工作空间中从源代码构建Image Pipeline软件包，命令如下： cd ~/dev_ws/src （根据您的工作空间更改成相应的目录） git clone https://github.com/ros-perception/image_pipeline.git cd image_pipeline git checkout ros2 cd ~/dev_ws colcon build --symlink-install --packages-select image_pipeline 如果您的工作空间中只有image_pipeline软件包，则可以不用--packages-select image_pipeline选项，即直接执行colcon build --symlink-install命令即可。 但如果您的工作空间有很多软件包，为节省时间和避免破坏已经构建好的软件包，则应该选择仅构建image_pipeline软件包。 实际上image_pipeline只是一个元软件包，真正起作用的是下面的这几个软件包：camera_calibration、depth_image_proc、image_proc、 image_publisher、image_rotate、image_view、stereo_image_proc。 如果在构建过程中出现错误，提示在您的系统上是否尚未构建camera_calibration、depth_image_proc、image_proc、 image_publisher、image_rotate、image_view、stereo_image_proc软件包，则需要先构建这些软件包. 命令为： colcon build --packages-select camera_calibration depth_image_proc image_proc image_publisher image_rotate image_view stereo_image_proc 构建好这些软件包后再运行colcon build --symlink-install --packages-select image_pipeline命令就可以成功构建该软件包了。 构建好image_pipeline软件包后，需要对该工作空间进行source。 为了避免每次打开终端时都需要设置环境，可以直接运行以下命令来一次性设置好运行环境： echo “source ~/dev_ws/install/setup.bash” >> .bashrc 此外，标定相机还需要依赖其它一些软件包，可以执行以下命令以确保安装了这些软件包： sudo apt install ros-foxy-vision-opencv ros-foxy-image-transport-plugins ros-foxy-image-transport ros-foxy-image-common sudo apt install python3-polled-camera polled-camera-tool libpolled-camera0d sudo apt install libtheora-dev libogg-dev libboost-python-dev sudo apt-get install -y libssl-dev libusb-1.0-0-dev pkg-config libgtk-3-dev sudo apt-get install -y libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev 如果您的机器上已经安装了这些软件包的最新版本，则会自动跳过，否则会安装这些软件包。 其它要求 还请确保您具备以下条件： （1）一个已知尺寸的大棋盘。本教程使用的是200mm×200mm的正方形棋盘，用于相机定标需要7行×9列棋盘格。由于相机标定使用的是棋盘格的内部顶点，因此需要8行×10列的棋盘，这样使用内部顶点参数就正好是7行×9列，如下图所示。可以从这里下载具有设定尺寸的棋盘格PDF文件，然后打印出来备用。 （2）一个明亮的5m×5m区域，没有障碍物和棋盘图案。 （3）一个可以在ROS上发布图像的单目相机。 具体标定步骤 在GUI中打开一个终端。 为您的相机启动ROS驱动程序。 对于普通USB相机，需要一个节点把普通USB相机的影像流转换成ROS的话题（topic）发布出来，才能被标定工具使用。在ROS 1中该节点是usb_cam；但在ROS 2中该节点更改为v4l2_camera。 在ROS 2中，执行以下命令来启动待标定相机： ros2 run v4l2_camera v4l2_camera_node --ros-args -p video_device:="/dev/video0" -p image_size:=[1280,720] 这样就会在终端中出现如下图所示的消息： 注意：此时会出现错误，提示找不到相机标定参数yaml文件，这是由于尚未标定该相机或者尚未将标定参数yaml文件复制到该目录中。 后面在完成相机标定并保存了标定结果后，就需要将相机标定参数yaml文件复制到该目录并修改成提示中的文件名。 如果不能成功地运行上一条命令，提示找不到v4l2_camera软件包，则需要手动安装该软件包，命令为： sudo apt install ros-foxy-v4l2-camera *提示：若您的ROS 2发行版不是Foxy，则需要把上一条命令中的“foxy”替换成您的具体发行版名称。 另外还注意到上条命令中的参数video_device:="/dev/video0"，用于指定要启动的摄像头设备，即要启动和标定哪个相机，特别是如果您的机器上安装有多个相机的话。 如果机器上只有一部USB相机，则一般应该是/dev/video0。 为了验证这一点，需要执行以下操作： 列出机器上的所有相机（摄像头）设备，命令为： ls /dev/video* 在笔者的机器上输出消息如下： /dev/video0 /dev/video1 找出真正的摄像头设备。笔者的笔记本电脑上只有一个USB相机，到底哪个才是真正的摄像头？这就需要用到v4l2-utils软件包的v4l2-ctl命令。 首先执行命令sudo apt install v4l2-utils安装该软件包，然后再对上面列出的2个摄像头设备的驱动程序信息进行查询，命令分别为 v4l2-ctl -d /dev/video0 --all和v4l2-ctl -d /dev/video1 --all。 这样，/dev/video0的驱动程序信息输出结果如下所示： Driver Info: Driver name : uvcvideo Card type : Integrated_Webcam_HD: Integrate Bus info : usb-0000:00:14.0-11 Driver version : 5.8.18 Capabilities : 0x84a00001 Video Capture Metadata Capture Streaming Extended Pix Format Device Capabilities Device Caps : 0x04200001 Video Capture Streaming Extended Pix Format Priority: 2 Video input : 0 (Camera 1: ok) Format Video Capture: Width/Height : 1280/720 Pixel Format : 'YUYV' (YUYV 4:2:2) Field : None Bytes per Line : 2560 Size Image : 1843200 Colorspace : sRGB Transfer Function : Default (maps to sRGB) YCbCr/HSV Encoding: Default (maps to ITU-R 601) Quantization : Default (maps to Limited Range) Flags : Crop Capability Video Capture: Bounds : Left 0, Top 0, Width 1280, Height 720 Default : Left 0, Top 0, Width 1280, Height 720 Pixel Aspect: 1/1 Selection Video Capture: crop_default, Left 0, Top 0, Width 1280, Height 720, Flags: Selection Video Capture: crop_bounds, Left 0, Top 0, Width 1280, Height 720, Flags: Streaming Parameters Video Capture: Capabilities : timeperframe Frames per second: 10.000 (10/1) Read buffers : 0 （省略部分内容） 而/dev/video1的驱动程序信息输出结果为： Driver Info: Driver name : uvcvideo Card type : Integrated_Webcam_HD: Integrate Bus info : usb-0000:00:14.0-11 Driver version : 5.8.18 Capabilities : 0x84a00001 Video Capture Metadata Capture Streaming Extended Pix Format Device Capabilities Device Caps : 0x04a00000 Metadata Capture Streaming Extended Pix Format Priority: 2 Format Metadata Capture: Sample Format : 'UVCH' (UVC Payload Header Metadata) Buffer Size : 1024 其中/dev/video0的驱动程序信息中出现了Video input : 0 (Camera 1: ok)这一行，而/dev/video1的则没有这行，表明/dev/video0才是真正的USB相机。 确保待标定的相机可以在ROS上发布图像。 为此，在待标定单目相机ROS驱动程序正在另一个终端中启动运行的情况下，在一个新的终端中执行以下命令： ros2 topic list 这样在该终端中应该就会输出如下消息： /camera_info /image_raw /image_raw/compressed /image_raw/compressedDepth /image_raw/theora /parameter_events /rosout 可以看到您的系统正在发布image_raw等多个与摄像头相关的ROS话题。 为了确认image_raw话题是一个真实的话题且实际上正在发布图像，需要检查该话题的发布频率，命令为： ros2 topic hz /image_raw 这样终端中应该就会不断输出如下所示的消息： average rate: 10.035 min: 0.096s max: 0.100s std dev: 0.00114s window: 12 average rate: 10.019 min: 0.096s max: 0.100s std dev: 0.00084s window: 23 average rate: 10.024 min: 0.096s max: 0.100s std dev: 0.00096s window: 34 average rate: 10.028 min: 0.096s max: 0.100s std dev: 0.00100s window: 45 average rate: 10.022 min: 0.096s max: 0.100s std dev: 0.00091s window: 55 average rate: 10.025 min: 0.096s max: 0.100s std dev: 0.00096s window: 66 从输出消息中可以看出，该相机确实正在发布图像，且其图像发布频率平均约为10帧/秒。 启动相机标定节点，命令为： ros2 run camera_calibration cameracalibrator --size 7x9 --square 0.015 --ros-args --remap /image:=/image_raw --ros-args --remap camera:=/custom_camera 这条命令中需要注意以下三个容易出错的地方： 棋盘格行列数即--size参数，7x9不能写成7*9或者7×9，那个乘号必须写成小写字母x，否则会出错； 棋盘格的格子大小即--square参数，应该是每个正方形格子的边长，单位为m，本例中用的格子边长为15mm，因此该参数应该为0.015，而不是英语原文教程中的0.2m； 指定相机名和话题名时应该使用新的语法，即采用--ros-args --remap选项来重映射图像话题名称和相机名称，否则会提示警告消息，但这并不会影响该命令的执行，而更重要的是重映射前后的话题名称/image和/image_raw以及相机名称camera和/custom_camera不能写错，而且等号前必须有冒号（:），否则该命令无法成功运行。 输入命令正确的话，应该就会成功地输出如下消息： Waiting for service camera/set_camera_info ... OK Waiting for service left_camera/set_camera_info ... OK Waiting for service right_camera/set_camera_info ... OK 然后就会弹出一个带有摄像头视频的窗口，如下图所示。 为了更好地标定相机，需要在相机视野范围内不断移动棋盘格和变换位置（先拿着打印好的棋盘格纸张置于摄像头前并适当歪斜，当视频窗口中的棋盘格上出现彩色线条时，就可以缓慢地上下左右平移棋盘格、让棋盘格远离和接近摄像头、旋转和倾斜棋盘格等），以便达到以下效果： 让棋盘格出现在相机视场或视野（FOV，field of view）的左、右、上、下不同部位 X条——相机视野中的左/右方向 Y条——相机视野的上/下方向 尺寸条——接近/远离相机和相对于相机进行倾斜 让棋盘格填满整个视野 让棋盘格向左、右、上、下倾斜（歪斜Skew） 随着棋盘格的不断移动，标定窗口侧边栏上的4个横条的长度会不断增长。而在终端窗口中，则会出现如下图所示的图像样本采集情况的消息输出。 当所有4个横条都变成绿色并且有足够的数据可用于标定时（笔者标定测验时系统采集了42幅图像时4个横条全部变成了绿色），“标定（CALIBRATE）”按钮将会变亮。单击它以执行相机标定。 标定大约需要花一分钟时间。 完成标定后，“保存（SAVE）”按钮和“提交（COMMIT）”按钮都会变亮。而且在终端中也可以看到相机标定的各个参数。 按下“保存”按钮来保存相机标定结果。相机标定数据会保存在/tmp/calibrationdata.tar.gz文件中 要使用该相机标定结果文件，请执行以下命令解压缩该文件： tar -xvf calibration.tar.gz 在标定结果文件夹中，可以看到系统采集的用于标定的所有图像以及ost.yaml和ost.txt文件（如下图所示）。 在启动相机驱动程序时会使用这个包含校准参数的yaml文件 为了让相机ROS 2启动程序能够找到该yaml文件，需要把这个ost.yaml文件复制到~/.ros/camera_info/目录下，并将文件名修改为integrated_webcam_hd:_integrate.yaml（具体路径和文件名可能在不同机器上会不相同，需要查看运行v4l2_camera节点启动相机时那条找不到相机标定参数yaml文件的错误消息，该错误消息中包含了相机驱动程序会寻找的标定参数yaml文件的相应路径和文件名）。 参考资料 Navigation2普通教程系列中的“相机标定”教程，其英语原文网址 古月居网站上机器视觉专栏文章“在ROS平台上标定普通摄像头与Kinect摄像头” ChinaUnix博客文章“ROS2 摄像头校准” CSDN博客文章“ubuntu20.04如何识别到连接的摄像头？” OS2 Image Pipeline Tutorial: How to create Point Clouds using two USB cameras
ROS2与Navigation2入门教程-在ROS 2/Nav2中获取错误回溯信息
说明： 介绍如何获取ROS 2和Nav2错误回溯信息的一系列方法 概述 本教程将会说明获取ROS 2和Nav2错误回溯信息的一系列方法。 有很多方法可以实现ROS 2和Nav2错误回溯，但对于没有GDB经验的C++开发人员新手来说，这是一个很好的起点。 下面这些步骤会向ROS 2使用者说明在遇到问题时如何修改 Nav2软件堆栈以从特定服务器获取错误跟踪信息。本教程适用于仿真机器人和物理机器人。 本教程将会涵盖如何使用ros2 run命令从特定节点、使用ros2 launch命令从含有单个节点的启动文件以及更复杂的节点组合中获取错误回溯信息。 在本教程结束时，当遇到ROS 2中的服务器崩溃时，应该就有能力获取其错误回溯信息。 前提条件 GDB（GNU symbolic debugger）是Unix系统上最流行的C++调试器。它可用于确定崩溃的原因并跟踪线程。它还可用于在代码中添加断点（breakpoints）以检查软件中某个特定点在内存中的值。 对于所有使用C/C++的软件开发人员来说，使用GDB都是一项关键技能。许多IDE会内置某种调试器或分析器，但对于ROS，可供选择的IDE很少。 因此，很重要的一点就是要了解如何使用这些可用的原始调试工具，而不是依赖IDE来提供调试工具。 而且，了解这些调试工具是C/C++开发的一项基本技能，如果您更改开发人员的这个角色且不再有能力使用调试工具，或者正在通过ssh会话用远程调试工具进行动态开发，则将调试工具留给IDE可能会出问题。 幸运的是，在掌握了基础知识之后，使用GDB就相当简单了。第一步要将-g添加到要分析/调试的ROS软件包的编译器标志中。这个标志会构建GDB和valgrind（Linux内存调试工具）可以读取的调试符号，以告诉您该项目中失败的具体代码行及失败原因。如果没有设置这个标志，虽然仍可以获取错误回溯信息，但不会提供失败的代码行号。完成调试后请务必删除这个标志，因为在运行时它会降低性能。 将下面这一行代码添加到您项目的CMakeLists.txt文件中应该就可以做到这一点。 如果您的项目中已经有add_compile_options()代码行，则可以简单地向该代码行中添加-g标志即可。 然后简单地用这个软件包运行colcon build --packages-select <package-name>命令来重建您的工作空间。 添加了-g标志后编译可能需要比平时更长的时间。 add_compile_options(-g) 现在您已准备就绪可以调试代码了！如果这是一个非ROS项目，这时可能需要执行下面这条命令。启动一个GDB会话并告诉程序立即运行。一旦程序崩溃，就会返回一个由(gdb)表示的gdb会话提示符。可以在这个提示符下获取您感兴趣的信息。 但是，由于这是一个包含大量节点配置和其他任务的ROS项目，因此对于初学者或那些不喜欢大量命令行工作和了解文件系统的人来说，这并不是一个很好的选择。 gdb ex run --args /path/to/exe/program 下面几节将会描述使用基于ROS 2的系统可能会遇到的3种主要情形。请阅读最符合您所遇问题情形的一节。 从节点进行回溯 就像在刚才那个非ROS项目示例中一样，需要在启动ROS 2节点之前设置GDB会话。 虽然可以在具有ROS 2文件系统的一些知识的情况下通过命令行进行GDB会话设置，但还可以使用Open Robotics的好心人提供的--prefix启动选项。 选项--prefix会在ros2命令之前执行一些代码，允许插入一些信息。 如果尝试执行类似于“前提条件”一节中的示例命令gdb ex run --args ros2 run ，则会发现它找不到ros2命令。 甚至如果您更聪明而尝试对工作空间进行source，还是会发现因类似原因而失败。 实际上，不必回头去查找可执行文件的安装路径并全部重新输入，而是使用--prefix选项。这允许使用您习惯的同一条ros2 run命令，而不必担心GDB的一些细节。具体命令语法如下： ros2 run --prefix 'gdb -ex run --args' <pkg> <node> --all-other-launch arguments 和前面一样，这个前缀会启动一个GDB会话并运行您请求的带有所有附加命令行参数的节点。 现在系统应该会让您的节点运行，并且应该会与一些调试打印信息一起运行。 服务器崩溃，就会看到如下提示符。此时，就可以获取错误回溯信息了。 (gdb) 在这个会话中，键入backtrace命令，这样它就会为您提供一个错误回溯信息。 根据您的需要复制这些错误回溯信息。 下面是一个错误回溯信息的示例： (gdb) backtrace #0 __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50 #1 0x00007ffff79cc859 in __GI_abort () at abort.c:79 #2 0x00007ffff7c52951 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6 #3 0x00007ffff7c5e47c in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6 #4 0x00007ffff7c5e4e7 in std::terminate() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6 #5 0x00007ffff7c5e799 in __cxa_throw () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6 #6 0x00007ffff7c553eb in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6 #7 0x000055555555936c in std::vector<int, std::allocator<int> >::_M_range_check ( this=0x5555555cfdb0, __n=100) at /usr/include/c++/9/bits/stl_vector.h:1070 #8 0x0000555555558e1d in std::vector<int, std::allocator<int> >::at (this=0x5555555cfdb0, __n=100) at /usr/include/c++/9/bits/stl_vector.h:1091 #9 0x000055555555828b in GDBTester::VectorCrash (this=0x5555555cfb40) at /home/steve/Documents/nav2_ws/src/gdb_test_pkg/src/gdb_test_node.cpp:44 #10 0x0000555555559cfc in main (argc=1, argv=0x7fffffffc108) at /home/steve/Documents/nav2_ws/src/gdb_test_pkg/src/main.cpp:25 在这个示例中，应该从底部开始按照以下方式阅读错误回溯： 在主（main）函数中的第25行，调用了一个函数VectorCrash。 在VectorCrash函数中的第44行，在Vector的输入为100的at()方法中崩溃。 在从范围检查失败中抛出异常后，在STL vector第1091行上的at()中崩溃。 需要一些时间来习惯对这些跟踪的阅读，但一般来说，从底部开始并沿着堆栈向上跟踪，直到看到它崩溃的那一行。 然后就可以推断出崩溃的原因。 使用GDB完成错误回溯后，请键入quit，这样就会退出该会话并终止所有仍在运行的进程。 最后系统可能会询问是否要关闭一些线程，回答“yes”即可。 从启动文件进行回溯 就像在刚才那个非ROS项目示例中一样，需要在启动 ROS 2启动文件之前设置一个GDB会话。 虽然可以通过命令行进行GDB会话设置，但还可以使用在ros2 run 节点示例中所采用的相同办法，只是现在使用的启动文件而不是可执行文件而已。 在启动文件中，找到想要进行调试的节点。本节中假设启动文件只包含一个节点（且可能还有其他信息）。 软件包launch_ros中使用的 Node函数可以接收一个字段prefix，该字段带有前缀参数列表。这里会在此处插入GDB代码段，但对节点示例进行了一项更改，即xterm的使用。xterm会弹出一个新的终端窗口来显示GDB并与之交互。 之所以要这样做，是因为处理启动文件上的stdin这一议题（例如，如果您按下CTRL+C组合键，您是在与GDB对话还是在与launch对话？）。 有关这一点的更多信息，请参阅这张工单。 请参阅下面这个调试SLAM工具箱的示例。 start_sync_slam_toolbox_node = Node( parameters=[ get_package_share_directory("slam_toolbox") + '/config/mapper_params_online_sync.yaml', {'use_sim_time': use_sim_time} ], package='slam_toolbox', executable='sync_slam_toolbox_node', name='slam_toolbox', prefix=['xterm -e gdb -ex run --args'], output='screen') 和前面一样，这个前缀会启动一个GDB会话，但现在是在xterm中，并且会运行您请求的带有所有附加启动参数定义的启动文件。 一旦服务器崩溃，就会在xterm会话中看到如下所示的提示符。此时，就可以获得错误回溯信息了。 (gdb) 在这个会话中，键入backtrace命令，这样它就会提供一个错误回溯信息。根据需要复制这些错误回溯信息。有关示例，请参阅上一节中的示例跟踪。 需要一些时间来习惯对这些跟踪的阅读，但一般来说，从底部开始并沿着堆栈向上跟踪，直到看到它崩溃的那一行。然后就可以推断出崩溃的原因。使用GDB完成错误回溯后，请键入quit，这样就会退出该会话并终止所有仍在运行的进程。 最后系统可能会询问是否要关闭一些线程，回答“yes”即可。 从Nav Bringup软件包进行回溯 使用具有多个节点的启动文件会有点不同，这样可以与GDB会话进行交互，而不会被同一终端中的其他日志记录所困扰。 因此，在从更大型启动文件进行回溯时，最好把需要回溯的具体服务器节点从中独立出来并单独启动它。 因此，对于这种情况，当看到想要调查的某个崩溃时，将这个服务器节点与其他节点分开是会有益处的。 如果想要回溯的服务器节点是从一个嵌套的启动文件（例如包含另一个启动文件的启动文件）中启动的，则可能需要执行以下操作： 注释掉启动文件中对父级启动文件进行包含的包含语句 使用用于调试符号的-g标志重新编译感兴趣的软件包 在终端中启动父级启动文件 按照本教程“从启动文件进行回溯”一节中的说明，在另一个终端中启动服务器节点的启动文件。 或者，如果感兴趣的服务器节点直接在这些文件（例如，您看到的Node、LifecycleNode 或位于ComponentContainer内部）中启动，则需要执行以下操作将该服务器节点与其他节点分开： 注释掉节点对父级启动文件进行包含的包含语句 使用用于调试符号的-g标志重新编译感兴趣的软件包 在一个终端中启动父级启动文件 按照本教程“从节点进行回溯”一节中的说明，在另一个终端中启动该服务器节点。 *注：请注意，在本例中，如果之前由启动文件提供重映射或参数文件，则可能需要对该节点进行重映射或向该节点提供参数文件。使用--ros-args选项可以为该节点指定新参数文件的路径、重映射或名称。有关所需命令行参数的更多信息，请参阅这个ROS 2教程。 我们知道这可能会很痛苦，所以鼓励您将每个可能的节点都作为一个单独包含的启动文件来简化调试。一组示例参数可能是--ros-args -r __node:=<node_name> --params-file /absolute/path/to/params.yaml （这可以作为参数模板）。 一旦服务器崩溃，就会在特定服务器的终端中看到如下所示的提示符。此时，就可以获取错误回溯信息了。 (gdb) 在这个会话中，键入backtrace命令，这样它就会提供一个错误回溯信息。根据需要复制这些错误回溯信息。 有关示例，请参阅上一节中的示例跟踪 需要一些时间来习惯对这些跟踪的阅读，但一般来说，从底部开始并沿着堆栈向上跟踪，直到看到它崩溃的那一行。 然后就可以推断出崩溃的原因。使用GDB完成错误回溯后，请键入quit，这样就会退出该会话并终止所有仍在运行的进程。 最后系统可能会询问是否要关闭一些线程，回答“yes”即可。 对系统崩溃（crash）进行自动回溯 库backward-cpp提供了漂亮的软件堆栈跟踪功能，而backward_ros封装器则简化了该库的集成使用。 只需将该封装器添加为软件包的依赖项并在软件包的CMakeLists.txt文件中对该库使用find_package语句来查找它，这样上述两个backward库就会被植入到软件包的所有可执行文件和库中。 参考： https://navigation.ros.org/tutorials/docs/get_backtrace.html