设置里程计

在本指南中，我们将研究如何将机器人的里程计系统与 Nav2 集成。首先，我们将简要介绍里程计，以及 Nav2 正常运行所需发布的必要消息和转换。接下来，我们将展示如何通过两种不同情况设置里程计。在第一种情况下，我们将展示如何为已经配备车轮编码器的机器人设置里程计系统。在第二种情况下，我们将使用 Gazebo 构建一个演示，模拟 sam_bot （我们在上一节中构建的机器人）上正常运行的里程计系统。之后，我们将讨论如何使用 robot_localization 包融合各种里程计源以提供平滑的里程计。最后，我们还将展示如何使用 robot_localization 发布 odom => base_link 转换。

See also

本教程中的完整源代码可在 sam_bot_description 包下的 navigation2_tutorials 存储库中找到。请注意，完成本指南中的所有教程后，存储库将包含完整代码。

里程表简介

里程表系统根据机器人的运动提供机器人姿势和速度的局部精确估计。里程表信息可以从各种来源获得，例如 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 消息包含以下信息：

# 这表示自由空间中的位置和速度估计值。
# 此消息中的姿势应在 header.frame_id 给出的坐标系中指定
# 此消息中的扭曲应在 child_frame_id 给出的坐标系中指定

# 包括姿势父级的帧 ID。
std_msgs/Header header

# 姿势指向的帧 ID。扭曲在此坐标系中。
string child_frame_id

# 通常相对于固定世界框架的估计姿势。
geometry_msgs/PoseWithCovariance pose

# 相对于 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 要求。有关如何执行此操作的基本示例，请参阅此处的里程表导航文档 located here

我们推荐的手动发布此信息的替代方法是通过 ros2_control 框架。 ros2_control 框架包含用于 ROS 2 中机器人实时控制的各种包。对于车轮编码器， ros2_control 在 ros2_control 包下有一个 diff_drive_controller``（差分驱动控制器）。 ``diff_drive_controller 接收在 cmd_vel 主题上发布的 geometry_msgs/Twist 消息，计算里程计信息，并在 odom 主题上发布 nav_msgs/Odometry 消息。 ros2_controller 中还提供处理不同类型传感器的其他软件包。

See also

有关更多信息，请参阅 ros2_control 文档 和 diff_drive_controller 的 Github 存储库。

对于其他类型的传感器（如 IMU、VIO 等），它们各自的 ROS 驱动程序应该有关于如何发布里程计信息的文档。请记住，Nav2 需要发布 nav_msgs/Odometry 消息和 odom => base_link 转换，这应该是您设置里程计系统时的目标。

使用 Gazebo 模拟里程计系统

在本节中，我们将使用 Gazebo 模拟 sam_bot 的里程计系统，这是我们在本教程系列的上一节中构建的机器人。您可以先阅读该指南，也可以在此处获取 complete source here. 。

Note

如果您正在开发自己的物理机器人并且已经设置了里程计传感器，您可以选择跳过本节并进入下一节，我们将融合 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 模拟器，可让我们观察虚拟机器人在模拟环境中的运行方式。要开始将 Gazebo 与 ROS 2 结合使用，请按照 `Gazebo Installation Documentation <http://gazebosim.org/tutorials?cat=install>`_中的安装说明进行操作。

我们还需要安装 gazebo_ros_pkgs 包来模拟里程计并使用 Gazebo 中的 ROS 2 控制机器人：

sudo apt install ros-<ros2-distro>-gazebo-ros-pkgs

您可以按照 given here 中的说明测试是否已成功设置 ROS 2 和 Gazebo 环境。

请注意，我们使用 URDF 描述了 sam_bot。但是，Gazebo 使用 Simulation Description Format (SDF) 来描述其模拟环境中的机器人。幸运的是，Gazebo 会自动将兼容的 URDF 文件转换为 SDF。URDF 与 Gazebo 兼容的主要要求是每个 <link> 元素中都有一个 <inertia> 元素。 sam_bot 的 URDF 文件中已经满足此要求，因此它已经可以在 Gazebo 中使用。

See also

For more information on how to use URDF in Gazebo, see Tutorial: Using a URDF in Gazebo. 有关如何在 Gazebo 中使用 URDF 的更多信息，请参阅 Tutorial: Using a URDF in Gazebo 。

将 Gazebo 插件添加到 URDF

我们现在将 IMU 传感器和 Gazebo 的差速驱动插件添加到我们的 URDF。有关 Gazebo 中可用的不同插件的概述，请查看 Tutorial: Using Gazebo plugins with ROS 。

对于我们的机器人，我们将使用 GazeboRosImuSensor，它是一个 SensorPlugin。SensorPlugin 必须附加到链接，因此我们将创建一个 imu_link，IMU 传感器将附加到该链接。此链接将在 <gazebo> 元素下引用。接下来，我们将 /demo/imu 设置为 IMU 将发布其信息的主题，并且我们将通过将 initalOrientationAsReference 设置为 false 来遵守 REP145。我们还将使用 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。我们将配置插件，以便在 /demo/odom 主题上发布 nav_msgs/Odometry 消息。左轮和右轮的关节将设置为 sam_bot 的车轮关节。车轮间距和车轮直径分别根据 wheel_ygap 和 wheel_radius 的定义值设置。

要将此插件包含在我们的 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/display.launch.py, 以在 Gazebo 中生成 sam_bot 。由于我们将模拟我们的机器人，我们可以通过删除 generate_launch_description() 中的以下几行来删除联合状态发布者的 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'))
)

删除以下 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',
  arguments=[default_model_path], #Add this line
  parameters=[{'robot_description': Command(['xarcro ', default_model_path])}], #Remove this line
  condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui')) # Remove this line
)

接下来，打开 package.xml 并删除此行：

<exec_depend>joint_state_publisher_gui</exec_depend>

要启动 Gazebo，请在 display.launch.py 中的 joint_state_publisher_node, 行之前添加以下内容

launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', '-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 提供融合的、局部精确且平滑的里程计信息。

机器人定位演示

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 时将地理坐标转换为机器人的世界坐标系。

如果在配置中启用了 robot_localization 包，则融合的传感器数据由 odometry/filtered 和 accel/filtered 主题发布。此外，它还可以在“/tf”主题上发布 odom => base_link 转换。

See also

More details on robot_localization can be found in the official Robot Localization Documentation. 有关“robot_localization”的更多详细信息，请参阅官方“机器人定位文档<http://docs.ros.org/en/noetic/api/robot_localization/html/index.html>”。

如果您的机器人只能提供一个里程计源，那么使用 robot_localization 除了平滑之外，效果会非常小。在这种情况下，另一种方法是通过单个里程计源节点中的 tf2 广播器发布变换。尽管如此，您仍然可以选择使用 robot_localization 来发布变换，并且仍然可能在输出中观察到一些平滑属性。

See also

有关如何编写 tf2 广播器的更多信息，您可以查看编写 tf2 广播器 (C++) (Python)。

在本节的其余部分，我们将展示如何使用 robot_localization 融合 sam_bot``的传感器。它将使用在 ``/demo/Imu 上发布的 sensor_msgs/Imu 消息和在 /demo/odom 上发布的 nav_msgs/Odometry 消息，然后它将在 odometry/filtered 、 accel/filtered 和 ``/tf``主题上发布数据。

配置机器人定位

现在让我们配置 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 的参数值。有关您可以修改的其他参数的更多信息，请参阅 Parameters of state estimation nodes，示例 efk.yaml 可在此处 https://github.com/cra-ros-pkg/robot_localization/blob/foxy-devel/params/ekf.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 。

You can specify which values from a sensor are to be used by the filter using the _config parameter. The order of the values of this parameter is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. In our example, we set everything in odom0_config to false except the 1st, 2nd, 3rd, and 12th entries, which means the filter will only use the x, y, z, and the vyaw values of odom0. 您可以使用“_config”参数指定过滤器要使用传感器的哪些值。此参数值的顺序为 x、y、z、roll、pitch、yaw、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。在我们的示例中，我们将 odom0_config 中的所有内容（第 1、2、3 和 12 个条目除外）设置为 false，这意味着过滤器将仅使用 odom0 的 x、y、z 和 vyaw 值。

在 imu0_config 矩阵中，您会注意到只使用了 roll、pitch 和 yaw。典型的移动机器人级 IMU 还将提供角速度和线性加速度。为了使 robot_localization 正常工作，您不应融合彼此衍生的多个字段。由于角速度在 IMU 内部融合以提供滚动、俯仰和偏航估计值，因此我们不应融合用于得出该信息的角速度。我们也不会融合角速度，因为当不使用质量极高（且昂贵）的 IMU 时，角速度具有噪声特性。

See also

For more advise on configuration of input data to robot_localization, see Preparing Your Data for Use with robot_localization, and Configuring robot_localization. 有关 robot_localization 输入数据配置的更多建议，请参阅 Preparing Your Data for Use with robot_localization 和 Configuring robot_localization。

启动和构建文件

现在，让我们将 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 并将 config 目录附加到 ``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/imu 。我们还可以看到 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]

结论

在本指南中，我们讨论了 Nav2 期望从里程计系统发出的消息和转换。我们已经了解了如何设置里程计系统以及如何验证已发布的消息。我们还讨论了如何使用多个里程计传感器来使用 robot_localization 提供过滤和平滑的里程计。我们还检查了 robot_localization 是否正确发布了 odom => base_link 转换。