目录
Setting Up Odometry on your Robot
Simulating an Odometry System using Gazebo
Adding Gazebo Plugins to a URDF
Configuring Robot Localization
Simulating Sensors using Gazebo
Adding Gazebo Plugins to a URDF
Setting Up the Robot’s Footprint
First-Time Robot Setup Guide
这个指南会介绍如何将机器人的测距系统（odometry system）与 Nav2 集成。首先，我们会简要介绍测距系统（odometry）以及 Nav2 正确运行所需的必要消息和变换。接下来，我们将展示两种不同情况下如何设置测距系统。在第一种情况下，我们将展示如何为已经有轮式编码器的机器人设置测距系统。在第二种情况下，我们将演示如何使用 Gazebo 在 sam_bot（我们在前面一节中构建的机器人）上模拟一个功能正常的测距系统。之后，我们将讨论如何融合各种测距系统的信息，利用 robot_localization 软件包来提供平滑的测距系统。最后，我们还会展示如何使用 robot_localization 发布 odom => base_link 变换。
| 这个教程的完整源代码可以在 navigation2_tutorials 仓库的 sam_bot_description 包中找到。请注意，该仓库包含了完成本指南中所有教程后的完整代码。 |
Odometry Introduction
里程计系统根据机器人的运动提供了一个局部准确的姿态和速度估计。里程计信息可以来自多种来源，比如IMU、激光雷达、雷达、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 twistheader: 提供了时间戳和坐标框架的信息。child_frame_id: 姿态指向的坐标框架，扭转相对于此坐标框架。pose: 估计的姿态，通常是相对于固定的世界坐标框架。twist: 相对于child_frame_id的估计线性和角速度。
这条消息告诉我们机器人姿态和速度的估计值。头部消息在特定的坐标框架中提供了带时间戳的数据。姿态消息提供了机器人相对于头部中指定的坐标框架的位置和方向。扭转消息提供了相对于定义在子坐标框架中的线性和角速度。
Setting Up Odometry on your Robot
设置物理机器人的导航系统的里程计取决于机器人配备的哪些里程计传感器。由于机器人可能有各种配置，具体的设置说明将不在本教程的范围之内。相反，我们将提供一些基本示例和有用资源，以帮助您为 Nav2 配置机器人。
首先，我们将以一个使用轮式编码器作为里程计来源的机器人示例进行说明。需要注意的是，轮式编码器对于 Nav2 来说并不是必需的，但在大多数设置中很常见。设置里程计的目标是计算里程计信息，并通过 ROS 2 发布 nav_msgs/Odometry 消息和 odom => base_link 变换。为了计算这些信息，您需要设置一些代码，将轮式编码器的信息转换为里程计信息，类似下面的代码片段：
（
设置里程计的主要目的是通过传感器获取里程计信息，并将其转换到机器人的基本坐标系（例如，base_link）中。在这个过程中，会进行坐标系之间的变换，以便正确地将局部坐标系中的位置和姿态信息映射到机器人的基本坐标系中。这种转换非常关键，因为它提供了 Nav2 所需的信息，使得导航系统能够理解机器人在空间中的位置和姿态。
）
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 还提供了处理不同类型传感器的其他包。
（
ros2_control → diff_drive_controller → 接收 cmd_vel 话题发布的 geometry_msgs/Twist 消息（控制机器人运动的指令）→ 计算里程信息 → 以nav_msgs/Odometry消息的形式发布到odom话题上
geometry_msgs/Twist消息是ROS中常用的消息类型之一，用于表示物体的线速度和角速度。它包含两个部分：
- 线速度（Linear Velocity）：用三个分量表示物体在三个轴向上的线性运动速度，通常以米/秒为单位。
- 角速度（Angular Velocity）：也是用三个分量表示物体绕三个轴旋转的速度，通常以弧度/秒为单位。
这种消息类型通常用于控制机器人运动或描述物体的运动状态。例如，在差分驱动机器人中，geometry_msgs/Twist消息可用于发送控制指令，指定机器人在各个轴向上的期望速度，从而控制其移动。
）
| 有关更多信息，请参阅 ros2_control 文档 ros2_control documentation 和 diff_drive_controller 的 GitHub 存储库 Github repository of diff_drive_controller 。 |
对于其他类型的传感器，如 IMU、VIO 等，它们各自的 ROS 驱动应该有关于如何发布里程计信息的文档。请记住，Nav2 需要发布 nav_msgs/Odometry 消息和 odom => base_link 变换，这应该是您设置里程计系统时的目标。
Simulating an Odometry System using Gazebo
在本节，我们将使用 Gazebo 来模拟 sam_bot 的里程计系统，该机器人是我们在本教程系列的先前部分构建的。您可以先阅读该指南，或者从这里获取完整的源代码 complete source here。
| 如果您正在使用自己的物理机器人，并已设置了里程计传感器，您可以选择跳过本节，直接进入下一节，我们将在其中融合 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 消息。
Setup and Prerequisites
Gazebo 是一个三维仿真器，可以让我们观察虚拟机器人在模拟环境中的运行情况。要开始使用 Gazebo 和 ROS 2，请按照 Gazebo 安装文档 Gazebo Installation Documentation 中的安装说明进行操作。
我们还需要安装 gazebo_ros_pkgs 包，在 Gazebo 中使用 ROS 2 进行仿真里程计和控制机器人。
sudo apt install ros-<ros2-distro>-gazebo-ros-pkgs你可以按照此处给出的说明测试given here是否成功设置了 ROS 2 和 Gazebo 环境。
需要注意的是，我们描述了使用URDF。然而，Gazebo使用仿真描述格式（SDF） Simulation Description Format (SDF) 来描述机器人在其模拟环境中的情况。幸运的是，Gazebo会自动将兼容的URDF文件转换为SDF。使URDF与Gazebo兼容的主要要求是每个<link>元素内部都有一个<inertia>元素。对于的URDF文件已经满足了这个要求，因此它已经可以在Gazebo中使用了。
Adding Gazebo Plugins to a URDF
现在我们将向我们的URDF文件中添加Gazebo的IMU传感器和差分驱动插件。要了解Gazebo中可用的不同插件，请查看教程：《使用Gazebo插件与ROS》 Tutorial: Using Gazebo plugins with ROS。
对于我们的机器人，我们将使用GazeboRosImuSensor GazeboRosImuSensor ，这是一个SensorPlugin。SensorPlugin必须附加到一个link上，因此我们将创建一个<imu_link>，IMU传感器将附加到这个link上。这个link将在<link>元素下进行引用。接下来，我们将设置<topic>作为IMU将发布其信息的主题，并遵守REP145REP145，设置<frame>为base_link。我们还将使用Gazebo的传感器噪声模型 sensor noise model给传感器配置一些噪声。
现在，根据上述描述，我们将设置我们的IMU传感器插件，方法是在我们的URDF文件中在</robot>之前添加以下行：
<link name="imu_link">
<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"/>
</link>
<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。我们将配置这个插件，使其在/cmd_vel主题上发布消息。左右轮的关节将被设置为机器人模型中7<sam_bot>的轮子关节。轮子间距和轮子直径将根据已定义的<wheel_ygap>和<wheel_radius>值进行设置，分别是nav_msgs/Odometry/demo/odom下的sam_bot。
要在我们的URDF中包含这个插件，请在IMU插件的结束标签</gazebo>之后添加以下行：
<gazebo>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
<ros>
<namespace>/demo</namespace>
</ros>
<!-- wheels -->
<left_joint>drivewhl_l_joint</left_joint>
<right_joint>drivewhl_r_joint</right_joint>
<!-- kinematics -->
<wheel_separation>0.4</wheel_separation>
<wheel_diameter>0.2</wheel_diameter>
<!-- limits -->
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<!-- output -->
<publish_odom>true</publish_odom>
<publish_odom_tf>false</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 and Build Files
我们现在将编辑我们的启动文件，launch/display.launch.py，以在Gazebo中生成模拟。由于我们将模拟我们的机器人，我们可以通过删除以下内容来移除关节状态发布器的 GUI： 在sam_botgenerate_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'))
)移除以下的 GUI 参数：
DeclareLaunchArgument(name='gui', default_value='True',
description='Flag to enable joint_state_publisher_gui')移除 joint_state_publisher_node 的条件。
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')) # Remove this line
)接下来，打开 package.xml 并删除这行：
<exec_depend>joint_state_publisher_gui</exec_depend>为了启动 Gazebo，在 joint_state_publisher_node 之前添加以下内容。
launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'], output='screen'), 我们现在将添加一个节点来生成 Gazebo。再次打开 launch/display.launch.py 文件，在 .sam_bot 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'
)在下面显示的行之前添加以下行，在 spawn_entity 和 rviz_node 之间。
robot_state_publisher_node,
spawn_entity,
rviz_node
])Build, Run and Verification¶
让我们运行我们的包，检查一下系统中是否存在 /demo/imu 和 /demo/odom 这两个主题。
转到项目的根目录，并执行以下命令：
colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.pyGazebo应该会启动，你应该能够看到一个名为 sam_bot 的三维模型：
要查看系统中活跃的话题，请打开一个新的终端并执行：
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类型的消息。这些主题发布的信息来自于 gazebo 模拟的 IMU 传感器和差分驱动器。同时注意到这两个主题目前没有订阅者。在接下来的部分中，我们将创建一个 robot_localization 节点，该节点将订阅这两个主题。然后，它将使用这两个主题发布的消息为 Nav2 提供融合的、本地精确和平滑的里程计信息。
Robot Localization Demo
robot_localization包用于从N个里程计传感器输入的数据中提供融合的、本地精确且平滑的里程计信息。这些信息可以通过nav_msgs/Odometry、sensor_msgs/Imu、geometry_msgs/PoseWithCovarianceStamped和geometry_msgs/TwistWithCovarianceStamped消息提供给该包。
通常的机器人设置至少包括轮子编码器和IMU作为其里程计传感器来源。当向robot_localization提供多个来源的数据时，它能够通过状态估计节点融合传感器提供的里程计信息。这些节点利用扩展卡尔曼滤波器（ekf_node）或无迹卡尔曼滤波器（ukf_node）来实现这种融合。此外，该包还实现了一个navsat_transform_node，在使用GPS时将地理坐标转换为机器人的世界坐标系。
navsat_transform_node是robot_localization包中的一个节点，用于处理GPS数据并将其转换为机器人世界坐标系中的地理坐标。
如果在其配置中启用了，robot_localization包通过odometry/filtered和accel/filtered主题发布融合的传感器数据。此外，它还可以在/tf主题上发布odom => base_link的变换。
| 机器人定位的更多细节可以在官方机器人定位文档 Robot Localization Documentation 中找到。 |
如果你的机器人只能提供一个里程计来源，使用robot_localization除了平滑外，影响可能不大。在这种情况下，一种替代方法是在你的单个里程计来源节点中通过tf2广播器发布变换。然而，你仍然可以选择使用robot_localization来发布变换，输出中可能仍会观察到一些平滑特性。
| 更多关于如何编写tf2广播器的信息，你可以查看《编写tf2广播器（C++）（Python）》 (C++) (Python)。 |
在本节的其余部分，我们将展示如何使用robot_localization来融合sam_bot的传感器。它将使用在/demo/Imu上发布的sensor_msgs/Imu消息和在/demo/odom上发布的nav_msgs/Odometry消息，然后在odometry/filtered、accel/filtered和/tf主题上发布数据。
Configuring 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]在这个配置中，我们定义了频率、二维模式、是否发布加速度、是否发布tf、地图坐标系、里程计坐标系、基础坐标系和世界坐标系的参数值。有关其他可以修改的参数的更多信息，请参阅状态估计节点的参数，并且可以在这里找到一个示例efk.yaml。
要将传感器输入添加到ekf_filter_node中，请将下一个数字添加到其基本名称中（odom、imu、pose、twist）。在我们的例子中，我们有一个nav_msgs/Odometry和一个sensor_msgs/Imu作为过滤器的输入，因此我们使用odom0和imu0。我们将odom0的值设置为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时，它具有噪声特性。
odom0 使用 x、y、z 和 vyaw，imu0 仅使用 roll、pitch 和 yaw。
| 有关配置输入数据到 robot_localization 的更多建议，请参阅《准备您的数据以与 robot_localization 一起使用》 Preparing Your Data for Use with robot_localization 和《配置 robot_localization》 Configuring robot_localization 。 |
Launch and Build Files
现在，让我们将 ekf_node 添加到启动文件中。打开 launch/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_node`，以启动机器人定位节点。
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...) 中追加 config 目录，如下面的片段所示。
install(
DIRECTORY src launch rviz config
DESTINATION share/${PROJECT_NAME}
)Build, Run and Verification
现在让我们构建并运行我们的包。转到项目的根目录，并执行以下命令：
colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.pyGazebo和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（ekf_filter_node是一个用于消息融合的节点，它订阅了多个传感器发布的消息，然后使用扩展卡尔曼滤波器（Extended Kalman Filter，EKF）对这些消息进行融合。通过接收来自不同传感器的数据，它尝试将这些数据整合到一个更准确和连贯的状态估计中）是否是这些话题的订阅者，请执行：
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实用程序验证robot_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]Conclusion
在本指南中，我们讨论了Nav2对于里程计系统所期望的消息和变换。我们了解了如何设置里程计系统以及如何验证发布的消息。我们还讨论了如何使用robot_localization来利用多个里程计传感器提供经过滤波和平滑处理的里程计数据。同时，我们还检查了robot_localization是否正确发布了odom => base_link的变换。
在本指南中，我们将讨论传感器在安全导航机器人中的重要性，以及如何在Nav2中设置传感器。在本教程的前半部分，我们将简要介绍Nav2中常用的传感器和常见的传感器消息。接下来，我们将在之前构建的模拟机器人sam_bot上添加基本的传感器设置。最后，我们将通过在RViz中可视化它们来验证sam_bot的模拟传感器消息。
一旦传感器在机器人上设置好，它们的读数可以用于地图绘制、定位和感知任务。在本指南的后半部分，我们将首先讨论地图绘制和定位如何使用传感器数据。然后，我们还将介绍Nav2的一个包，nav2_costmap_2d，它生成的成本地图最终将用于Nav2的路径规划。我们将设置此包的基本配置参数，以便它正确接收来自sam_bot的传感器信息。最后，我们将在RViz中可视化生成的成本地图，以验证其接收的数据。
Sensor Introduction
移动机器人配备了多种传感器，使它们能够观察和感知周围环境。这些传感器获取信息，可用于构建和维护环境地图、将机器人定位于地图上以及识别环境中的障碍物。这些任务对于安全高效地将机器人导航穿越动态环境至关重要。
常用传感器的示例包括激光雷达、雷达、RGB摄像头、深度摄像头、惯性测量单元（IMU）和全球定位系统（GPS）。为了标准化这些传感器的消息格式，并且让不同供应商的设备更易于互操作，ROS提供了sensor_msgs包，定义了通用的传感器接口。这也使用户可以使用任何遵循sensor_msgs标准格式的传感器供应商的设备。在接下来的子部分中，我们将介绍一些导航中常用的消息，即sensor_msgs/LaserScan、sensor_msgs/PointCloud2、sensor_msgs/Range和sensor_msgs/Image。
除了sensor_msgs包，还有radar_msgs和vision_msgs的标准接口也值得关注。radar_msgs定义了用于雷达特定传感器的消息，而vision_msgs包定义了计算机视觉中使用的消息，例如目标检测、分割和其他机器学习模型。此包支持的消息包括vision_msgs/Classification2D、vision_msgs/Classification3D、vision_msgs/Detection2D和vision_msgs/Detection3D等。
| 要了解更多信息，请参阅sensor_msgssensor_msgs、radar_msgsradar_msgs和vision_msgs vision_msgs的API文档。 |
你的实际机器人的传感器可能已经有为其编写的ROS驱动程序（例如，连接到传感器、将数据填充到消息中并发布这些消息供机器人使用的ROS节点），这些驱动程序遵循sensor_msgs包中的标准接口。sensor_msgs包使得你可以轻松地使用来自不同制造商的许多不同传感器。通用软件包如Nav2可以读取这些标准化的消息并执行与传感器硬件无关的任务。在像sam_bot这样的模拟机器人中，Gazebo具有传感器插件，也会按照sensor_msgs包发布其信息。
Common Sensor Messages
在本小节中，我们将讨论在设置Nav2时可能遇到的一些常见sensor_msgs类型。我们将为每种传感器提供简要描述，展示其在Gazebo中的模拟图像以及在RViz中显示的传感器读数。
| 除了下面列出的类型之外，还有其他类型的sensor_msgs。您可以在sensor_msgs文档 sensor_msgs documentation 中找到完整的消息列表及其定义。 |
sensor_msgs/LaserScan
这条消息代表来自平面激光雷达的单次扫描。它在slam_toolbox和nav2_amcl中用于定位和建图，或在nav2_costmap_2d中用于感知。 
sensor_msgs/PointCloud2
这个消息包含一组三维点，以及关于每个点的可选附加信息。这可能来自三维激光雷达、二维激光雷达、深度摄像头或其他设备。
sensor_msgs/Range
这是来自主动传感器的单个范围读数，它发射能量并报告在测量距离处沿弧线有效的一个范围读数。声纳、红外传感器或一维距离传感器都是使用这个消息的传感器的示例。
sensor_msgs/Image
这代表来自RGB或深度相机的传感器读数，对应于RGB或深度数值。
Simulating Sensors using Gazebo
为了更好地理解如何在模拟机器人上设置传感器，我们将在之前的教程基础上继续，为我们的模拟机器人 sam_bot 添加传感器。与之前的教程类似，在 sam_bot 上添加里程计传感器时我们使用了 Gazebo 插件，现在我们将使用 Gazebo 插件来模拟在 sam_bot 上添加激光雷达和深度相机传感器。如果你正在处理真实机器人，大多数这些步骤仍然需要为设置 URDF 框架，同时也可以添加 Gazebo 插件以备后用。
为了能够继续进行本节的学习，请确保你已正确安装了 Gazebo。你可以按照前面教程中的《设置和先决条件》 Setup and Prerequisites 部分的说明来设置 Gazebo。
Adding Gazebo Plugins to a URDF
让我们首先给 sam_bot 添加一个激光雷达传感器。打开 URDF 文件，路径为 src/description/sam_bot_description.urdf，然后在 </robot> 标签之前粘贴以下行。
<link name="lidar_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.125"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0508" length="0.055"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.0508" length="0.055"/>
</geometry>
</visual>
</link>
<joint name="lidar_joint" type="fixed">
<parent link="base_link"/>
<child link="lidar_link"/>
<origin xyz="0 0 0.12" rpy="0 0 0"/>
</joint>
<gazebo reference="lidar_link">
<sensor name="lidar" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>5</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1.000000</resolution>
<min_angle>0.000000</min_angle>
<max_angle>6.280000</max_angle>
</horizontal>
</scan>
<range>
<min>0.120000</min>
<max>3.5</max>
<resolution>0.015000</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="scan" filename="libgazebo_ros_ray_sensor.so">
<ros>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>lidar_link</frame_name>
</plugin>
</sensor>
</gazebo>在上面的代码片段中，我们创建了一个名为 lidar_link 的链接，这将被 gazebo_ros_ray_sensor 插件（"gazebo_ros_ray_sensor" 是 Gazebo 仿真环境中的一个 ROS 插件，用于模拟光线传感器，例如激光雷达。该插件允许在 Gazebo 中模拟激光雷达传感器的行为，并将其与 ROS 中的话题进行连接，以便发布激光雷达传感器扫描数据。通过这个插件，你可以在仿真环境中模拟出激光雷达的功能，以便进行仿真测试和开发）引用为我们传感器的连接位置。我们还为模拟激光雷达的扫描和范围属性设置了值。最后，我们将 /scan 设置为它将发布 sensor_msgs/LaserScan 消息的主题。
接下来，让我们给 sam_bot 添加一个深度摄像头。在激光雷达传感器的 </gazebo> 标签之后，粘贴以下行。
<link name="camera_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.015 0.130 0.022"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.015 0.130 0.022"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.035"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
</link>
<joint name="camera_joint" type="fixed">
<parent link="base_link"/>
<child link="camera_link"/>
<origin xyz="0.215 0 0.05" rpy="0 0 0"/>
</joint>
<link name="camera_depth_frame"/>
<joint name="camera_depth_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
<parent link="camera_link"/>
<child link="camera_depth_frame"/>
</joint>
<gazebo reference="camera_link">
<sensor name="depth_camera" type="depth">
<visualize>true</visualize>
<update_rate>30.0</update_rate>
<camera name="camera">
<horizontal_fov>1.047198</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.05</near>
<far>3</far>
</clip>
</camera>
<plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">
<baseline>0.2</baseline>
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<frame_name>camera_depth_frame</frame_name>
<pointCloudCutoff>0.5</pointCloudCutoff>
<pointCloudCutoffMax>3.0</pointCloudCutoffMax>
<distortionK1>0</distortionK1>
<distortionK2>0</distortionK2>
<distortionK3>0</distortionK3>
<distortionT1>0</distortionT1>
<distortionT2>0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0</Cx>
<Cy>0</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
</gazebo> 与激光雷达传感器类似，我们创建了一个名为 camera_link 的链接，该链接将被 gazebo_ros_camera 插件引用作为传感器的连接位置。我们还创建了一个名为 camera_depth_frame 的链接，它连接到 camera_link 上，并将设置为深度相机插件的 <frame_name>。我们还配置了插件，使其会将 sensor_msgs/Image 和 sensor_msgs/PointCloud2 消息发布到 /depth_camera/image_raw 和 /depth_camera/points 话题上。最后，我们还设置了深度相机的其他基本配置属性。（准确地说，gazebo_ros_camera 是一个 Gazebo 插件，用于模拟相机设备，可以生成普通相机图像。而 camera_depth_frame 通常用于代表深度相机的内部结构或是深度相机测量数据的特定帧。这个名字可能被用作深度相机插件来识别和处理深度相关的信息。所以，camera_depth_frame 不一定将普通相机转变为深度相机，而是作为深度相机插件在仿真环境中的一个关键标识）
Launch and Build Files
要验证传感器设置正确并且能够在环境中检测到物体，让我们在包含物体的Gazebo世界中启动sam_bot。我们将创建一个Gazebo世界，其中包含一个立方体和一个球体，它们位于sam_bot传感器范围内，以便我们可以验证它是否能正确地检测到这些物体。
要创建这个世界，在项目的根目录下创建一个名为 world 的目录，并在其中创建一个名为 my_world.sdf 的文件。然后将 world/my_world.sdf world/my_world.sdf 的内容复制并粘贴到 my_world.sdf 中。
现在，让我们编辑我们的启动文件 launch/display.launch.py launch/display.launch.py，以便使用我们刚刚创建的世界启动Gazebo。首先，在 generate_launch_description() 中添加 my_world.sdf 的路径，添加以下行：（在启动文件中启动my_world.sdf）
world_path=os.path.join(pkg_share, 'world/my_world.sdf'),最后，在 `launch.actions.ExecuteProcess(cmd=['gazebo',...` 行中添加世界路径，如下所示。
launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world_path], output='screen'),把世界目录也添加到我们的 CMakeLists.txt 文件（CMakeLists.txt 文件是 CMake 构建系统的配置文件，用于指示如何构建和管理项目。它定义了项目的构建过程，包括编译源代码、链接库、生成可执行文件或库以及安装所需的文件和目录。通常，它包含了构建项目所需的各种指令和设置）中。打开 CMakeLists.txt 文件，在 `install(DIRECTORY…)` 的括号内追加世界目录，如下所示的代码片段。
install(
DIRECTORY src launch rviz config world
DESTINATION share/${PROJECT_NAME}
)Build, Run and Verification
我们现在可以构建并运行项目。前往项目根目录并执行以下命令：
colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.pyRViz和Gazebo将随后启动，sam_bot将同时出现在两个界面中。在Gazebo窗口中，我们创建的世界应该会被加载，sam_bot会出现在那个世界中。你现在应该能够观察到带有360度激光雷达传感器和深度相机的sam_bot，就像下面的图片展示的那样。
在RViz窗口中，我们可以验证我们是否正确建模了传感器，并且我们新添加的传感器的变换是否正确：
最后，我们也可以在RViz中可视化传感器的读数。要可视化在/scan主题上发布的sensor_msgs/LaserScan消息，请单击RViz窗口底部的“添加”按钮。然后转到“按主题”选项卡，在/scan下选择LaserScan选项，如下所示。
接下来，在RViz中将“可靠性策略”设置为“尽力而为”，将大小设置为0.1以便更清楚地看到点。您应该会看到如下所示的LaserScan检测可视化。这对应于我们在Gazebo世界中添加的检测到的立方体和球体。
要可视化 "sensor_msgs/Image" 和 "sensor_msgs/PointCloud2"，请分别在 RViz 中添加话题 /depth_camera/image_raw 和 /depth_camera/points。
在 RViz 中添加 /depth_camera/image_raw 话题后，请将 RViz 中的可靠性策略设置为最佳尝试。然后您应该在 RViz 窗口左下方的图像窗口中看到立方体，如下所示。
您还应该看到 sensor_msgs/PointCloud2，如下所示。
Mapping and Localization
我们现在有一个机器人及其传感器设置，可以使用获得的传感器信息构建环境地图，并对地图中的机器人进行定位。slam_toolbox包是ROS2中用于在潜在的大型地图中进行二维同时定位与建图（SLAM）的一组工具和功能。它还是Nav2中官方支持的SLAM库之一，在需要在机器人设置上使用SLAM的情况下建议使用该软件包。除了slam_toolbox，定位也可以通过nav2_amcl软件包来实现。该软件包实现了自适应蒙特卡洛定位（AMCL），用于估计机器人在地图中的位置和方向。还可能存在其他技术，请查阅Nav2文档以获取更多信息。
slam_toolbox和nav2_amcl都使用激光扫描传感器的信息来感知机器人的环境。因此，为了验证它们能够访问激光扫描传感器的读数，我们必须确保它们已经订阅了发布sensor_msgs/LaserScan消息的正确主题。这可以通过将它们的scan_topic参数设置为发布该消息的主题来配置。按照惯例，将sensor_msgs/LaserScan消息发布到/scan主题。因此，默认情况下，scan_topic参数设置为/scan。回想一下，在前面的部分中，我们添加了lidar传感器到sam_bot时，将传感器_msgs/LaserScan消息发布的主题设置为了/scan（传感器消息（sensor_msgs）是ROS中用于在机器人系统中传递传感器数据的消息类型。sensor_msgs/LaserScan是一种消息类型，表示来自激光雷达等激光传感器的扫描数据。而/scan是一个ROS话题（topic）名称，用于发布这些激光扫描数据消息。在ROS中，话题是消息传递的通道，它允许节点之间相互通信，发布者节点将消息发布到话题上，而订阅者节点则可以从这些话题中接收消息）。
深入讨论完整的配置参数不在我们教程的范围内，因为它们可能相当复杂。相反，我们建议您查看以下链接中的官方文档。
| slam_toolbox的完整配置参数列表，请参阅slam_toolbox的Github存储库 Github repository of slam_toolbox 。 nav2_amcl的完整配置参数列表和示例配置，请参阅AMCL配置指南 AMCL Configuration Guide 。 |
您也可以参考《SLAM 导航和建图》 (SLAM) Navigating While Mapping guide 指南，了解如何在 Nav2 中使用 SLAM。您可以通过在 RViz 中可视化地图和机器人的姿态来验证 slam_toolbox 和 nav2_amcl 是否已经正确设置，类似于前面展示的内容。
Costmap 2D
成本地图 2D 包利用传感器信息以占用网格的形式提供机器人环境的表示。占用网格中的单元格存储了 0 到 254 之间的成本值，表示通过这些区域的成本。成本为 0 意味着单元格是自由的，而成本为 254 意味着单元格是致命占用的。在这些极端值之间的值被导航算法用作潜在场地，以引导您的机器人远离障碍物。Nav2 中的成本地图是通过 nav2_costmap_2d 包实现的。
成本地图的实现包括多个层，每个层都有特定的功能，有助于确定单元格的总体成本。该包包括以下层，但是基于插件，允许自定义和使用新层：静态层、膨胀层、范围层、障碍层和体素层。静态层表示成本地图的地图部分，是通过发布到 /map 主题的消息获得的，类似于 SLAM 生成的消息。障碍层包括传感器检测到的对象，这些传感器发布 LaserScan 和 PointCloud2 消息中的任一或两者。体素层类似于障碍层，它可以使用 LaserScan 和 PointCloud2 传感器信息，但处理的是 3D 数据。范围层允许包括由声呐和红外传感器提供的信息。最后，膨胀层表示致命障碍物周围的附加成本值，使我们的机器人避免由于机器人的几何特性而导航到障碍物中。在本教程的下一部分中，我们将讨论 nav2_costmap_2d 中不同层的基本配置。
-
静态层（Static Layer）： 这个层用于代表环境的地图部分。它通过消息发布到
/map主题（通常由 SLAM 生成的消息）来获取环境的静态地图信息。 -
膨胀层（Inflation Layer）： 这一层会在障碍物周围添加额外的成本值。通过增加这些值，它创建了一个安全区域，确保机器人不会因为几何形状而误入障碍物。
-
范围层（Range Layer）： 这个层可以集成来自声呐和红外传感器的信息。它帮助识别周围的障碍物，从而影响机器人的导航路径。
-
障碍层（Obstacle Layer）： 这个层使用传感器（如激光雷达或深度摄像头）检测到的物体信息。它将这些信息转换成成本地图，以便机器人可以避开这些物体。
-
体素层（Voxel Layer）： 与障碍层类似，不同之处在于它处理的是 3D 数据。通常使用 3D 激光雷达或深度相机生成的数据。这对于处理多维度的传感器信息非常重要。
这些层通过插件接口集成到成本地图中，然后根据用户指定的膨胀半径进行膨胀，如果启用了膨胀层 inflation radius。要深入讨论成本地图概念，您可以查看 ROS1 的 costmap_2D 文档 ROS1 costmap_2D documentation。请注意，nav2_costmap_2d 包基本上是 ROS1 导航栈版本的 ROS2 移植，需要进行少量更改以支持 ROS2，并包含一些新的层插件。
Configuring nav2_costmap_2d
在这一小节中，我们将展示 `nav2_costmap_2d` 的一个示例配置，以便利用 sam_bot 的激光雷达传感器提供的信息。我们将展示一个示例配置，其中使用了静态层、障碍层、体素层和膨胀层。我们将障碍层和体素层设置为使用由激光雷达传感器发布到 `/scan` 主题的激光扫描消息。我们还设置了一些基本参数，以定义如何在成本地图中反映检测到的障碍物。请注意，这个配置是要包含在 Nav2 的配置文件中的。
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
robot_radius: 0.22
resolution: 0.05
track_unknown_space: false
rolling_window: false
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: True
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.22
plugins: ["voxel_layer", "inflation_layer"]
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: True在 global_costmap 部分，配置了以下参数：
update_frequency和publish_frequency：更新和发布频率。global_frame和robot_base_frame：定义全局框架和机器人基础框架。use_sim_time：指示是否使用模拟时间。robot_radius：机器人的半径。resolution：地图的分辨率。track_unknown_space和rolling_window：跟踪未知空间和使用滚动窗口的选项。plugins：定义所使用的层。static_layer、obstacle_layer和inflation_layer：每个层的详细设置，包括所使用的插件以及相关的参数。
对于 local_costmap 部分，同样设置了参数和层，但这是用于局部成本地图的。其中包括了体素层 (voxel_layer) 作为局部地图的一部分，并设置了体素地图的参数和所用的传感器。
observation_sources 参数指定了用于该层的传感器源，如 scan（激光雷达扫描）。每个传感器源（例如 scan）都有其自己的配置，包括主题 (topic)、数据类型 (data_type)、障碍物最大和最小范围等。
在上述配置中，请注意我们为两个不同的成本地图设置了参数：`global_costmap` 和 `local_costmap`。我们设置了两个成本地图，因为 `global_costmap` 主要用于对整个地图进行长期规划，而 `local_costmap` 用于短期规划和避碰。
我们在配置中使用的层都在 `plugins` 参数中定义，如第 13 行的 `global_costmap` 和第 50 行的 `local_costmap` 所示。这些值被设置为一系列映射的层名称，这些名称也作为我们设置的层参数的命名空间，从第 14 行和第 51 行开始。请注意，在此列表中的每个层/命名空间都必须具有一个 `plugin` 参数（如第 15、18、32、52 和 68 行所示），用于定义加载特定层的插件类型。
对于静态层（第 14-16 行），我们将 `map_subscribe_transient_local` 参数设置为 True。这将设置地图主题的 QoS 设置。静态层的另一个重要参数是 `map_topic`，它定义了要订阅的地图主题。如果未定义，默认为 `/map` 主题。
对于障碍物层（第 17-30 行），我们在 `observation_sources` 参数（第 20 行）下定义其传感器来源，如 `scan`，其参数设置在第 22-30 行。我们将其 `topic` 参数设置为发布定义的传感器来源的主题，并根据将使用的传感器来源设置 `data_type`。在我们的配置中，障碍物层将使用由激光雷达传感器发布到 `/scan` 的激光扫描。
请注意，障碍物层和体素层都可以使用激光雷达和点云作为其 `data_type`，但默认情况下设置为激光雷达。下面的代码片段显示了使用激光雷达和点云作为传感器来源的示例。当设置自己的物理机器人时，这可能特别有用。
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan pointcloud
scan:
topic: /scan
data_type: "LaserScan"
pointcloud:
topic: /depth_camera/points
data_type: "PointCloud2"对于障碍层（obstacle layer）的其他参数，`max_obstacle_height` 参数设置传感器读数返回给占用栅格的最大高度。最小传感器读数高度也可以使用 `min_obstacle_height` 参数进行设置，默认为0，因为我们没有在配置中设置它。`clearing` 参数用于设置是否从代价地图中移除障碍物（在代价地图中，清除障碍物意味着从地图中删除被传感器视为不再存在的障碍物。通常，这是通过光线追踪或其他传感器数据的处理来完成的。当传感器探测到一个障碍物，在一段时间后，如果传感器再次未检测到该障碍物，系统可能会将其视为不再存在，然后从地图中清除该障碍物标记）。清除操作是通过对网格进行光线追踪来完成的。可以使用 `raytrace_max_range` 和 `raytrace_min_range` 分别设置从代价地图中光线追踪清除对象的最大和最小范围。`marking` 参数用于设置是否将插入的障碍物标记到代价地图中。我们还通过 `obstacle_max_range` 和 `obstacle_min_range` 设置了标记障碍物在代价地图中的最大和最小范围。
对于膨胀层（inflation layer）（第31-34行和67-70行），我们使用 `cost_scaling_factor` 参数设置膨胀半径上的指数衰减因子。通过 `inflation_radius` 参数定义了围绕致命障碍物膨胀的半径值。
（
-
cost_scaling_factor：这个参数用于确定从致命障碍物（标记为最大代价）向外扩散的代价值如何衰减。较小的因子将导致膨胀区域的代价缓慢减少，而较大的因子将加快这一过程。这个衰减过程是指从致命障碍物边缘开始，代价逐渐减小，直到达到膨胀半径。 -
inflation_radius：这个参数定义了从致命障碍物周围向外膨胀的半径范围。膨胀后的区域会被赋予代价值，机器人规划路径时会考虑这些代价值。如果机器人距离致命障碍物小于膨胀半径，它会受到膨胀区域的影响，并尝试避开这些区域。这有助于确保机器人在路径规划时考虑到其周围环境的安全性，尤其是在靠近障碍物时采取预防措施。
）
对于体素层（voxel layer）（第51-66行），我们将 `publish_voxel_map` 参数设置为 True，以启用三维体素网格的发布。使用 `z_resolution` 参数定义体素在高度方向的分辨率，`z_voxels` 参数定义每个列中的体素数量。`mark_threshold` 参数设置要标记为占用的最小列体素数目。我们将体素层的 `observation_sources` 参数设置为 `scan`，并设置了扫描参数（第61-66行），与我们讨论的障碍层的参数类似。根据其 `topic` 和 `data_type` 参数的定义，体素层将使用由激光雷达扫描器在 `/scan` 主题上发布的激光扫描数据。
请注意，我们没有在配置中使用范围层（range layer），但它可能对您自己的机器人设置很有用。对于范围层，其基本参数包括 `topics`、`input_sensor_type` 和 `clear_on_max_reading` 参数。`topics` 参数定义要订阅的范围主题。`input_sensor_type` 设置为 `ALL`、`VARIABLE` 或 `FIXED`。`clear_on_max_reading` 是一个布尔参数，设置是否在最大范围时清除传感器读数。如果需要设置它，请参考下面链接中的配置指南。
| 有关 nav2_costmap_2d 的更多信息以及图层插件参数的完整列表，请参阅 Costmap 2D 配置指南 Costmap 2D Configuration Guide 。 |
Build, Run and Verification
我们首先会启动 display.launch.py，它会启动机器人状态发布者，提供 URDF 中 base_link => sensors 的变换。同时，它还会启动 Gazebo，作为我们的物理模拟器，并且会提供 odom => base_link 的变换，这是我们在上一篇指南“使用 Gazebo 模拟里程计系统”中添加到 sam_bot 的差分驱动插件的输出。它还会启动 RViz，我们可以用它来可视化机器人和传感器信息。
接着，我们会启动 slam_toolbox，它会发布到 /map 主题，并提供 map => odom 的变换。回想一下，map => odom 变换是 Nav2 系统的主要要求之一。在 /map 主题上发布的消息将由 global_costmap 的静态层使用。
在我们正确设置了机器人描述、里程计传感器和必要的变换之后，我们最终会启动 Nav2 系统本身。暂时，我们将只探索 Nav2 的成本地图生成系统。启动 Nav2 后，我们将在 RViz 中可视化成本地图，以确认我们的输出。
（
-
启动 display.launch.py
- 启动机器人状态发布者，提供 URDF 中 base_link => sensors 的变换，也就是各个坐标系之间的变换。
- 启动 Gazebo 作为物理模拟器，提供 odom => base_link 的变换，将里程计提供的机器人位置和方向信息转换为机器人坐标系中的位置和方向信息。这个转换非常重要，因为它允许机器人理解自己相对于一个固定参考点的位置和方向。
- 启动 RViz，用于机器人和传感器信息的可视化。
-
启动 slam_toolbox
- 发布到 /map 主题，提供 map => odom 的变换。这是 Nav2 系统所需的重要变换之一。
- /map 主题上发布的消息将由 global_costmap 的静态层使用。
-
验证输出
- 确认机器人描述、里程计传感器和必要的变换设置正确无误。
- 启动 Nav2 系统，并探索其成本地图生成系统。
- 在 RViz 中可视化成本地图，验证输出是否符合预期。
）
Launching Description Nodes, RViz and Gazebo
让我们通过 launch 文件 display.launch.py 启动我们的机器人描述节点、RViz 和 Gazebo。在新的终端中执行以下命令。
colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.py现在 RViz 和 Gazebo 应该已经启动，并且 sam_bot 应该同时出现在两个环境中。回想一下，base_link => sensors 的变换现在由 robot_state_publisher 发布，而 odom => base_link 变换由我们的 Gazebo 插件发布。这两个变换现在应该在 RViz 中显示出来，而且没有错误。
Launching slam_toolbox
要启动 slam_toolbox，请确保已安装 slam_toolbox 包，执行以下命令：
sudo apt install ros-<ros2-distro>-slam-toolbox我们将使用 slam_toolbox 包内置的 launch 文件启动 slam_toolbox 的 async_slam_toolbox_node。打开一个新的终端，然后执行以下命令：
ros2 launch slam_toolbox online_async_launch.pyslam_toolbox 现在应该正在发布到 /map 主题并提供 map => odom 的变换。
我们可以在 RViz 中验证 /map 主题是否 正在发布。在 RViz 窗口中，点击左下角的添加按钮，然后转到“By topic”选项卡，选择 /map 主题下的 Map。你应该能够可视化接收到的 /map 主题中的消息，如下图所示。
我们也可以通过在新的终端执行以下命令来检查变换是否正确：
ros2 run tf2_tools view_frames.py注意：对于 Galactic 和更新的版本，应该使用 `view_frames` 而不是 `view_frames.py`。上述命令将创建一个 `frames.pdf` 文件，显示当前的变换树。你的变换树应该类似于下面所示的内容：
Launching Nav2
首先，确保你已经安装了 Nav2 包，执行以下命令：
sudo apt install ros-<ros2-distro>-navigation2
sudo apt install ros-<ros2-distro>-nav2-bringup我们现在将使用 nav2_bringup 包内置的 launch 文件 navigation_launch.py 来启动 Nav2。打开一个新的终端并执行以下命令：
ros2 launch nav2_bringup navigation_launch.py请注意，我们在前面子节中讨论的 nav2_costmap_2d 的参数包含在 navigation_launch.py 的默认参数中。除了 nav2_costmap_2d 的参数外，它还包含了 Nav2 实现中其他节点的参数。
在正确设置和启动 Nav2 后，/global_costmap 和 /local_costmap 主题现在应该是活动的。
| 为了显示成本地图，请按以下顺序运行这三个命令： 1. 启动描述节点、RViz 和 Gazebo - 在日志中等待“Connected to gazebo master”消息。 |
Visualizing Costmaps in RViz
全局成本地图、局部成本地图和检测到的障碍物的体素表示可以在 RViz 中进行可视化。
要在 RViz 中可视化全局成本地图global_costmap，点击 RViz 窗口左下角的“添加”按钮。转到“按主题”By topic选项卡，然后选择“/global_costmap/costmap”主题下的“Map”。全局成本地图应该会显示在 RViz 窗口中，如下所示。全局成本地图显示了在 Gazebo 模拟世界中应该避开的区域（黑色），机器人在导航时应该避开这些区域。
要在 RViz 中可视化局部成本地图，请在“/local_costmap/costmap”主题下选择“Map”。在 RViz 中将颜色方案设置为 costmap，使其看起来类似于下面的图像。
要可视化检测到物体的体素表示，请打开一个新的终端并执行以下命令：
ros2 run nav2_costmap_2d nav2_costmap_2d_markers voxel_grid:=/local_costmap/voxel_grid visualization_marker:=/my_marker上面的命令设置了标记将发布到的主题为 /my_marker。要在 RViz 中查看标记，请选择 /my_marker 主题下的 Marker，如下所示。
（这条命令的作用是使用 `nav2_costmap_2d_markers` 节点，将 `/local_costmap/voxel_grid` 中的三维体素数据可视化为标记，并发布到 `/my_marker` 主题下供 RViz 或其他可视化工具使用。）
然后将 RViz 中的固定框架设置为 odom，然后你应该可以在 RViz 中看到体素，它们代表我们在 Gazebo 世界中放置的立方体和球体。
Conclusion
在我们的机器人设置指南的这一部分中，我们讨论了传感器信息在与 Nav2 相关的不同任务中的重要性。具体来说，涉及到制图（SLAM）、定位（AMCL）和感知（costmap）等任务。
我们还讨论了 Nav2 中常见的传感器消息类型，这些消息格式标准化了不同传感器供应商的消息格式。我们还讨论了如何使用 Gazebo 向模拟机器人添加传感器以及如何通过 RViz 验证传感器是否正常工作。
最后，我们使用不同的层设置了 nav2_costmap_2d 包的基本配置，以生成全局和局部成本地图。然后，通过在 RViz 中可视化这些成本地图来验证我们的工作。
在下一篇文章
在下一篇文章
7174
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
