Gazebo Classic 传感器设置

本指南介绍传感器在安全导航中的重要性，以及如何为 Nav2 配置传感器。前半部分简要介绍 Nav2 中常用的传感器及常见传感器消息类型，然后在此前构建的仿真机器人 sam_bot 上添加基本的传感器配置。最后，通过 RViz 可视化来验证 sam_bot 的仿真传感器消息。

传感器一旦部署到机器人上，其读数即可用于建图、定位和感知任务。本指南后半部分首先讨论建图和定位如何使用传感器数据，然后介绍 Nav2 中的 nav2_costmap_2d 包——它生成供 Nav2 路径规划使用的代价地图。我们会为该包配置基本参数，使其正确接收来自 sam_bot 的传感器信息，最后在 RViz 中可视化生成的代价地图以验证数据接收是否正常。

传感器简介

移动机器人搭载大量传感器，用于感知周围环境。传感器获取的信息可用于构建和维护环境地图、在地图上定位机器人，以及检测环境中的障碍物。这些任务对于在动态环境中实现安全高效的导航至关重要。

常见的传感器包括激光雷达（lidar）、雷达（radar）、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_msgs、radar_msgs 和 vision_msgs 的 API 文档。

实体机器人的传感器通常都有对应的 ROS 驱动程序（即负责连接传感器、将数据填充到消息中并发布供机器人使用的 ROS 节点），这些驱动遵循 sensor_msgs 包中的标准接口。sensor_msgs 包使用户能够轻松集成不同制造商的传感器。Nav2 等通用软件包读取这些标准化消息后，即可执行与具体传感器硬件无关的任务。在 sam_bot 等仿真机器人上，Gazebo 也提供传感器插件，同样按照 sensor_msgs 标准发布信息。

常见传感器消息

本小节介绍在配置 Nav2 时可能遇到的一些常见 sensor_msgs 类型。每种传感器都会附上简要说明、在 Gazebo 中的仿真截图，以及相应的 RViz 传感器读数可视化效果。

注意：除下面列出的类型外，sensor_msgs 还包含其他消息类型。完整的消息列表及定义可在 sensor_msgs 文档中查阅。

sensor_msgs/LaserScan

该消息表示平面激光测距仪的单次扫描。用于 slam_toolbox 和 nav2_amcl 的定位与建图，也用于 nav2_costmap_2d 的感知。

sensor_laserscan

sensor_msgs/PointCloud2

该消息包含一组 3D 点，每个点可附带可选的附加信息。这些点可来自 3D 激光雷达、2D 激光雷达、深度相机等。

sensor_pointcloud2

sensor_msgs/Range

这是来自有源测距仪的单次距离读数。有源测距仪发射能量并报告沿一条弧线有效的测量距离。声呐、红外传感器或一维测距仪是使用该消息的典型传感器示例。

sensor_range

sensor_msgs/Image

表示来自 RGB 或深度相机的传感器读数，分别对应 RGB 值或距离值。

sensor_image

使用 Gazebo Classic 仿真传感器

为帮助读者掌握在仿真机器人上配置传感器的方法，本节将在上一个教程的基础上，为仿真机器人 sam_bot 添加传感器。与上一个教程中使用 Gazebo 插件为 sam_bot 添加里程计传感器类似，这里将使用 Gazebo 插件为 sam_bot 仿真一个激光雷达和一个深度相机。如果使用的是真实机器人，大部分步骤仍然用于配置 URDF 坐标系，同时提前添加 Gazebo 插件也便于日后切换到仿真环境。

请确保已正确安装 Gazebo，可按照上一个教程中的设置与先决条件说明进行安装。

向 URDF 添加 Gazebo 插件

首先为 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 插件引用。其中还配置了激光雷达的扫描范围和距离属性。最后，将 /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_link 上的 camera_depth_frame，作为深度相机插件的 <frame_name>。插件配置为分别向 /depth_camera/image_raw 和 /depth_camera/points 话题发布 sensor_msgs/Image 和 sensor_msgs/PointCloud2 消息。其余基本配置属性也在同一处设置。

启动与构建文件

为验证传感器配置正确并能看到环境中的物体，需要在带有物体的 Gazebo 世界中启动 sam_bot。

首先创建一个 Gazebo 世界，其中放置一个立方体和一个球体，位于 sam_bot 的传感器检测范围内，以便验证传感器能否正确检测到这些物体。

在项目根目录下创建一个名为 world 的目录，并在其中创建 my_world.sdf 文件。然后将 world/my_world.sdf 的内容复制粘贴到该文件中。

接下来修改启动文件 launch/display.launch.py，使用刚创建的世界来启动 Gazebo。首先，在 generate_launch_description() 中添加以下行，指定 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'),

还需要将 world 目录添加到 CMakeLists.txt 中。打开 CMakeLists.txt，在 install(DIRECTORY...) 中添加 world 目录，如下所示。

install(
  DIRECTORY src launch rviz config world
  DESTINATION share/${PROJECT_NAME}
)

构建、运行与验证

现在可以构建并运行项目了。进入项目根目录，执行以下命令：

colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.py

随后 RViz 和 Gazebo 将启动，sam_bot 同时出现在两者中。Gazebo 窗口中应加载刚创建的世界，sam_bot 生成于其中。此时应能看到搭载 360 度激光雷达和深度相机的 sam_bot，如下图所示。

gazebo_sensors

在 RViz 窗口中，可以验证传感器建模是否正确，以及新添加传感器的坐标变换是否正确：

rviz_sensors

还可以在 RViz 中可视化传感器读数。要可视化发布在 /scan 话题上的 sensor_msgs/LaserScan 消息，请点击 RViz 窗口底部的 Add 按钮，然后切换到 By topic 选项卡，在 /scan 下选择 LaserScan，如下所示。

add_topic_laserscan

接下来，将 RViz 中的 Reliability Policy 设置为 Best Effort，并将 size 设置为 0.1，以便更清晰地看到这些点。此时应看到如下所示的 LaserScan 可视化效果，对应的正是添加到 Gazebo 世界中的立方体和球体。

demo_laserscan_rviz

要可视化 sensor_msgs/Image 和 sensor_msgs/PointCloud2，请对 /depth_camera/image_raw 和 /depth_camera/points 话题分别执行相同的操作：

add_topic_image_pointcloud2

在 RViz 中添加 /depth_camera/image_raw 话题后，将 Reliability Policy 设置为 Best Effort。此时应在 RViz 窗口左下方的图像窗口中看到立方体，如下所示。

demo_image_rviz

还应看到 sensor_msgs/PointCloud2 的可视化效果，如下所示。

pointcloud2_data

结论

本指南介绍了 Nav2 中常见的传感器消息类型——它们为不同传感器厂商标准化了消息格式。同时介绍了如何使用 Gazebo 向仿真机器人添加传感器，以及如何通过 RViz 验证传感器是否正常工作。