北極熊導航復現日誌

1.簡介

介紹：https://bbs.robomaster.com/wiki/4577/714072?source=7

原始碼1：https://github.com/SMBU-PolarBear-Robotics-Team/pb2025_sentry_nav 原始碼2：https://github.com/SMBU-PolarBear-Robotics-Team/rmu_gazebo_simulator

Fork：https://github.com/tungchiahui/ysu2026_rm_sentry

2.執行方式

2.1.demo

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

# 导航
ros2 launch pb2025_nav_bringup rm_navigation_simulation_launch.py \
world:=rmuc_2025 \
slam:=False

# 启动仿真
ros2 launch rmu_gazebo_simulator bringup_sim.launch.py

2.2.YSU

克隆程式碼：

git clone https://github.com/tungchiahui/ysu2026_rm_sentry.git

安裝small_gicp:

sudo apt install -y libeigen3-dev libomp-dev

git clone https://github.com/koide3/small_gicp.git
cd small_gicp
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release && make -j
sudo make install

安裝xmacro和vcstool2：

sudo apt install python3-pip python3-vcstool
pip install xmacro

安裝依賴：

cd ./ysu2026_rm_sentry

sudo apt update
rosdep install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y

colcon build --symlink-install

3.功能包

3.1.ysu2026_robot_description

本功能包複製於pb2025_robot_description：

然後把packages.xml和cmakelists.txt裡的pb2025_robot_description全部改成ysu2026_robot_description。

把/src/ysu2026_robot_description/xmacro/pb2025_sentry_robot.sdf.xmacro複製一份為/src/ysu2026_robot_description/xmacro/ysu2026_sentry_robot.sdf.xmacro。

隨後刪掉pb2025_infantry_robot.sdf.xmacro和pb2025_sentry_robot.sdf.xmacro。

3.1.1.擴充套件知識點

3.1.1.1.xmacro

1. 變數基本完全對應

xacro

<xacro:property name="wheel_radius" value="0.076"/>

<cylinder radius="${wheel_radius}" length="0.03"/>

xmacro

<xmacro_define_value name="wheel_radius" value="0.076"/>

<cylinder>
    <radius>${wheel_radius}</radius>
    <length>0.03</length>
</cylinder>

2. 巨集定義也幾乎一一對應

xacro

<xacro:macro name="wheel" params="name radius width">
    <link name="${name}">
        ...
    </link>
</xacro:macro>

呼叫：

<xacro:wheel
    name="front_left_wheel"
    radius="0.076"
    width="0.03"
/>

xmacro

<xmacro_define_block
    name="wheel"
    params="name radius width">

    <link name="${name}">
        ...
    </link>

</xmacro_define_block>

呼叫：

<xmacro_block
    name="wheel"
    name_arg="front_left_wheel"
    radius="0.076"
    width="0.03"
/>

3. include

xacro

<xacro:include filename="wheel.xacro"/>

xmacro

<xmacro_include uri="file://wheel.sdf.xmacro"/>

在當前專案中還可以看到：

<xmacro_include
    uri="model://rm25_example_robot/rm25_example_robot.def.xmacro"
/>

這裡的model://是 sdformat_tools 針對 Gazebo / SDF 做的 URI 解析支援。

4. ${} 表示式

xmacro 和 xacro 都支援類似：

${radius}

以及數學表示式：

${2 * radius}

例如：

<ixx>${m*(y*y+z*z)/12}</ixx>

所以可以在宏引數中進行簡單數學運算。

5. 條件

xacro

<xacro:if value="${use_lidar}">
    ...
</xacro:if>

xmacro

xmacro 中通常可以通過巨集呼叫的 condition 控制：

<xmacro_block
    name="lidar"
    condition="${use_lidar}"
    ...
/>

條件為真時展開這個宏，條件為假時不展開。

6. .def.xmacro

例如：

rm25_example_robot.def.xmacro

這裡的.def並不是 xmacro 的特殊語法，只是一種檔案命名習慣。

通常表示：這個檔案主要負責定義宏。

類似：

xxx.def.xmacro
    ↓
定义机器人结构、link、joint 等宏

然後：

simulation_robot.sdf.xmacro
    ↓
include 前面的 def.xmacro
    ↓
调用其中定义的宏
    ↓
生成完整 SDF

例如以後可以寫：

ysu2026_sentry_robot.def.xmacro

裡面定義：

<xmacro_define_block
    name="ysu2026_sentry_robot"
    params="chassis_height gimbal_yaw_height">

    ...

</xmacro_define_block>

然後在：

simulation_robot.sdf.xmacro

中引入並呼叫：

<xmacro_include
    uri="model://ysu2026_sentry_robot/ysu2026_sentry_robot.def.xmacro"
/>

<xmacro_block
    name="ysu2026_sentry_robot"
    chassis_height="${chassis_height}"
    gimbal_yaw_height="${gimbal_yaw_height}"
/>

記憶：

xmacro_define_value
    ≈ xacro:property

xmacro_define_block
    ≈ xacro:macro

xmacro_block
    ≈ 调用 xacro macro

xmacro_include
    ≈ xacro:include

${...}
    ≈ ${...}

3.1.2.總體框架

3.1.2.1.xmacro介紹

這個xmacro是本專案裡自定義的一種格式，為了實現：

                  ┌→ SDF → Gazebo
.sdf.xmacro ──────┤
                  └→ SDF → URDF → robot_state_publisher

而之前學過的xacro+urdf,這個專案主要是xmacro+sdf。（對比著理解）

然後我們之前接觸的都是

robot.urdf.xacro
      ↓
    xacro
      ↓
robot_description
      ↓
robot_state_publisher
      ↓
TF

而這個專案是：

simulation_robot.sdf.xmacro
          ↓
      xmacro4sdf
          ↓
         SDF
       ↙     ↘
   Gazebo    sdf2urdf
                 ↓
               URDF
                 ↓
      robot_state_publisher
                 ↓
                TF

3.1.2.2.如何先建立自己的功能包和模型包

這個sdf.xmacro會被依賴去釋出模擬裡的模型和實物的tf關係。

這裡是include了下面這個檔案，所以要找這個檔案。

<xmacro_include uri="model://rm25_example_robot/rm25_example_robot.def.xmacro" />

他在/src/rmoss_gz_resources/resource/models/rm25_example_robot裡， 你複製下這個大資料夾，並重命名為ysu2026_sentry_robot，裡面的xmacro也命名為ysu2026_sentry_robot.def.xmacro。

這個檔案裡面需要修改的是：

<!--macro defination: rm25_example_robot-->
<xmacro_define_block
    name="rm25_example_robot"
    params="sticker_type chassis_height gimbal_pitch_height gimbal_yaw_height">

把這裡的name改成

<!--macro definition: ysu2026_sentry_robot-->
<xmacro_define_block
    name="ysu2026_sentry_robot"
    params="sticker_type chassis_height gimbal_pitch_height gimbal_yaw_height">

接下來去修改同目錄的model.sdf.xmacro：

    <!--include defination-->
    <xmacro_include uri="model://rm25_example_robot/rm25_example_robot.def.xmacro" />

    <!--rm25_example_robot model-->
    <model name="rm25_example_robot">
        <pose>0 0 0.15 0 0 0</pose>
        <xmacro_block name="rm25_example_robot" sticker_type="${global_armor_sticker_type}" chassis_height="${chassis_height}" gimbal_yaw_height="${gimbal_yaw_height}" gimbal_pitch_height="${gimbal_pitch_height}"/>

把上面rm25_example_robot全部改成ysu2026_sentry_robot。

    <!--include defination-->
    <xmacro_include uri="model://ysu2026_sentry_robot/ysu2026_sentry_robot.def.xmacro" />

    <!--ysu2026_sentry_robot model-->
    <model name="ysu2026_sentry_robot">
        <pose>0 0 0.15 0 0 0</pose>
        <xmacro_block name="ysu2026_sentry_robot" sticker_type="${global_armor_sticker_type}" chassis_height="${chassis_height}" gimbal_yaw_height="${gimbal_yaw_height}" gimbal_pitch_height="${gimbal_pitch_height}"/>

接下來去修改同目錄的model.config：

<?xml version="1.0"?>
<model>
  <name>rmua19_standard_robot</name>
  <version>1.0</version>
  <sdf version="1.7">model.sdf</sdf>

  <author>
    <name>Lihan Chen</name>
    <email>lihanchen2004@163.com</email>
  </author>

  <description>
    The Basic Version of Standard Robot for RoboMaster University Series.
  </description>
</model>

修改成：

<?xml version="1.0"?>
<model>
  <name>ysu2026_sentry_robot</name>
  <version>1.0</version>
  <sdf version="1.7">model.sdf</sdf>

  <author>
    <name>Tung Chia-hui</name>
    <email>tungchiahui@gmail.com</email>
  </author>

  <description>
    Yanshan University RoboMaster 2026 Sentry Robot.
  </description>
</model>

這裡的model.sdf不需要手動改，這玩意是自動生成的：

model.sdf.xmacro
      ↓
xmacro4sdf
      ↓
model.sdf

你可以看看model.sdf開頭，明確說了這件事，而且不建議你直接修改：

<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xmacro4sdf from model.sdf.xmacro          | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->

所以以後真正維護的是：

关于模型：
ysu2026_sentry_robot.def.xmacro
model.sdf.xmacro

关于（这个不改问题也不大）：
model.config

回到/src/ysu2026_robot_description/resource/xmacro/ysu2026_sentry_robot.sdf.xmacro：

    <!--include defination-->
    <xmacro_include uri="model://rm25_example_robot/rm25_example_robot.def.xmacro" />

    <!--pb2025_sentry_robot model-->
    <model name="pb2025_sentry_robot">

        <pose>0 0 0.15 0 0 0</pose>
        <xmacro_block name="rm25_example_robot" sticker_type="${global_armor_sticker_type}" chassis_height="${chassis_height}" gimbal_yaw_height="${gimbal_yaw_height}" gimbal_pitch_height="${gimbal_pitch_height}"/>

把裡面的rm25_example_robot和pb2025_sentry_robot全改成ysu2026_sentry_robot：

    <!--include defination-->
    <xmacro_include uri="model://ysu2026_sentry_robot/ysu2026_sentry_robot.def.xmacro" />

    <!--ysu2026_sentry_robot model-->
    <model name="ysu2026_sentry_robot">

        <pose>0 0 0.15 0 0 0</pose>
        <xmacro_block name="ysu2026_sentry_robot" sticker_type="${global_armor_sticker_type}" chassis_height="${chassis_height}" gimbal_yaw_height="${gimbal_yaw_height}" gimbal_pitch_height="${gimbal_pitch_height}"/>

至此上面這些在reality環境下已經正常了，但simulation還不行，開啟同目錄下的simulation_robot.sdf.xmacro：

    <!--include defination-->
    <xmacro_include uri="model://rm25_example_robot/rm25_example_robot.def.xmacro" />

    <!--simulation_robot model-->
    <model name="simulation_robot">

        <pose>0 0 0.15 0 0 0</pose>
        <xmacro_block name="rm25_example_robot" sticker_type="${global_armor_sticker_type}" chassis_height="${chassis_height}" gimbal_yaw_height="${gimbal_yaw_height}" gimbal_pitch_height="${gimbal_pitch_height}"/>

改成：

    <!--include defination-->
    <xmacro_include uri="model://ysu2026_sentry_robot/ysu2026_sentry_robot.def.xmacro" />

    <!--simulation_robot model-->
    <model name="simulation_robot">

        <pose>0 0 0.15 0 0 0</pose>
        <xmacro_block name="ysu2026_sentry_robot" sticker_type="${global_armor_sticker_type}" chassis_height="${chassis_height}" gimbal_yaw_height="${gimbal_yaw_height}" gimbal_pitch_height="${gimbal_pitch_height}"/>

至此，你就修改完了。

grep -R "rm25_example_robot" src/ysu2026_robot_description src/rmoss_gz_resources/resource/models/ysu2026_sentry_robot

grep -R "pb2025_sentry_robot" src/ysu2026_robot_description src/rmoss_gz_resources/resource/models/ysu2026_sentry_robot

正常應該是：

root@Desktop-Fedora:/home/tungchiahui/UserFolder/MySource/ysu2026_rm_sentry$ grep -R "rm25_example_robot" src/ysu2026_robot_description src/rmoss_gz_resources/resource/models/ysu2026_sentry_robot
src/rmoss_gz_resources/resource/models/ysu2026_sentry_robot/model.sdf:  <!--rm25_example_robot model-->
src/rmoss_gz_resources/resource/models/ysu2026_sentry_robot/model.sdf:  <model name="rm25_example_robot">
root@Desktop-Fedora:/home/tungchiahui/UserFolder/MySource/ysu2026_rm_sentry$ grep -R "pb2025_sentry_robot" src/ysu2026_robot_description src/rmoss_gz_resources/resource/models/ysu2026_sentry_robot

只有model.sdf裡還有殘留：

找到這個指令碼：/src/rmoss_gz_resources/update_model.sh

#!/bin/bash
path=`pwd`
for dir in $path/models/*; do
    echo "parse "$dir
    cd $dir
    tempfile=`mktemp temp.XXXXXX`
    xmacro4sdf model.sdf.xmacro > $tempfile
    lines_num=`cat $tempfile | wc -l`
    if (($lines_num > 3 )) ;then
        cat $tempfile > model.sdf
    else
        cat $tempfile
    fi
    rm -f temp.*
done

修改為：

#!/bin/bash

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
MODEL_DIR="$SCRIPT_DIR/resource/models"

# 让 xmacro4sdf 能解析 model://xxx/...
export IGN_GAZEBO_RESOURCE_PATH="$MODEL_DIR:${IGN_GAZEBO_RESOURCE_PATH:-}"
export GAZEBO_MODEL_PATH="$MODEL_DIR:${GAZEBO_MODEL_PATH:-}"

for dir in "$MODEL_DIR"/*; do
    [ -d "$dir" ] || continue
    [ -f "$dir/model.sdf.xmacro" ] || continue

    echo "parse $dir"

    cd "$dir" || exit 1

    tempfile=$(mktemp temp.XXXXXX)

    if xmacro4sdf model.sdf.xmacro > "$tempfile"; then
        lines_num=$(wc -l < "$tempfile")

        if (( lines_num > 3 )); then
            cat "$tempfile" > model.sdf
            echo "generated: $dir/model.sdf"
        else
            echo "ERROR: generated content too short"
            cat "$tempfile"
        fi
    else
        echo "ERROR: xmacro4sdf failed: $dir"
    fi

    rm -f "$tempfile"
done

執行：

chmod +x ./src/rmoss_gz_resources/update_model.sh
./src/rmoss_gz_resources/update_model.sh

生成完後，返回ws目錄：

grep -R "rm25_example_robot" src/ysu2026_robot_description src/rmoss_gz_resources/resource/models/ysu2026_sentry_robot

會發現也沒有輸出了。

3.1.2.3.修改機器人引數

主要是機器人的底盤長寬、輪距/軸距、輪徑、雲臺高度、Mid360 安裝位置：

1. 底盘长宽高 / collision
2. 四个轮子的位置
3. 轮子半径
4. chassis_height
5. gimbal_yaw_height
6. gimbal_pitch_height
7. Mid360 的 parent 和 pose

首先你得知道ROS預設規定的方向：

+x：车头方向
+y：车体左侧
+z：竖直向上

而且北極熊現在的模型裡，base_footprint 在地面，chassis 原點基本位於四個輪子的輪軸中心高度。

開啟/src/ysu2026_robot_description/resource/xmacro/ysu2026_sentry_robot.sdf.xmacro

主要修改下面幾個：

1. 底盤長度和底盤寬度

底盤長寬和軸距/輪距不是一個東西

比如現在北極熊：

        <!--chassis-->
        <link name="chassis">
            <inertial>
                <pose>0 0 0.055 0 0 0</pose>
                <xmacro_block name="inertia_box" m="15.0" x="0.36" y="0.24" z="0.13"/>

底盘 collision:
长度 x = 0.36
宽度 y = 0.24

但：

轴距 = 0.414
轮距 = 0.388

所以以後你可能得到：

底盘主体：
长 360 mm
宽 300 mm

轮中心：
轴距 420 mm
轮距 390 mm

2. 底盤高度

這個引數如其字面意思一樣，就是底盤的厚度，也就是底盤的上頂面到下底面的距離。

3. 軸距和輪距也要量“輪中心”，不是底盤外殼

北極熊的是：

找到/src/rmoss_gz_resources/resource/models/ysu2026_sentry_robot：

        <!--front_left wheel link and joint-->
        <xmacro_block name="chassis_wheel_l" prefix="front_left" parent="chassis" pose="0.207 0.194 0 0 0 0"/>
        <!--front_right wheel link and joint-->
        <xmacro_block name="chassis_wheel_r" prefix="front_right" parent="chassis" pose="0.207 -0.194 0 0 0 0"/>
        <!--rear_left wheel link and joint-->
        <xmacro_block name="chassis_wheel_r" prefix="rear_left" parent="chassis" pose="-0.207 0.194 0 0 0 0"/>
        <!--rear_right wheel link and joint-->
        <xmacro_block name="chassis_wheel_l" prefix="rear_right" parent="chassis" pose="-0.207 -0.194 0 0 0 0"/>

FL:  0.207   0.194
FR:  0.207  -0.194
RL: -0.207   0.194
RR: -0.207  -0.194

那麼軸距是 = 0.207 - (-0.207) = 0.414 m

那麼輪距是 = 0.194 - (-0.194) = 0.388 m

示意圖：

                 +X 前

       FL ●────────────● FR
          │            │
          │            │
          │            │
          │            │
       RL ●────────────● RR

          ←  轮距  →

前后两个轮轴中心的距离
        = 轴距

所以要測量的資料是： 前左輪中心 到 後左輪中心：X 方向距離 左輪中心面 到 右輪中心面：Y 方向距離 不是量輪胎最外側。

2. 輪徑

這是輪子的直徑，不是半徑，注意下。

3. 底盤距離水平地面的高度chassis_height：

    <!-- 底盘距离水平地面的高度 -->
    <xmacro_define_value name="chassis_height" value="0.077" />
    <!-- gimbal_yaw 相对于底盘的 z 轴安装偏移 -->
    <xmacro_define_value name="gimbal_yaw_height" value="0.026" />
    <!-- gimbal_pitch 相对于 gimbal_yaw 的高度差 -->
    <xmacro_define_value name="gimbal_pitch_height" value="0.355" />

這裡的幾個引數，這裡的chassis_height所描述的是：

<joint name="base_to_chassis" type="fixed">
    <pose>0 0 ${chassis_height} 0 0 0</pose>
    <parent>base_footprint</parent>
    <child>chassis</child>
</joint>

由於四個輪子的 joint 都是：

parent="chassis"
pose="... ... 0 0 0 0"

所以結構其實是：

          chassis 原点
               ●──────── 轮轴中心（一般来讲是底盘原点，正好这个是轮轴中心重合了）
               │
               │ chassis_height
               │
地面 ──────────●──────────
          base_footprint

2. 雲臺高度gimbal_yaw_height

<pose relative_to='chassis'>
    0 0 ${gimbal_yaw_height} 0 0 0
</pose>

他描述的是

chassis 坐标系原点
        │
        │ gimbal_yaw_height
        │
        ▼
云台 yaw 旋转轴原点

也就是chassis 座標系原點到 yaw 旋轉軸中心的 Z 向距離。

如果雲臺相對於底盤有前後偏移，那麼不能只填gimbal_yaw_height，應該填完整的pose。

2. 雲臺高度gimbal_pitch_height

也就是 yaw 旋轉軸中心 → pitch 旋轉軸中心的垂直距離。

                    ● pitch 旋转轴
                    │
                    │ gimbal_pitch_height
                    │
                    ● yaw 旋转轴
                    │
                    │ gimbal_yaw_height
                    │
                    ● chassis 原点
                    │
                    │ chassis_height
                    │
────────────────────● 地面

3. mid360

   1. mid360的原點：

   大疆官方資料

   規定的方向：

   

   

   如上圖可知，位於雷射雷達的中心軸上，並距離底面47mm處。

   也就是

   底部安装面中心
       │
       │ +47 mm
       ↓
      O 点

   先量 chassis / 雲臺座標原點 到 Mid360 底部安裝面中心的位置，然後 Z 再補這 47 mm，就能得到 chassis / 雲臺 → Mid360 官方 O 點的位置。

   官方說明：

   官方手冊明確規定：Mid-360 的 O-XYZ 是點雲座標系，其中 O 是點雲原點。

   

   而內建 IMU 晶片相對於這個點雲座標系的位置是：

   x = +11.00 mm
   y = +23.29 mm
   z = -44.12 mm

   

   front_mid360 = LiDAR 点云 O 点
           ●
           │
           │  (+11, +23.29, -44.12) mm
           │
           ● IMU 芯片原点

   這裡先提前說明下``front_mid360`就是雷達的O_XYZ原點，具體怎麼推出來的，請往下看。

   2. mid360的pose

   <!--livox-->
   <xmacro_block
       name="livox"
       prefix="front_"
       parent="chassis"
       pose="0.16 0.0 0.18 ${pi/6} 0.0 ${pi/2}"
       ...
   />

   • 這個 pose 是 ： chassis 座標系 → front_mid360 座標系
   • 按北極熊這個來，是嚴格相對於 chassis 原點。
   • 他的雷達是裝在底盤上的，所以parent="chassis"，如果雷達要裝在雲臺，就改成parent="gimbal_yaw"，那麼偏移也會變成相對於gimbal_yaw 座標系的偏移。.

   x = +0.16 m
   y =  0
   z = +0.18 m

   意思是 Mid360 O原點相對於 chassis 原點：

   向前 160 mm
   左右不偏
   向上 180 mm

   他這裡pose的後三個引數是RPY,也就是相對於chassis 原點的偏移：

   roll  = π/6 = 30°
   pitch = 0°
   yaw   = π/2 = 90°

   另外你在 Mid360 model 裡還會看到：

   <visual>
       <pose>0 0 0 0 0 ${pi}</pose>

   這個只是外觀 mesh 轉了 180°，不等於雷達感測器 frame 轉了 180°。不要拿這個來算 TF。

   3. mid360到底量它外殼哪個點？

   這裡還要再精確一層。 北極熊當前 Mid360 模型內部是：

   <joint ...>
       <child>${prefix}mid360</child>
   </joint>
   
   <link name="${prefix}mid360">

   這裡的${prefix}mid360會被解析為front_mid360：

   這個front_mid360一直很懵逼，他到底是什麼？ 他在readme裡寫的模擬裡是簡化的，所以不要在模擬裡反推這個座標系的原點到底是什麼？ 在reality的Point LIO裡你可以看到extrinsic_T: [-0.011, -0.02329, 0.04412]，這正是官方IMU的偏移。 而frame_id = front_mid360，所以說，反推出來front_mid360是我們點雲的座標系，也就是O_XYZ.

   而北極熊的 LiDAR 光學/掃描感測器實際上還額外：

   <sensor name="${prefix}mid360_lidar" ...>
   <pose>0 0 0.03 0 0 0</pose>

   front_mid360 link 原点
           │
           │ +30 mm
           ▼
   LiDAR 扫描原点

   而 IMU：

   <sensor name="${prefix}mid360_imu" type="imu">

   沒有額外 pose，所以當前模擬裡基本相當於：IMU 原點 = front_mid360 原點

   4. mid360最後各個資料究竟填什麼？

   front_mid360為mid360的O_XYZ的原點。

   那麼Mid360官方點雲原點就和O_XYZ原點重合。 那麼Mid360官方IMU原點就和O_XYZ有(+11, +23.29, -44.12)mm的偏移量。（但這裡沒必要給pose,在point_lio裡給pose即可）

   所以

   <!-- chassis -> Mid360 官方点云 O -->
   <xmacro_block
       name="livox"
       prefix="front_"
       parent="chassis"
       pose="实际 x 实际 y 实际 z 实际 roll 实际 pitch 实际 yaw"
   />

   然後 Mid360 模擬內部：

   <sensor name="${prefix}mid360_lidar" type="gpu_lidar">
   <pose>0 0 0 0 0 0</pose>

   而IMU這邊，北極熊的方案我看懂了，雖然他讓IMU座標系與點雲座標系直接重合，並且模擬裡也是重合的。 而在reality裡，他是extrinsic_T: [-0.011, -0.02329, 0.04412]，這裡明顯是已經考慮到了這個偏移，所以我們沒必要修改IMU的座標系偏移。

綜上，你應該需要的資料是：

1. 轮子实际直径

2. 前后轮中心轴距
   前轮中心 → 后轮中心

3. 左右轮中心轮距
   左轮中心面 → 右轮中心面

4. chassis 刚性主体：
   X 方向长度
   Y 方向宽度
   Z 方向高度

5. chassis/轮轴中心 → 云台 yaw 旋转轴中心：
   Δx
   Δy
   Δz

   如果云台不在底盘正中央，x/y 也得知道。

6. yaw 旋转轴中心 → 云台 pitch 旋转轴中心：
   Δx
   Δy
   Δz

7. Mid360 如果挂 chassis：
   chassis 原点 → Mid360 reference frame（mid360的原点：距离底面中心高47mm处）
   Δx Δy Δz
   roll pitch yaw

   如果挂 gimbal_yaw：
   yaw 轴原点 → Mid360 reference frame（mid360的原点：距离底面中心高47mm处）
   Δx Δy Δz
   roll pitch yaw

3.2.把各種功能包裡的launch全部修改了

修改launch重定向到咱們的模型，主要先看下面的launch：

• pb2025_nav_bringup裡的rm_navigation_simulation_launch.py

  • 同功能包下的robot_state_publisher_launch.py

• rmu_gazebo_simulator裡的bringup_sim.launch.py

  • 同功能包下的spawn_robots.launch.py
  • 同功能包下的package.xml

• ysu2026_robot_description裡的robot_description_launch.py

3.2.1.robot_state_publisher_launch.py：

    pkg_pb2025_robot_description_dir = get_package_share_directory(
        "pb2025_robot_description"
    )

    declare_robot_name_cmd = DeclareLaunchArgument(
        "robot_name",
        default_value="pb2025_sentry_robot",
        description="The file name of the robot xmacro to be used",
    )

改成

    pkg_robot_description_dir = get_package_share_directory(
        "ysu2026_robot_description"
    )

    declare_robot_name_cmd = DeclareLaunchArgument(
        "robot_name",
        default_value="ysu2026_sentry_robot",
        description="The file name of the robot xmacro to be used",
    )

3.2.2.spawn_robots.launch.py：

    pkg_pb2025_robot_description = get_package_share_directory(
        "pb2025_robot_description"
    )

    robot_xmacro_path = os.path.join(
        pkg_pb2025_robot_description,
        "resource",
        "xmacro",
        "simulation_robot.sdf.xmacro",
    )

改成：

    pkg_robot_description = get_package_share_directory(
        "ysu2026_robot_description"
    )

    robot_xmacro_path = os.path.join(
        pkg_robot_description,
        "resource",
        "xmacro",
        "simulation_robot.sdf.xmacro",
    )

3.2.3.robot_description_launch.py:

def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory("pb2025_robot_description")

改成

def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory("ysu2026_robot_description")

3.2.4.rmu_gazebo_simulator裡的package.xml

<depend>pb2025_robot_description</depend>

改成

<depend>ysu2026_robot_description</depend>

3.2.5.測試

rm -rf build install log
colcon build --symlink-install

source install/setup.bash
ros2 launch rmu_gazebo_simulator bringup_sim.launch.py

source install/setup.bash

ros2 launch pb2025_nav_bringup rm_navigation_simulation_launch.py \
  world:=rmuc_2025 \
  slam:=False

測試下Gazebo載入模型還正常不，如果正常則說明沒毛病。

至此全部結束。

3.3.ysu2026_robot_description修改掉mid360的位置

北極熊的雷達是放在底盤上的，我們準備放到Yaw雲臺上。

        <!--livox-->
        <xmacro_block name="livox" prefix="front_" parent="chassis" pose="0.0496 0.0936 0.435 -${pi/12} 0 0" update_rate="20" samples="1875"/>

這代表：

chassis
└── front_mid360

只需要改掉parent="chassis"即可。

我們想要的TF鏈是：

chassis
  │
  │ gimbal_yaw_odom_joint = joint_yaw
  ▼
gimbal_yaw_odom
  │
  │ fixed
  ▼
gimbal_pitch_odom
  │
  │ gimbal_yaw_joint = 0
  ▼
gimbal_yaw
  │
  │ gimbal_pitch_joint = joint_pitch
  ▼
gimbal_pitch

如果沒有什麼大Yaw小Yaw雲臺，那麼掛在gimbal_yaw_odom和gimbal_yaw沒有任何區別，都不會影響導航。

但是，由於沒有什麼大Yaw小Yaw雲臺，然後還要根據robot_description釋出靜態TF，所以肯定是掛在gimbal_yaw_odom更加合理。 （但如果gimbal_yaw_odom和gimbal_yaw重合，那其實掛在哪都一樣）

chassis
  │
  │ joint_yaw
  ▼
gimbal_yaw_odom
  ├── front_mid360
  │
  └── ...

        <!--livox-->
        <xmacro_block name="livox" prefix="front_" parent="gimbal_yaw_odom" pose="0.0496 0.0936 0.435 -${pi/12} 0 0" update_rate="20" samples="1875"/>

3.4.Serial_Comm

https://github.com/tungchiahui/Serial_Pack

可以參考序列埠驅動與序列埠包協議

3.5.Joy

可以參考Joy

然後搞兩個launch.py。

一個joy_controller.launch.py是用來發/cmd_vel的。（實車用的是這個）

一個joy_controller_sim.launch.py是用來發/red_standard_robot1/cmd_vel的。（模擬用的是這個）

3.6.MCU到Linux的資料

3.6.1.總概

chassis
  │
  ├─ gimbal_yaw_odom_joint      绕 Z，可动
  ▼
gimbal_yaw_odom
  │
  ├─ gimbal_pitch_odom_joint    fixed
  ▼
gimbal_pitch_odom
  │
  ├─ gimbal_yaw_joint           绕 Z，可动
  ▼
gimbal_yaw
  │
  ├─ gimbal_pitch_joint         绕 Y，可动
  ▼
gimbal_pitch

北極熊官方導航 README 明確把實車序列埠提供的關節稱為：

gimbal_yaw_odom   = 大 yaw
gimbal_yaw        = 小 yaw
gimbal_pitch      = pitch

gimbal_pitch_odom_joint：可以認為就是 0，因為你現在的 description 裡它本來就是 fixed。 gimbal_yaw_odom_joint：上電那一刻可以是 0，但執行時不能固定為 0，前提是咱們有北極熊同樣的“大 yaw”機構。它應該跟著大 yaw 的真實角度更新。

北極熊官方 standard_robot_pp_ros2 實際上有兩類 MCU→Linux 資料。

第一類是 IMU 資料：

yaw，pitch，roll然後Linux 收到以後，釋出 sensor_msgs/msg/Imu。

但其實這個資料沒啥用，北極熊並沒在導航裡用到roll,rpy_vel這些資料。

imu_state

roll       rad
pitch      rad
yaw        rad

roll_vel   rad/s
pitch_vel  rad/s
yaw_vel    rad/s

然後 pitch → gimbal_pitch_joint yaw → gimbal_yaw_joint

第二類是 JointState/雲臺參考位置資料：

這個很重要

joint_state

pitch_odom   rad
yaw_odom     rad

Linux 收到以後並不馬上釋出，而是儲存：

last_gimbal_pitch_odom_joint_ = pitch_odom;
last_gimbal_yaw_odom_joint_   = yaw_odom;

下一次 IMU 資料到來時，北極熊一次釋出完整四項：

joint_msg.name = {
    "gimbal_pitch_joint",
    "gimbal_yaw_joint",
    "gimbal_pitch_odom_joint",
    "gimbal_yaw_odom_joint",
};

joint_msg.position = {
    imu_pitch,
    imu_yaw,
    pitch_odom,
    yaw_odom,
};

但是咱們沒有大小yaw結構的話：

但為了儘量不改北極熊後面的 TF / Nav2 框架，最合適的相容方法是：

你们真实 yaw
    ↓
gimbal_yaw_odom_joint

不存在的小 yaw
    ↓
gimbal_yaw_joint = 0

你们真实 pitch
    ↓
gimbal_pitch_joint

chassis
  │
  │ gimbal_yaw_odom_joint = 实际 yaw（大yaw）
  ▼
gimbal_yaw_odom
  │
  │ gimbal_pitch_odom_joint = fixed
  ▼
gimbal_pitch_odom
  │
  │ gimbal_yaw_joint = 0 (不存在小yaw)
  ▼
gimbal_yaw
  │
  │ gimbal_pitch_joint = 实际 pitch
  ▼
gimbal_pitch

      msg_jointstate.name = {
          "gimbal_pitch_joint",      // 小pitch
          "gimbal_yaw_joint",        //小yaw
          "gimbal_pitch_odom_joint",   //fixed
          "gimbal_yaw_odom_joint"    //大yaw
      };

      msg_jointstate.position = {
                      pitch,   // 真实 pitch
                      0.0,     // 不存在的小 yaw
                      0.0,     // fixed
                      yaw      // 真实 yaw
      };

chassis
  │
  │ gimbal_yaw_odom_joint   ← 大 yaw，动态
  ▼
gimbal_yaw_odom
  │
  │ gimbal_pitch_odom_joint ← fixed
  ▼
gimbal_pitch_odom
  │
  │ gimbal_yaw_joint        ← 小 yaw，动态
  ▼
gimbal_yaw
  │
  │ gimbal_pitch_joint      ← pitch，动态
  ▼
gimbal_pitch

最後 chassis → gimbal_yaw 依然完全正確。

綜上，咱們正真需要的是：

MCU → Linux

roll       rad
pitch      rad
yaw        rad
joint_yaw  rad
joint_pitch rad

其中雲臺 IMU 的 RPY 用來生成 /serial/imu 的 orientation。

雲臺電機返回的角度用來joint_yaw和joint_pitch。

但其實，如果你的雷達放在Yaw的雲臺上，其實joint_pitch無所謂，不影響最終的導航，但可能和自瞄有關係。

檢視sensor_msgs/msg/JointState：

ros2 interface show sensor_msgs/msg/JointState

# This is a message that holds data to describe the state of a set of torque controlled joints.
#
# The state of each joint (revolute or prismatic) is defined by:
#  * the position of the joint (rad or m),
#  * the velocity of the joint (rad/s or m/s) and
#  * the effort that is applied in the joint (Nm or N).
#
# Each joint is uniquely identified by its name
# The header specifies the time at which the joint states were recorded. All the joint states
# in one message have to be recorded at the same time.
#
# This message consists of a multiple arrays, one for each part of the joint state.
# The goal is to make each of the fields optional. When e.g. your joints have no
# effort associated with them, you can leave the effort array empty.
#
# All arrays in this message should have the same size, or be empty.
# This is the only way to uniquely associate the joint name with the correct
# states.

std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

string[] name
float64[] position
float64[] velocity
float64[] effort