RtabMap仿真之无人车建模 - 李晓刚的博客

源码

lxg@lxg:~/code/xt/gitlab/rtabmap_sim_ws/src$ tree
.
├── rtabmap_sim_bringup
│   ├── CMakeLists.txt
│   ├── launch
│   │   ├── bringup_mapping.launch.py
│   │   └── bringup_navigation.launch.py
│   ├── maps
│   ├── package.xml
│   └── rviz
│       ├── mapping.rviz
│       └── navigation.rviz
├── rtabmap_sim_description
│   ├── CMakeLists.txt
│   ├── launch
│   │   └── display.launch.py
│   ├── meshes
│   ├── package.xml
│   ├── rviz
│   │   └── display.rviz
│   └── urdf
│       ├── gz_plugins.xacro
│       ├── robot.urdf.xacro
│       └── sensors
│           ├── imu.xacro
│           ├── lidar_2d.xacro
│           └── rgbd_camera.xacro
├── rtabmap_sim_gazebo
│   ├── CMakeLists.txt
│   ├── config
│   │   └── ros_gz_bridge.yaml
│   ├── launch
│   │   └── spawn_robot.launch.py
│   ├── materials
│   │   └── textures
│   │       ├── brick_wall.png
│   │       ├── checker.png
│   │       ├── concrete.png
│   │       └── wood_floor.png
│   ├── package.xml
│   └── worlds
│       ├── empty_test.sdf
│       └── sim_world.sdf
├── rtabmap_sim_localization
│   ├── CMakeLists.txt
│   ├── config
│   │   └── ekf.yaml
│   ├── launch
│   │   └── ekf.launch.py
│   └── package.xml
├── rtabmap_sim_navigation
│   ├── CMakeLists.txt
│   ├── config
│   │   ├── behavior_tree.xml
│   │   └── nav2_params.yaml
│   ├── launch
│   │   └── navigation.launch.py
│   └── package.xml
├── rtabmap_sim_slam
│   ├── CMakeLists.txt
│   ├── config
│   │   └── rtabmap.yaml
│   ├── launch
│   │   └── slam.launch.py
│   └── package.xml
└── rtabmap_sim_teleop
    ├── CMakeLists.txt
    ├── launch
    │   └── teleop.launch.py
    └── package.xml

坐标树结构

map
 └── odom                                (动态, 由 rtabmap 发布, 低频修正)
      └── base_footprint                 (动态, 由 EKF 发布, 高频连续)
           └── base_link                 (静态, base_footprint→base_link 固定高度差, robot_state_publisher)
                ├── wheel_left_link      (动态-关节, 车轮转动角度, robot_state_publisher ← /joint_states)
                ├── wheel_right_link    (动态-关节)
                ├── caster_wheel_link   (静态或动态，视万向轮建模而定)
                ├── imu_link            (静态, robot_state_publisher)
                ├── laser_link          (静态, 2D LiDAR 安装位置, robot_state_publisher)
                └── camera_link         (静态, robot_state_publisher)
                     ├── camera_rgb_frame
                     │    └── camera_rgb_optical_frame     (静态, REP-103 光学坐标系: Z前 X右 Y下)
                     └── camera_depth_frame
                          └── camera_depth_optical_frame  (静态)

rosgraph

Topic

机器人纯几何模型 rtabmap_sim_description

每个刚体部件就是一个 link。一个 link 由三部分组成：

link
 ├── visual     —— 长得什么样（只是视觉显示，不影响物理）
 ├── collision  —— 碰撞检测用（可以比 visual 简化以提升性能）
 └── inertial   —— 质量/惯量

子元素	🟢 通用	🔵 仿真	🟠 实物	说明
`visual`	✅ RViz 显示、TF 可视化	✅ Gazebo 视觉渲染	✅ RViz 显示	通用
`collision`	✅ Nav2 代价地图避障	✅ Gazebo 物理碰撞检测	✅ Nav2 代价地图避障	通用，但仿真中物理碰撞更精细
`inertial`	❌ 实物不依赖 URDF 惯量	✅ Gazebo 物理引擎必须	❌ 实物用真实物理，不需要 URDF 中的惯性数据	🔵 仿真必须写；🟢 实物可以不写或写个近似值（ROS 控制不依赖它）

仿真差速小车模型

对本项目的差速小车：

       俯视图（从上方看）
   ┌─────────────────────┐
   │       前万向轮        │
   │         ●            │
   │ 左轮■           ■右轮 │
   │   (base_link 中心)    │
   │         ●            │
   │       后万向轮        │
   └─────────────────────┘

       侧视图（Z 方向高度关系）

               ↑ Z
   ┌───────────┴───────────┐   ← base_link 中心
   │        底盘           │       z = base_footprint_to_base_link_z
   │    (base_link)        │         = wheel_radius + base_height/2.0
   │    高=base_height     │         = 0.05 + 0.05 = 0.10m
   └───────────┬───────────┘
               │
               │ base_height/2.0 = 0.05m
               │
          ─────┴─────      ← 轮子安装面（底盘底面）
               │
          ─────┴─────      ← 驱动轮中心（z = wheel_radius = 0.05m）
               │
            ───┴───        ← 驱动轮触地点
        ═══════╧════════   ← 地面（z = 0），base_footprint 在此平面

  注：万向轮半径(0.025m) < 驱动轮半径(0.05m)
       所以万向轮 Z = -(base_height/2.0) - (0.05-0.025) = -0.075m
       比驱动轮的 -(base_height/2.0) = -0.05m 多下移 0.025m 才能接地
        └──────┘

robot.urdf.xacro

<?xml version="1.0"?>
<!--
  Step 1：纯几何模型（无 Gazebo 插件、无传感器）。
  坐标系与命名遵循 docs/架构设计.md 第3节 TF 坐标树设计：
    base_footprint -> base_link -> {left_wheel_link, right_wheel_link,
                      front_caster_wheel_link, rear_caster_wheel_link}
  后续步骤会在此文件基础上：
    - Step 2  : 引入 gz_plugins.xacro（gz-sim DiffDrive / JointStatePublisher 插件）
    - Step 3~5: 引入 sensors/lidar_2d.xacro、imu.xacro、rgbd_camera.xacro

  本文件仅作为编排入口，通过 xacro:include 按依赖顺序组合各模块。
-->
<robot name="rtabmap_sim_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Step 1a: 参数配置 -->
  <xacro:include filename="$(find rtabmap_sim_description)/urdf/config/robot_params.xacro"/>

  <!-- Step 1b: 惯性计算宏（被 body 和传感器文件引用） -->
  <xacro:include filename="$(find rtabmap_sim_description)/urdf/config/robot_inertia_macros.xacro"/>

  <!-- Step 1c: 机器人本体（base_footprint / base_link / 驱动轮 / 万向轮） -->
  <xacro:include filename="$(find rtabmap_sim_description)/urdf/config/robot_body.xacro"/>

  <!-- Step 2: gz-sim 仿真插件（DiffDrive / JointStatePublisher） -->
  <xacro:include filename="$(find rtabmap_sim_description)/urdf/gz_plugins.xacro"/>

  <!-- Step 3: 2D LiDAR 传感器 -->
  <xacro:include filename="$(find rtabmap_sim_description)/urdf/sensors/lidar_2d.xacro"/>

  <!-- Step 4: IMU 传感器 -->
  <xacro:include filename="$(find rtabmap_sim_description)/urdf/sensors/imu.xacro"/>

  <!-- Step 5: RGBD 相机 -->
  <xacro:include filename="$(find rtabmap_sim_description)/urdf/sensors/rgbd_camera.xacro"/>

</robot>

robot_params.xacro

这辆车有多大、多重

<?xml version="1.0"?>
<!--
  Step 1 参数配置文件：包含机器人几何/物理参数、材质定义和惯量宏。
  由 robot.urdf.xacro 在开头 include 引入。
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- ============ 底盘参数 🟢 ============ -->
  <xacro:property name="base_length" value="0.30"/>   <!-- 🟢 底盘长(m)，X方向 -->
  <xacro:property name="base_width" value="0.22"/>    <!-- 🟢 底盘宽(m)，Y方向 -->
  <xacro:property name="base_height" value="0.10"/>   <!-- 🟢 底盘高(m)，Z方向 -->
  <xacro:property name="base_mass" value="5.0"/>      <!-- 🟢 底盘质量(kg)，实物可不填 -->

  <!-- ============ 驱动轮参数 🟢 ============ -->
  <xacro:property name="wheel_radius" value="0.05"/>        <!-- 🟢 驱动轮半径(m) -->
  <xacro:property name="wheel_thickness" value="0.04"/>     <!-- 🟢 驱动轮厚度/宽度(m) -->
  <xacro:property name="wheel_mass" value="0.2"/>           <!-- 🟢 单个驱动轮质量(kg)，实物可不填 -->
  <xacro:property name="wheel_separation" value="${base_width + wheel_thickness}"/>  <!-- 🟢 左右轮中心距=底盘宽+轮厚 -->
  <xacro:property name="wheel_x_offset" value="0.0"/>       <!-- 🟢 驱动轮在X方向的偏移，0=前后居中 -->

  <!-- ============ 万向轮参数（十字矩形四点支撑布局）🟢 ============ -->
  <!-- 参数说明：底盘前后各一个万向轮，+ 左右两个驱动轮，形成四点支撑 -->
  <xacro:property name="caster_radius" value="0.025"/>                <!-- 🟢 万向轮半径(m)，比驱动轮小 -->
  <xacro:property name="caster_mass" value="0.05"/>                   <!-- 🟢 单个万向轮质量(kg)，实物可不填 -->
  <!-- 🟢 后万向轮X偏移(车尾方向，负X) = -(半车长 - 半轮半径 - 预留间隙) -->
  <xacro:property name="caster_x_offset" value="${-(base_length/2.0 - caster_radius - 0.01)}"/>
  <!-- 🟢 前万向轮X偏移(车头方向，正X) = 半车长 - 半轮半径 - 预留间隙 -->
  <xacro:property name="front_caster_x_offset" value="${base_length/2.0 - caster_radius - 0.01}"/>

  <!-- 🟢 base_footprint(地面z=0) 到 base_link(几何中心)的Z高度 -->
  <!--     = wheel_radius(轮心到地面) + base_height/2.0(底盘半高) -->
  <xacro:property name="base_footprint_to_base_link_z" value="${wheel_radius + base_height/2.0}"/>

  <!-- ============ 材质 🟢 ============ -->
  <material name="chassis_grey">
    <color rgba="0.6 0.6 0.6 1.0"/>  <!-- 🟢 底盘灰色：RGB 0.6 中灰 -->
  </material>
  <material name="wheel_black">
    <color rgba="0.1 0.1 0.1 1.0"/>  <!-- 🟢 轮子黑色：RGB 0.1 近黑 -->
  </material>
  <material name="caster_dark">
    <color rgba="0.2 0.2 0.2 1.0"/>  <!-- 🟢 万向轮深灰 -->
  </material>

</robot>

robot_inertia_macros.xacro

每个部件的物理特性

🔵 仿真必须：Gazebo 物理引擎需要惯量来计算转动加速度。 🟠 实物可选：实物小车的 ROS 软件栈（Nav2、RSP）不依赖 URDF 中的惯性数据；但如果你要用 ros2_control 做高级控制或者做动力学建模，则需要。

robot_body.xacro

这些部件怎么组装

<?xml version="1.0"?>
<!--
  机器人本体（base_footprint + base_link + 驱动轮 + 万向轮）。
  依赖 robot_params.xacro 中的参数和 robot_inertia_macros.xacro 中的惯量宏。
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- ============================ base_footprint ============================ -->
  <!-- 虚拟根坐标系，投影在地面(z=0)，Nav2 碰撞检测的参考点 -->
  <link name="base_footprint"/>

  <!-- base_joint：从地面抬升到车身几何中心 -->
  <!-- 升高度 = wheel_radius(轮心高) + base_height/2.0(半车高) -->
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 ${base_footprint_to_base_link_z}" rpy="0 0 0"/>
  </joint>

  <!-- ============================== base_link =============================== -->
  <!-- 底盘主体：长×宽×高 = base_length × base_width × base_height -->
  <!-- 所有轮子和传感器都装在 base_link 上（parent 都是它） -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>  <!-- 长(X) × 宽(Y) × 高(Z) -->
      </geometry>
      <material name="chassis_grey"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${base_mass}"/>  <!-- 底盘质量(kg) -->
      <xacro:box_inertia mass="${base_mass}" lx="${base_length}" ly="${base_width}" lz="${base_height}"/>
    </inertial>
  </link>

  <!-- ======================== drive_wheel 宏 ================================ -->
  <!-- 驱动轮：差速驱动，一轮一宏                                     -->
  <!-- 宏参数：                                                       -->
  <!--   prefix  = "left" / "right"                                   -->
  <!--   reflect = ±1：左轮+1→Y正(左边)，右轮-1→Y负(右边)           -->
  <!--                                                                 -->
  <!-- 安装位置：                                                     -->
  <!--   X = wheel_x_offset = 0（前后居中）                           -->
  <!--   Y = reflect × wheel_separation / 2 （左正右负）              -->
  <!--       wheel_separation = base_width + wheel_thickness           -->
  <!--   Z = -(base_height/2.0)（从底盘中心向下到底面再装轮子）       -->
  <!-- ================================================================ -->
  <xacro:macro name="drive_wheel" params="prefix reflect">

    <!-- 轮子 link：圆柱体，绕 X 轴转 90° 让轴向从 Z 变到 Y -->
    <link name="${prefix}_wheel_link">
      <visual>
        <origin xyz="0 0 0" rpy="1.5708 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>  <!-- 半径 × 厚度 -->
        </geometry>
        <material name="wheel_black"/>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="1.5708 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
        </geometry>
      </collision>
      <inertial>
        <origin xyz="0 0 0" rpy="1.5708 0 0"/>
        <mass value="${wheel_mass}"/>  <!-- 单个驱动轮质量(kg) -->
        <xacro:cylinder_inertia mass="${wheel_mass}" radius="${wheel_radius}" length="${wheel_thickness}"/>
      </inertial>
    </link>

    <!-- 轮子 joint：continuous = 可无限旋转 -->
    <joint name="${prefix}_wheel_joint" type="continuous">
      <parent link="base_link"/>
      <child link="${prefix}_wheel_link"/>
      <origin xyz="
        ${wheel_x_offset}
        ${reflect * wheel_separation / 2.0}
        ${-(base_height/2.0)}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>  <!-- 绕 Y 轴旋转（差速驱动标准） -->
      <dynamics damping="0.1" friction="0.0"/>
    </joint>
  </xacro:macro>

  <!-- 实例化左右两个驱动轮 -->
  <xacro:drive_wheel prefix="left" reflect="1"/>
  <xacro:drive_wheel prefix="right" reflect="-1"/>

  <!-- ======================== caster_wheel 宏 =============================== -->
  <!-- 万向轮：固定球体被动脚轮，前后各一个，十字矩形四点支撑                 -->
  <!-- 宏参数：                                                               -->
  <!--   prefix = "front" / "rear"                                            -->
  <!--   x, y   = 安装偏移（前轮+X，后轮-X，Y=0 居中）                       -->
  <!--                                                                         -->
  <!-- Z 公式：-(base_height/2.0) - (wheel_radius - caster_radius)            -->
  <!--    = 到底盘底面 + 额外下移                                            -->
  <!--  驱动轮半径(0.05m) > 万向轮半径(0.025m)，需要多下移 0.025m 才能接地    -->
  <!-- ======================================================================== -->
  <xacro:macro name="caster_wheel" params="prefix x y">

    <link name="${prefix}_caster_wheel_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${caster_radius}"/>
        </geometry>
        <material name="caster_dark"/>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <sphere radius="${caster_radius}"/>
        </geometry>
      </collision>
      <inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="${caster_mass}"/>  <!-- 单个万向轮质量(kg) -->
        <xacro:sphere_inertia mass="${caster_mass}" radius="${caster_radius}"/>
      </inertial>
    </link>

    <!-- fixed = 固定关节，万向轮不旋转 -->
    <joint name="${prefix}_caster_wheel_joint" type="fixed">
      <parent link="base_link"/>
      <child link="${prefix}_caster_wheel_link"/>
      <origin xyz="${x} ${y} ${-(base_height/2.0) - (wheel_radius - caster_radius)}" rpy="0 0 0"/>
    </joint>
  </xacro:macro>

  <!-- 实例化前后两个万向轮，Y=0 居中 -->
  <!-- front_caster_x_offset = base_length/2.0 - caster_radius - 0.01  (到前边缘，缩半轮半径，留空隙) -->
  <!-- caster_x_offset       = -(base_length/2.0 - caster_radius - 0.01) (方向朝后) -->
  <xacro:caster_wheel prefix="front" x="${front_caster_x_offset}" y="0"/>
  <xacro:caster_wheel prefix="rear"  x="${caster_x_offset}" y="0"/>

</robot>

lidar_2d.xacro

2D激光雷达模型参数

<?xml version="1.0"?>
<!--
  Step 3：2D LiDAR。
  base_link -> laser_link 固定关节（几何/惯量），并通过 <gazebo reference="laser_link">
  声明 gz-sim gpu_lidar 传感器（由 world 级别已加载的 gz::sim::systems::Sensors 插件驱动，
  无需在此额外声明 <plugin>）。

  桥接：/scan（GZ→ROS），见 config/ros_gz_bridge.yaml。
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- 🟢 以下参数仿真和实物共用，决定传感器外观尺寸和TF位置 -->
  <xacro:property name="laser_radius" value="0.03"/>   <!-- 🟢 激光雷达外壳圆柱半径(m) -->
  <xacro:property name="laser_height" value="0.04"/>   <!-- 🟢 激光雷达外壳圆柱高度(m) -->
  <xacro:property name="laser_mass" value="0.05"/>     <!-- 🟢 激光雷达质量(kg)，实物可不填 -->

  <material name="laser_orange">
    <color rgba="0.9 0.4 0.05 1.0"/>  <!-- 🟢 橙色，RViz中显示用 -->
  </material>

  <!-- 🟢 link + joint：描述传感器安装位置，实物也需要（定义TF树和碰撞体积） -->
  <!-- 🔵 inertial：实物不依赖URDF惯量，但仿真需要 -->
  <link name="laser_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="${laser_radius}" length="${laser_height}"/>
      </geometry>
      <material name="laser_orange"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="${laser_radius}" length="${laser_height}"/>
      </geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${laser_mass}"/>
      <xacro:cylinder_inertia mass="${laser_mass}" radius="${laser_radius}" length="${laser_height}"/>
    </inertial>
  </link>

  <!-- 🟢 安装在底盘正上方中心，实物传感器也装在这个位置 -->
  <!-- 🟢 Z = base_height/2.0(底盘半高) + laser_height/2.0(传感器半高)，让底部贴紧底盘顶面 -->
  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <origin xyz="0 0 ${base_height/2.0 + laser_height/2.0}" rpy="0 0 0"/>
  </joint>

  <!-- ====================================================================== -->
  <!-- 🔵 以下为Gazebo仿真专用，物理小车不需要 -->
  <!--     实物小车用自己的激光雷达硬件驱动发布/scan话题               -->
  <!-- ====================================================================== -->
  <gazebo reference="laser_link">
    <sensor name="lidar_2d" type="gpu_lidar">  <!-- 🔵 GPU加速的2D激光雷达仿真传感器 -->
      <pose>0 0 0 0 0 0</pose>  <!-- 🔵 传感器相对laser_link的偏移，0=重合 -->
      <topic>scan</topic>  <!-- 🔵 Gazebo发布的话题名，经ros_gz_bridge桥接为ROS2的/scan -->
      <gz_frame_id>laser_link</gz_frame_id>  <!-- 🔵 scan消息的frame_id，与link名一致 -->
      <update_rate>10</update_rate>  <!-- 🔵 仿真扫描频率(Hz) -->
      <always_on>true</always_on>  <!-- 🔵 仿真启动后持续运行 -->
      <visualize>true</visualize>  <!-- 🔵 在Gazebo GUI中显示激光射线，便于调试 -->
      <lidar>
        <scan>
          <horizontal>
            <samples>360</samples>           <!-- 🔵 每圈采样点数，360=1°/点 -->
            <resolution>1</resolution>       <!-- 🔵 相邻射线间距(°) -->
            <min_angle>-3.14159265</min_angle>  <!-- 🔵 扫描起始角(rad)，-π=正右方开始 -->
            <max_angle>3.14159265</max_angle>    <!-- 🔵 扫描结束角(rad)，+π=正右方结束=完整360° -->
          </horizontal>
        </scan>
        <range>
          <min>0.12</min>    <!-- 🔵 最小探测距离(m) -->
          <max>12.0</max>    <!-- 🔵 最大探测距离(m) -->
          <resolution>0.01</resolution>  <!-- 🔵 距离分辨率(m) -->
        </range>
      </lidar>
    </sensor>
  </gazebo>

</robot>

rgbd_camera.xacro

<?xml version="1.0"?>
<!--
  Step 5：RGBD 相机。
  base_link -> camera_link（机械安装系，X 前/Z 上，与机器人本体一致）
             -> camera_rgb_frame -> camera_rgb_optical_frame（光学系，REP-103：Z前/X右/Y下）

  gz-sim 的 rgbd_camera 传感器是"单一视点"模拟（不像真实硬件 RGB 与 Depth 光心分离），
  因此 color/depth 两路图像共用同一个 gz_frame_id = camera_rgb_optical_frame，
  不再单独建 camera_depth_optical_frame（与设计文档相比的一个基于仿真实现现实的简化，
  已记录在仓库记忆中）。

  桥接：/camera/color/image_raw、/camera/color/camera_info、
        /camera/depth/image_raw、/camera/depth/camera_info（均 GZ→ROS），
  见 config/ros_gz_bridge.yaml。
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- 🟢 相机外壳尺寸和质量参数，仿真和实物共用（实物质量可不填） -->
  <xacro:property name="camera_len_x" value="0.03"/>    <!-- 🟢 相机外壳长(m)，X方向（车身前后） -->
  <xacro:property name="camera_len_y" value="0.09"/>    <!-- 🟢 相机外壳宽(m)，Y方向（车身左右） -->
  <xacro:property name="camera_len_z" value="0.025"/>   <!-- 🟢 相机外壳高(m)，Z方向 -->
  <xacro:property name="camera_mass" value="0.1"/>      <!-- 🟢 相机质量(kg) -->

  <!-- 🟢 安装位置参数 -->
  <!-- 🟢 camera_x_offset：相机安装在底盘前缘(X正方向)，离中心 0.10m -->
  <!-- 🟢 mast_height：桅杆高度 0.15m，抬高相机避免视野被地板占据 -->
  <!-- 🟢 camera_z_offset = 底盘半高 + mast_height + 相机半高，从地面算起 -->
  <xacro:property name="camera_x_offset" value="0.10"/>
  <xacro:property name="mast_height" value="0.15"/>
  <xacro:property name="camera_z_offset" value="${base_height/2.0 + mast_height + camera_len_z/2.0}"/>

  <material name="camera_black">
    <color rgba="0.05 0.05 0.05 1.0"/>
  </material>

  <!-- 🟢 camera_link + camera_joint：定义相机安装位置和TF -->
  <!-- 🔵 inertial：实物不需要URDF惯量 -->
  <link name="camera_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${camera_len_x} ${camera_len_y} ${camera_len_z}"/>
      </geometry>
      <material name="camera_black"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${camera_len_x} ${camera_len_y} ${camera_len_z}"/>
      </geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${camera_mass}"/>
      <xacro:box_inertia mass="${camera_mass}" lx="${camera_len_x}" ly="${camera_len_y}" lz="${camera_len_z}"/>
    </inertial>
  </link>

  <!-- 🟢 相机安装在底盘前上方，Z 抬高约 0.21m -->
  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin xyz="${camera_x_offset} 0 ${camera_z_offset}" rpy="0 0 0"/>
  </joint>

  <!-- 🟢 光学坐标系：纯坐标系 link，无几何/惯量，仅用于TF树 -->
  <link name="camera_rgb_frame"/>
  <joint name="camera_rgb_joint" type="fixed">
    <parent link="camera_link"/>
    <child link="camera_rgb_frame"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- 🟢 与camera_link原点重合 -->
  </joint>

  <link name="camera_rgb_optical_frame"/>
  <joint name="camera_rgb_optical_joint" type="fixed">
    <parent link="camera_rgb_frame"/>
    <child link="camera_rgb_optical_frame"/>
    <!-- 🟢 REP-103 旋转：机体系(X前/Y左/Z上) → 光学系(Z前/X右/Y下) -->
    <!--   rpy顺序：先绕Z转-90°(X→Y)，再绕X转-90°(Z→X)，结果Z指向前方 -->
    <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
  </joint>

  <!-- ====================================================================== -->
  <!-- 🔵 以下为Gazebo仿真专用，物理小车不需要                         -->
  <!--     实物RGBD相机由硬件驱动发布 /camera/color/image_raw 等话题    -->
  <!-- ====================================================================== -->
  <gazebo reference="camera_link">
    <sensor name="rgbd_camera" type="rgbd_camera">  <!-- 🔵 RGBD相机仿真传感器 -->
      <!-- 🔵 <pose>与camera_link一致，identity即沿本地+X看 -->
      <pose>0 0 0 0 0 0</pose>
      <topic>camera</topic>  <!-- 🔵 Gazebo发布的话题前缀，桥接后为/camera/* -->
      <gz_frame_id>camera_rgb_optical_frame</gz_frame_id>  <!-- 🔵 图像消息的frame_id -->
      <update_rate>15</update_rate>  <!-- 🔵 帧率(Hz)，标准RGBD相机约15~30Hz -->
      <always_on>true</always_on>  <!-- 🔵 仿真启动后持续运行 -->
      <camera>
        <horizontal_fov>1.047</horizontal_fov>  <!-- 🔵 水平视场角(rad)，约60° -->
        <image>
          <width>424</width>        <!-- 🔵 图像宽度(像素)，降低分辨率可提升仿真性能 -->
          <height>240</height>      <!-- 🔵 图像高度(像素) -->
          <format>R8G8B8</format>   <!-- 🔵 RGB彩色图，8bit/通道 -->
        </image>
        <clip>
          <near>0.1</near>   <!-- 🔵 最近渲染距离(m)，小于此值不显示 -->
          <far>10.0</far>    <!-- 🔵 最远渲染距离(m)，超出此值不显示 -->
        </clip>
      </camera>
      <!-- 🔵 深度相机：必须显式声明，否则Gazebo不初始化深度渲染管线 -->
      <depth_camera>
        <clip>
          <near>0.1</near>   <!-- 🔵 深度图最近探测距离(m) -->
          <far>10.0</far>    <!-- 🔵 深度图最远探测距离(m) -->
        </clip>
      </depth_camera>
    </sensor>
  </gazebo>

</robot>

imu.xacro

<?xml version="1.0"?>
<!--
  Step 4：IMU。
  base_link -> imu_link 固定关节（几何/惯量很小，贴近底盘中心安装），
  通过 <gazebo reference="imu_link"> 声明 gz-sim imu 传感器（由 world 级别已加载的
  gz::sim::systems::Imu 插件驱动）。

  桥接：/imu/data（GZ→ROS），见 config/ros_gz_bridge.yaml。
  注意 gz 消息类型是 `gz.msgs.IMU`（全大写 IMU，不是 Imu）。
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- 🟢 IMU体积和质量参数，仿真和实物共用（实物质量可不填） -->
  <xacro:property name="imu_size" value="0.02"/>    <!-- 🟢 IMU外壳边长(m)，立方体 -->
  <xacro:property name="imu_mass" value="0.02"/>   <!-- 🟢 IMU质量(kg) -->

  <material name="imu_red">
    <color rgba="0.8 0.1 0.1 1.0"/>  <!-- 🟢 红色，RViz中识别用 -->
  </material>

  <!-- 🟢 imu_link：定义IMU外观和碰撞体积，实物也需要 -->
  <!-- 🔵 inertial：实物不需要URDF惯量 -->
  <link name="imu_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${imu_size} ${imu_size} ${imu_size}"/>  <!-- 🟢 小立方体 -->
      </geometry>
      <material name="imu_red"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${imu_size} ${imu_size} ${imu_size}"/>
      </geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${imu_mass}"/>  <!-- 🟢 IMU质量(kg) -->
      <xacro:box_inertia mass="${imu_mass}" lx="${imu_size}" ly="${imu_size}" lz="${imu_size}"/>
    </inertial>
  </link>

  <!-- 🟢 安装在底盘几何中心(0,0,0)，实物IMU也建议装在重心附近以减少离心力干扰 -->
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- 🟢 与base_link原点完全重合 -->
  </joint>

  <!-- ====================================================================== -->
  <!-- 🔵 以下为Gazebo仿真专用，物理小车不需要                         -->
  <!--     实物IMU由硬件驱动发布 /imu/data 话题                        -->
  <!-- ====================================================================== -->
  <gazebo reference="imu_link">
    <sensor name="imu_sensor" type="imu">  <!-- 🔵 IMU仿真传感器 -->
      <pose>0 0 0 0 0 0</pose>  <!-- 🔵 传感器与imu_link原点重合 -->
      <topic>imu/data</topic>  <!-- 🔵 Gazebo话题名，桥接后为ROS2的/imu/data -->
      <gz_frame_id>imu_link</gz_frame_id>  <!-- 🔵 IMU消息的frame_id -->
      <update_rate>100</update_rate>  <!-- 🔵 IMU采样频率(Hz)，实物IMU典型值100~400Hz -->
      <always_on>true</always_on>  <!-- 🔵 仿真启动后持续运行 -->
      <imu>
        <angular_velocity>  <!-- 🔵 角速度噪声：高斯噪声 N(mean, stddev)，rad/s -->
          <x><noise type="gaussian"><mean>0.0</mean><stddev>0.0002</stddev></noise></x>  <!-- 🔵 X轴角速度噪声标准差 0.0002 rad/s -->
          <y><noise type="gaussian"><mean>0.0</mean><stddev>0.0002</stddev></noise></y>
          <z><noise type="gaussian"><mean>0.0</mean><stddev>0.0002</stddev></noise></z>
        </angular_velocity>
        <linear_acceleration>  <!-- 🔵 线加速度噪声：高斯噪声，m/s² -->
          <x><noise type="gaussian"><mean>0.0</mean><stddev>0.017</stddev></noise></x>  <!-- 🔵 X轴线加速度噪声标准差 0.017 m/s² -->
          <y><noise type="gaussian"><mean>0.0</mean><stddev>0.017</stddev></noise></y>
          <z><noise type="gaussian"><mean>0.0</mean><stddev>0.017</stddev></noise></z>
        </linear_acceleration>
      </imu>
    </sensor>
  </gazebo>

</robot>

读取机器人模型参数命令


ros2 param get /robot_state_publisher robot_description

display.launch.py

#!/usr/bin/env python3
"""🔵 Step 1 验证用 launch 文件：仅加载机器人几何模型，不接入 Gazebo。

启动内容（每个 Node 为一个独立进程）：
  - robot_state_publisher  : 🟢 订阅 /joint_states，发布 TF 与 robot_description
  - joint_state_publisher_gui: 🔵 提供滑块手动驱动各关节，模拟 /joint_states
  - rviz2                  : 🟢 加载预置视图，显示 RobotModel + TF

🔵 本 launch 是仿真/开发调试工具，不是实物运行时的启动入口。
    实物小车另有自己的 launch 文件（启动硬件驱动等），不会用这个。

用法：
  ros2 launch rtabmap_sim_description display.launch.py
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    """构建 launch 描述：声明参数 → 定义节点 → 返回 LaunchDescription。"""
    # 通过 FindPackageShare 获取本包在 install 目录下的路径，用于构造模型和 RViz 配置文件的绝对路径
    pkg_share = FindPackageShare("rtabmap_sim_description")

    # 默认模型路径：install/.../urdf/robot.urdf.xacro
    default_model_path = PathJoinSubstitution(
        [pkg_share, "urdf", "robot.urdf.xacro"]
    )
    # 默认 RViz 配置文件路径：install/.../rviz/display.rviz
    default_rviz_config_path = PathJoinSubstitution(
        [pkg_share, "rviz", "display.rviz"]
    )

    # 声明 launch 参数，允许用户在命令行覆盖：ros2 launch ... model:=<你的路径>
    model_arg = DeclareLaunchArgument(
        name="model",
        default_value=default_model_path,
        description="机器人 xacro 模型文件的绝对路径",
    )
    rviz_arg = DeclareLaunchArgument(
        name="rviz_config",
        default_value=default_rviz_config_path,
        description="RViz2 预置视图配置文件路径",
    )

    # 编译 xacro 得到 robot_description（运行 `xacro` 命令，结果作为字符串传入参数）
    robot_description = {
        "robot_description": ParameterValue(
            Command(["xacro ", LaunchConfiguration("model")]), value_type=str
        )
    }

    # 🟢 节点 1：robot_state_publisher
    # 🟢 加载 robot_description → 发布 /robot_description 话题 + 根据 URDF 中的 joint 定义发布 TF
    # 🟢 实物也需要它来宣告机器人模型和 TF 树
    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="screen",
        parameters=[robot_description],
    )

    # 🔵 节点 2：joint_state_publisher_gui
    # 🔵 弹出滑块窗口，用户手动拖动各关节 → 发布 /joint_states
    # 🔵 实物不需要：实物有电机编码器驱动直接发布真实的 /joint_states
    joint_state_publisher_gui_node = Node(
        package="joint_state_publisher_gui",
        executable="joint_state_publisher_gui",
        name="joint_state_publisher_gui",
        output="screen",
    )

    # 🟢 节点 3：rviz2
    # 🟢 3D 可视化窗口，加载预置的 display.rviz 视图（已配置 RobotModel + TF 显示）
    # 🟢 实物开发调试时也需要 RViz 查看传感器数据
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", LaunchConfiguration("rviz_config")],
    )

    # 按顺序组装：先声明参数，再依次启动 3 个节点
    return LaunchDescription(
        [
            model_arg,
            rviz_arg,
            robot_state_publisher_node,
            joint_state_publisher_gui_node,
            rviz_node,
        ]
    )

0 次点赞

源码