ROS2与Navigation2入门教程-设置里程计（Odometry）

说明：

本教程将会研究如何将机器人的里程计系统与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传感器插件：

<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>
  <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.025</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>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

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号