从零构建 Nav2 仿真机器人：一条命令的背后

<xacro:macro name="wheel" params="prefix x_reflect y_reflect">
  <link name="${prefix}_link">
    <visual>
      <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
      <geometry><cylinder radius="${wheel_radius}" length="${wheel_width}"/></geometry>
      <material name="Gray"><color rgba="0.5 0.5 0.5 1.0"/></material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
      <geometry><cylinder radius="${wheel_radius}" length="${wheel_width}"/></geometry>
    </collision>
    <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
  </link>
  <joint name="${prefix}_joint" type="continuous">
    <parent link="base_link"/>
    <child link="${prefix}_link"/>
    <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>
</xacro:macro>

<xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />   <!-- 左后轮 -->
<xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />  <!-- 右后轮 -->

<xacro:macro name="box_inertia" params="m w h d">
  <inertial>
    <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
    <mass value="${m}"/>
    <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0"
             iyy="${(m/12) * (w*w + d*d)}" iyz="0.0"
             izz="${(m/12) * (w*w + h*h)}"/>
  </inertial>
</xacro:macro>

<xacro:macro name="cylinder_inertia" params="m r h">
  <inertial>
    <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
    <mass value="${m}"/>
    <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy="0" ixz="0"
             iyy="${(m/12) * (3*r*r + h*h)}" iyz="0"
             izz="${(m/2) * (r*r)}"/>
  </inertial>
</xacro:macro>

<gazebo>
  <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
    <ros><namespace>/demo</namespace></ros>

    <left_joint>drivewhl_l_joint</left_joint>
    <right_joint>drivewhl_r_joint</right_joint>

    <wheel_separation>0.4</wheel_separation>    <!-- 左右轮中心距 0.4m -->
    <wheel_diameter>0.2</wheel_diameter>         <!-- 轮径 0.2m -->

    <max_wheel_torque>20</max_wheel_torque>
    <max_wheel_acceleration>1.0</max_wheel_acceleration>

    <publish_odom>true</publish_odom>            <!-- 发布里程计消息 -->
    <publish_odom_tf>false</publish_odom_tf>      <!-- 不发布 TF（留给 ekf_node） -->
    <publish_wheel_tf>true</publish_wheel_tf>

    <odometry_frame>odom</odometry_frame>
    <robot_base_frame>base_link</robot_base_frame>
  </plugin>
</gazebo>

<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>
    <update_rate>100</update_rate>
    <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, 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, z 同理 -->
      </linear_acceleration>
    </imu>
  </sensor>
</gazebo>

ekf_filter_node:
    ros__parameters:
        frequency: 30.0
        two_d_mode: false
        publish_acceleration: true
        publish_tf: true                           # 发布 odom -> base_link

        map_frame: map
        odom_frame: odom
        base_link_frame: base_link
        world_frame: odom

        odom0: demo/odom
        odom0_config: [false, false, false,        # X Y Z 位置 -> 不融（累积误差大）
                       false, false, false,        # roll pitch yaw -> 不融
                       true,  true,  false,        # 线速度 X Y 融合 ← 核心！
                       false, false, true,         # 角速度 Yaw 融合 ← 核心！
                       false, false, false]        # 线加速度 -> 忽略

        imu0: demo/imu
        imu0_config: [false, false, false,
                      false, false, false,
                      false, false, false,
                      false, false, true,          # 只融合角速度 Yaw
                      false, false, false]

<gazebo reference="lidar_link">
  <sensor name="lidar" type="ray">
    <ray>
      <scan>
        <horizontal>
          <samples>360</samples>              <!-- 360 个采样点 -> 1 度分辨率 -->
          <min_angle>0.000000</min_angle>
          <max_angle>6.280000</max_angle>     <!-- 360 度全覆盖 -->
        </horizontal>
      </scan>
      <range>
        <min>0.120000</min>                   <!-- 12cm 盲区 -->
        <max>3.5</max>                        <!-- 3.5m 最大距离 -->
      </range>
    </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>

base_link（主体，42cm x 31cm x 18cm，青色）
  |
  +-- base_footprint（地面投影，z=-0.15m）          ← Nav2 避障圆心
  |
  +-- imu_link（z=0.01m，固定）                     ← IMU 数据源坐标
  |
  +-- lidar_link（z=0.12m，360 度扫描）             ← /scan 数据源坐标
  |
  +-- camera_link（前方 0.215m，高 0.05m）          ← 摄像头壳体
  |     |
  |     +-- camera_depth_frame（rotated）           ← /depth_camera/points 数据源坐标
  |
  +-- drivewhl_l_link（左后轮，continuous 旋转）
  |
  +-- drivewhl_r_link（右后轮，continuous 旋转）
  |
  +-- front_caster（前脚轮，fixed，低摩擦）

/map（SLAM 输出）---------> global_costmap
                              坐标系：map
                              更新：1 Hz
                              图层：static_layer（地图）+ obstacle_layer（扫描）+ inflation_layer（膨胀）
                              足迹：圆形（robot_radius=0.3m）
                              用途：全局路径规划
                                   |
                                   v
                              /plan（全局路径）
                                   |
/odom（里程计）-----------> local_costmap
                              坐标系：odom
                              大小：3x3m 滚动窗口
                              更新：5 Hz
                              图层：voxel_layer（3D 体素）+ inflation_layer（膨胀）
                              足迹：矩形 polygon [[0.21,0.195]...]
                              用途：局部避障、轨迹优化
                                   |
                                   v
                         /local_plan（局部轨迹）-> /cmd_vel

map（全局固定坐标系，SLAM 起点）
  ^
  | slam_toolbox 发布 map -> odom（scan-to-map matching 持续纠正漂移）
  |
odom（里程计坐标系，连续平滑但随时间漂移）
  ^
  | ekf_node 发布 odom -> base_footprint（融合 /odom + /imu）
  |
base_footprint（地面投影，避障圆心，z = -(wheel_radius + wheel_zoff) = -0.15m）
  ^
  | robot_state_publisher 发布 base_footprint -> base_link（URDF static joint）
  |
base_link（机器人主体中心，42cm x 31cm x 18cm）
  |
  +-- imu_link（z=0.01m，IMU 传感器坐标，fixed）
  |
  +-- lidar_link（z=0.25m，激光雷达坐标，360 度扫描，fixed）
  |
  +-- camera_link（前方 0.215m，高 0.05m，摄像头壳体，fixed）
  |     |
  |     +-- camera_depth_frame（rotated，深度图/点云坐标系，fixed）
  |
  +-- drivewhl_l_link（左后轮，continuous 旋转）
  |
  +-- drivewhl_r_link（右后轮，continuous 旋转）
  |
  +-- front_caster（前脚轮，fixed，低摩擦）

桌面/nav/
  .gitignore                          # 排除 build/ install/ log/
  day1.md                             # 学习日志
  day2.md
  day3.md
  camera_calibration.yaml             # 相机标定结果（独立实验）
  sam_bot_description/
    CMakeLists.txt                    # 安装规则
    package.xml                       # 依赖声明
    config/
      ekf.yaml                        # EKF 融合配置
      nav2_params.yaml                # Nav2 导航参数（足迹+AMCL+costmap+规划器+控制器）
    launch/
      display.launch.py               # 三终端合并主启动文件
    rviz/
      config.rviz                     # RViz 布局
    world/
      my_world.sdf                    # Gazebo 世界（地面+红色立方体+蓝色球体）
    src/description/
      sam_bot_description.urdf        # 机器人模型（9 links + 3 关节 + 5 Gazebo 插件）
      sam_bot_description.sdf         # SDF 版本（备用）