Gazebo 里程计设置
本指南介绍如何将机器人的里程计系统与 Nav2 集成。首先简要介绍里程计的基本概念，以及 Nav2 正常运行所需发布的消息和变换。接下来分两种场景演示里程计的设置方式：第一种是为已配备车轮编码器的实体机器人设置里程计；第二种是使用 Gazebo 仿真，在上一节构建的 sam_bot 上模拟一套可正常工作的里程计系统。
注意：本教程的完整源代码可在 navigation2_tutorials 仓库的
sam_bot_description包中找到。该仓库包含的是完成全部教程后的最终代码。
里程计系统根据机器人的运动，对位姿和速度进行局部估计。里程计数据可来自多种传感器，如 IMU、激光雷达、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 所定义坐标系的线速度和角速度。
在机器人上设置里程计
Section titled “在机器人上设置里程计”为实体机器人设置 Nav2 的里程计系统，方式取决于机器人所配备的传感器。由于机器人配置多种多样，本教程不提供具体的设置步骤，而是给出一些基本示例和参考资源，帮助你完成配置。
下面以使用车轮编码器作为里程计数据源的机器人为例。车轮编码器并非 Nav2 的必需条件，但在大多数配置中很常见。设置里程计的目标是计算里程计信息，并通过 ROS 2 发布 nav_msgs/Odometry 消息和 odom => base_link 变换。为此，需要编写代码将车轮编码器数据转换为里程计信息，核心计算类似于以下代码片段：
linear = (right_wheel_est_vel + left_wheel_est_vel) / 2angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation;其中 right_wheel_est_vel 和 left_wheel_est_vel 分别是右轮和左轮的估计速度，wheel_separation 是两轮之间的轮距。左右轮的估计速度可通过轮关节位置随时间的变化来计算。获得这些数据后，即可发布 Nav2 所需的消息和变换。基本示例可参考 Navigation 文档中的相关教程。
除了手动发布这些信息外，推荐使用 ros2_control 框架。该框架包含用于 ROS 2 机器人实时控制的多种包。针对车轮编码器，ros2_control 在 ros2_controllers 包下提供了 diff_drive_controller（差速驱动控制器）。diff_drive_controller 订阅 cmd_vel 话题上的 geometry_msgs/Twist 消息，据此计算里程计信息，并在 odom 话题上发布 nav_msgs/Odometry 消息。ros2_control 中也有处理其他类型传感器的包。
注意：更多信息请参阅 ros2_control 文档 和 diff_drive_controller 的 GitHub 仓库。
对于其他类型的传感器（如 IMU、VIO 等），各自的 ROS 驱动通常会提供发布里程计信息的说明。总之，Nav2 要求发布 nav_msgs/Odometry 消息和 odom => base_link 变换，这是设置里程计系统时的核心目标。
使用 Gazebo 模拟里程计系统
Section titled “使用 Gazebo 模拟里程计系统”本节使用 Gazebo 模拟 sam_bot（上一节教程中构建的机器人）的里程计系统。你可以阅读本指南，也可以直接获取完整源代码。
注意：如果使用的是自己的实体机器人，且里程计传感器已配置完毕，可以跳过本节，直接进入下一节——在那里会融合 IMU 和里程计消息，提供平滑的
odom=>base_link变换。
本节概述：首先设置 Gazebo 及使其与 ROS 2 协同工作所需的包。然后添加 Gazebo 插件，分别模拟 IMU 传感器和差速驱动里程计系统，发布 sensor_msgs/Imu 和 nav_msgs/Odometry 消息。最后在 Gazebo 环境中生成（spawn）sam_bot，并通过 ROS 2 验证发布的消息。
设置与前置条件
Section titled “设置与前置条件”Gazebo 是一款 3D 仿真器，可用于观察虚拟机器人在仿真环境中的表现。要在 ROS 2 中使用 Gazebo，请按照 Gazebo 安装文档 中的说明进行安装。
首先，加载 ROS 2 环境变量：
source /opt/ros/<ros2-distro>/setup.bash然后安装 ROS-Gazebo 集成包：
sudo apt install ros-$ROS_DISTRO-ros-gz向 URDF/SDF 添加 Gazebo 插件
Section titled “向 URDF/SDF 添加 Gazebo 插件”接下来将 Gazebo 的 IMU 传感器和差速驱动插件添加到 URDF/SDF 中。Gazebo 支持的传感器类型可参考传感器文档。
传感器必须附着在连杆上，因此需要创建一个 imu_link 供 IMU 传感器挂载。如果使用 URDF，该连杆将在 <gazebo> 元素中引用。然后将 /demo/imu 设为 IMU 消息的发布话题，并将 initialOrientationAsReference 设为 false，以符合 REP 145 规范。同时，参考 Gazebo 的传感器噪声模型为传感器配置添加噪声。
按以下步骤设置 IMU 传感器。
在机器人 SDF 的 </model> 行之前添加以下内容：
<joint name='imu_joint' type='fixed'> <parent>base_link</parent> <child>imu_link</child> <pose relative_to="base_link">0.0 0.0 0.01 0 0 0</pose></joint>
<link name='imu_link'> <pose relative_to="imu_joint"/> <visual name="imu_link_visual"> <geometry> <box><size> 0.1 0.1 0.1 </size></box> </geometry> </visual>
<collision name="imu_link_collision"> <geometry> <box><size> 0.1 0.1 0.1 </size></box> </geometry> </collision>
<xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>
<sensor name="imu_sensor" type="imu"> <always_on>true</always_on> <update_rate>100</update_rate> <visualize>true</visualize> <topic>demo/imu</topic> <gz_frame_id>imu_link</gz_frame_id> <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></link>接下来添加 DiffDrive 插件和 JointStatePublisher 插件。配置这些插件，使 nav_msgs/Odometry 消息发布到 /demo/odom 话题，tf2_msgs/msg/TFMessage 消息发布到 /tf 话题，两轮的 sensor_msgs/msg/JointState 消息发布到 /joint_states。左右轮关节设为 sam_bot 的轮关节，轮距和轮半径分别使用已定义的 wheel_ygap 和 wheel_radius 值。
在 SDF 中 IMU 传感器的 </link> 标签之后添加以下内容：
<plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive"> <!-- wheels --> <left_joint>drivewhl_l_joint</left_joint> <right_joint>drivewhl_r_joint</right_joint>
<!-- kinematics --> <wheel_separation>0.4</wheel_separation> <wheel_radius>${wheel_radius}</wheel_radius>
<!-- limits --> <max_linear_acceleration>0.1</max_linear_acceleration>
<!-- input --> <topic>/demo/cmd_vel</topic>
<!-- output --> <odom_topic>/demo/odom</odom_topic> <tf_topic>/tf</tf_topic>
<frame_id>odom</frame_id> <child_frame_id>base_link</child_frame_id></plugin>
<plugin filename="gz-sim-joint-state-publisher-system" name="gz::sim::systems::JointStatePublisher"> <topic>joint_states</topic></plugin>此外，按如下方式修改 SDF 中的万向轮（caster）连杆，将其摩擦系数设为接近零，以简化仿真。
<link name="front_caster"> <pose relative_to="caster_joint"/>
<visual name="front_caster_visual"> <geometry> <sphere> <radius>${(wheel_radius+wheel_zoff-(base_height/2))}</radius> </sphere> </geometry> <material> <ambient>0 1 1 1</ambient> <diffuse>0 1 1 1</diffuse> </material> </visual>
<collision name="front_caster_collision"> <geometry> <sphere> <radius>${(wheel_radius+wheel_zoff-(base_height/2))}</radius> </sphere> </geometry> <surface><friction><ode> <mu>0.001</mu> <mu2>0.001</mu2> </ode></friction></surface> </collision>
<xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/></link>创建 ROS<->Gazebo 桥接
Section titled “创建 ROS<->Gazebo 桥接”ros_gz_bridge 包提供了一个桥接（bridge），可在 Gazebo 话题和 ROS 话题之间进行双向转换。启动桥接时需要指定一个配置文件，声明要桥接哪些话题。
在包的 config 目录中创建 bridge_config.yaml 文件，添加以下内容，定义需要在 ROS 和 Gazebo 之间桥接的话题：
---- ros_topic_name: "/clock" gz_topic_name: "/clock" ros_type_name: "rosgraph_msgs/msg/Clock" gz_type_name: "gz.msgs.Clock" direction: GZ_TO_ROS
- ros_topic_name: "/demo/imu" gz_topic_name: "/demo/imu" ros_type_name: "sensor_msgs/msg/Imu" gz_type_name: "gz.msgs.IMU" direction: GZ_TO_ROS
# Topic published by DiffDrive plugin- ros_topic_name: "/demo/odom" gz_topic_name: "/demo/odom" ros_type_name: "nav_msgs/msg/Odometry" gz_type_name: "gz.msgs.Odometry" direction: GZ_TO_ROS
# Topic published by JointStatePublisher plugin- ros_topic_name: "/joint_states" gz_topic_name: "/joint_states" ros_type_name: "sensor_msgs/msg/JointState" gz_type_name: "gz.msgs.Model" direction: GZ_TO_ROS
# Topic subscribed to by DiffDrive plugin- ros_topic_name: "/demo/cmd_vel" gz_topic_name: "/demo/cmd_vel" ros_type_name: "geometry_msgs/msg/TwistStamped" gz_type_name: "gz.msgs.Twist" direction: ROS_TO_GZ
- ros_topic_name: "/tf" gz_topic_name: "/tf" ros_type_name: "tf2_msgs/msg/TFMessage" gz_type_name: "gz.msgs.Pose_V" direction: GZ_TO_ROS最后，将以下变量添加到启动文件中，下一节将用它指定桥接配置文件的路径：
bridge_config_path = os.path.join(pkg_share, 'config', 'bridge_config.yaml')制作 Gazebo 世界
Section titled “制作 Gazebo 世界”在包中创建 world 目录，将这段代码保存为 my_world.sdf 文件：
还需要将 world 目录添加到 CMakeLists.txt 中。打开 CMakeLists.txt，在 install(DIRECTORY...) 中添加 world 目录，如下所示：
install( DIRECTORY src launch rviz config world DESTINATION share/${PROJECT_NAME})最后，将以下变量添加到启动文件中，下一节将用它指定 Gazebo 世界文件的路径：
world_path = os.path.join(pkg_share, 'world', 'my_world.sdf')启动与构建文件
Section titled “启动与构建文件”接下来修改启动文件，使 sam_bot 能在 Gazebo 中生成。由于 JointStatePublisher 插件现在会发布 joint_states，可以从 generate_launch_description() 中删除以下与关节状态发布器相关的代码：
joint_state_publisher_node = Node( package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', parameters=[{'robot_description': Command(['xacro ', default_model_path])}], condition=UnlessCondition(LaunchConfiguration('gui')))joint_state_publisher_gui_node = Node( package='joint_state_publisher_gui', executable='joint_state_publisher_gui', name='joint_state_publisher_gui', condition=IfCondition(LaunchConfiguration('gui')))从 return LaunchDescription([]) 中移除以下内容：
DeclareLaunchArgument(name='gui', default_value='True', description='Flag to enable joint_state_publisher_gui'),joint_state_publisher_node,joint_state_publisher_gui_node,接下来，打开 package.xml 并删除这些行：
<exec_depend>joint_state_publisher</exec_depend><exec_depend>joint_state_publisher_gui</exec_depend>用以下内容替换原有的 import 语句。其中 RosGzBridge 用于启动 ROS 2 与 Gazebo 之间的桥接，GzServer 用于启动 Gazebo 仿真。同时导入 get_package_share_directory、ExecuteProcess、IncludeLaunchDescription 和 PythonLaunchDescriptionSource，供后续修改使用。
import os
from ament_index_python.packages import get_package_share_directoryfrom launch import LaunchDescriptionfrom launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescriptionfrom launch.launch_description_sources import PythonLaunchDescriptionSourcefrom launch.substitutions import Command, LaunchConfigurationfrom launch_ros.actions import Nodefrom ros_gz_bridge.actions import RosGzBridgefrom ros_gz_sim.actions import GzServer由于不再导入 FindPackageShare，改用 get_package_share_directory 函数来查找包的 share 目录。修改 pkg_share 变量定义：
pkg_share = get_package_share_directory('sam_bot_description')添加以下两个变量，用于启动 Gazebo 世界和生成机器人：
ros_gz_sim_share = get_package_share_directory('ros_gz_sim')gz_spawn_model_launch_source = os.path.join(ros_gz_sim_share, "launch", "gz_spawn_model.launch.py")要让 robot_state_publisher 使用 use_sim_time，按以下方式修改：
robot_state_publisher_node = Node( package='robot_state_publisher', executable='robot_state_publisher', parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}, {'use_sim_time': LaunchConfiguration('use_sim_time')}])还需在 LaunchDescription([]) 中声明 use_sim_time 参数，添加以下内容：
DeclareLaunchArgument(name='use_sim_time', default_value='True', description='Flag to enable use_sim_time'),要启动 Gazebo 并在其中生成 sam_bot，在 display.launch.py 的 rviz_node 定义之后添加以下内容：
gz_server = GzServer( world_sdf_file=world_path, container_name='ros_gz_container', create_own_container='True', use_composition='True',)ros_gz_bridge = RosGzBridge( bridge_name='ros_gz_bridge', config_file=bridge_config_path, container_name='ros_gz_container', create_own_container='False', use_composition='True',)spawn_entity = IncludeLaunchDescription( PythonLaunchDescriptionSource(gz_spawn_model_launch_source), launch_arguments={ 'world': 'my_world', 'topic': '/robot_description', 'entity_name': 'sam_bot', 'z': '0.65', }.items(),)还需向 LaunchDescription([]) 添加以下内容：
ExecuteProcess(cmd=['gz', 'sim', '-g'], output='screen'),gz_server,ros_gz_bridge,spawn_entity,最后，在 package.xml 中添加以下依赖：
<exec_depend>ros_gz_bridge</exec_depend><exec_depend>ros_gz_sim</exec_depend>此时，完整的启动文件应如下所示：
import os
from ament_index_python.packages import get_package_share_directoryfrom launch import LaunchDescriptionfrom launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescriptionfrom launch.launch_description_sources import PythonLaunchDescriptionSourcefrom launch.substitutions import Command, LaunchConfigurationfrom launch_ros.actions import Nodefrom ros_gz_bridge.actions import RosGzBridgefrom ros_gz_sim.actions import GzServer
def generate_launch_description(): pkg_share = get_package_share_directory('sam_bot_description') ros_gz_sim_share = get_package_share_directory('ros_gz_sim') gz_spawn_model_launch_source = os.path.join(ros_gz_sim_share, "launch", "gz_spawn_model.launch.py") default_model_path = os.path.join(pkg_share, 'src', 'description', 'sam_bot_description.sdf') default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'config.rviz') world_path = os.path.join(pkg_share, 'world', 'my_world.sdf') bridge_config_path = os.path.join(pkg_share, 'config', 'bridge_config.yaml')
robot_state_publisher_node = Node( package='robot_state_publisher', executable='robot_state_publisher', parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}, {'use_sim_time': LaunchConfiguration('use_sim_time')}] ) rviz_node = Node( package='rviz2', executable='rviz2', name='rviz2', output='screen', arguments=['-d', LaunchConfiguration('rvizconfig')], ) gz_server = GzServer( world_sdf_file=world_path, container_name='ros_gz_container', create_own_container='True', use_composition='True', ) ros_gz_bridge = RosGzBridge( bridge_name='ros_gz_bridge', config_file=bridge_config_path, container_name='ros_gz_container', create_own_container='False', use_composition='True', ) spawn_entity = IncludeLaunchDescription( PythonLaunchDescriptionSource(gz_spawn_model_launch_source), launch_arguments={ 'world': 'my_world', 'topic': '/robot_description', 'entity_name': 'sam_bot', 'z': '0.65', }.items(), )
return LaunchDescription([ DeclareLaunchArgument(name='use_sim_time', default_value='True', description='Flag to enable use_sim_time'), DeclareLaunchArgument(name='model', default_value=default_model_path, description='Absolute path to robot model file'), DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path, description='Absolute path to rviz config file'), ExecuteProcess(cmd=['gz', 'sim', '-g'], output='screen'), robot_state_publisher_node, rviz_node, gz_server, ros_gz_bridge, spawn_entity, ])构建、运行与验证
Section titled “构建、运行与验证”运行包，检查 /demo/imu 和 /demo/odom 话题是否处于活跃状态。
进入项目根目录，执行以下命令：
colcon build. install/setup.bashros2 launch sam_bot_description display.launch.pyGazebo 启动后，应能看到 sam_bot 的 3D 模型：
要查看系统中的活跃话题，打开一个新终端，执行：
ros2 topic list话题列表中应包含以下内容：
/clock/demo/cmd_vel/demo/imu/demo/odom/joint_states/tf要查看话题的详细信息，执行：
ros2 topic info /topic然后在 RViz 中将固定坐标系（fixed frame）设为 odom，执行以下命令移动机器人：
ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true --remap cmd_vel:=/demo/cmd_vel此时应能看到机器人在 Gazebo 和 RViz 中同步移动。这里使用的是 TwistStamped 消息，因为从 ROS 2 Jazzy 版本起，它已成为大多数包的标准。部分 Nav2 节点提供 enable_stamped_cmd_vel 参数，可设为 true。对于 ROS 2 Jazzy 及更新版本，该参数默认即为 true。
本指南介绍了 Nav2 期望从里程计系统获得的消息和变换，讲解了如何设置里程计系统，以及如何验证发布的消息。