Linux與MCU數據對接

1.Linux與MCU數據對接

1.1.Linux到MCU的數據

導航這邊主要是下發cmd_vel相關的數據,主要包含vx,vy,wz。這裏的wz是wz_navigation，而最終機械人自旋速度是wz_final = wz_navigation + wz_spin;（最終自旋速度 = 導航速度 + 自旋速度）

而北極熊導航這邊是由上位機決定的自旋速度

fake_vel_transform:
  ros__parameters:
    input_cmd_vel_topic: "cmd_vel_nav2_result"
    output_cmd_vel_topic: "cmd_vel"
    cmd_spin_topic: "cmd_spin"
    init_spin_speed: 3.14

這裏表示，他把nav2最後給的速度話題設置成了cmd_vel_nav2_result，而經過這個節點融合了自旋速度最後生成了cmd_vel，裏面的wz就是wz_final。

所以我們給下位機下發的是vx,vy,wz_final。

關於fake_vel_transform的詳細説明請看雲台系速度轉換與底盤自旋

1.2.MCU到Linux的數據

1.2.1.總概

北極熊原版本（大小Yaw）

以下雙yaw結構作為原方案背景保留；YSU當前只有一個實際yaw，填寫方式見下面的YSU表格。

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 的真實角度更新。

YSU沒有大小yaw結構。當前模型把實際yaw放在gimbal_yaw_joint，實際pitch放在gimbal_pitch_joint，兩個odom關節固定。下文rel_pitch、rel_yaw分別表示機械相對角relative_pitch、relative_yaw，不是MCU世界IMU姿態。

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

因為底盤要一直小陀螺狀態，所以推斷一下：

而 JointState 記錄 0° → −90° → −180°，是為了告訴 TF：底盤與雲台的相對關係正在變化。

导航/串口 vx、vy：按云台方向表达
串口 wz：最终底盘转速

JointState yaw：云台相对底盘的机械角
JointState pitch：实际 pitch 的机械相对角

云台 IMU RPY：单独保留世界/惯性姿态语义

第一類是 IMU 數據：

yaw，pitch，roll然後Linux 收到以後，發佈 sensor_msgs/msg/Imu。

當前Point-LIO使用MID360的livox/imu，並不直接讀取這段MCU姿態。MCU姿態仍應保留明確的世界／慣性參考系含義，不能直接當作機械關節角。

imu_state

roll       rad
pitch      rad
yaw        rad

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

下面是原串口示例的字段連接記法，是否屬於機械相對角需按原驅動核對；YSU當前JointState仍按上表使用rel_pitch、rel_yaw：

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;

以下保留原串口示例的四字段組織方式；字段語義需按原驅動核對，它不是YSU當前的填寫方案。世界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 （因為這是為了發佈TF而用的角度）框架，最合適的兼容方法是：

真实机械 yaw   → gimbal_yaw_joint
真实机械 pitch → gimbal_pitch_joint
两个 odom 关节 → fixed，position填0

JointState按pitch、yaw、pitch_odom、yaw_odom排序，position為{relative_pitch, relative_yaw, 0, 0}。機械零位、正方向和弧度單位需要上實物核對，世界IMU姿態另按其參考系處理。

chassis
  │
  │ gimbal_yaw_odom_joint = fixed
  ▼
gimbal_yaw_odom
  │
  │ gimbal_pitch_odom_joint = fixed
  ▼
gimbal_pitch_odom
  │
  │ gimbal_yaw_joint = 唯一实际 yaw
  ▼
gimbal_yaw
  │
  │ gimbal_pitch_joint = 实际 pitch
  ▼
gimbal_pitch

      msg_jointstate.name = {
          "gimbal_pitch_joint",      // 实际pitch
          "gimbal_yaw_joint",        // 唯一实际yaw
          "gimbal_pitch_odom_joint",   // 固定层
          "gimbal_yaw_odom_joint"    // 固定层
      };

      msg_jointstate.position = {
                static_cast<fp64>(rel_pitch),  // 唯一机械 pitch
                static_cast<fp64>(rel_yaw),    // 唯一机械 yaw
                0.0,                          // 固定层
                0.0                           // 固定层
      };

chassis
  │
  │ gimbal_yaw_odom_joint   ← fixed，固定层
  ▼
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無所謂，不影響最終的導航，但可能和自瞄有關係。

單位、方向和零位，可以從當前模型推出：

咱們需要跟MCU規定好，他需要把雲台電機的角度進行轉換：

• 雲台朝底盤正方向時為0
• 範圍必須歸一化，[-π, π) rad，也就是 [-180°, 180°)
• 旋轉方向按照標準座標系，也就是在某個軸的正方向看，繞他的逆時針為正。

然後pitch_odom要進行限幅：

<joint name="gimbal_pitch_joint" type="revolute">
    ...
    <axis>
        <xyz>0 1 0</xyz>
        <limit>
            <effort>100</effort>
            <lower>${-3.14*0.25}</lower>
            <upper>${3.14*0.18}</upper>
            <velocity>50</velocity>
        </limit>
    </axis>
</joint>

如果規定機械水平位是0的話，目前：

• lower：約 −45°，向上仰。
• upper：約 +32.4°，向下俯。 單位都是 rad。

這裏只是會限制ROS2裏Gazebo的限幅。（真正的限幅應該讓MCU端根據機械限位進行限位）

查看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

這個sensor_msgs/msg/JointState裏的數據主要用於搭配robot_description來發布動態TF。

比如：

<joint name="gimbal_yaw_odom_joint" type="revolute">
    <parent>chassis</parent>
    <child>gimbal_yaw_odom</child>
    <axis>
        <xyz>0 0 1</xyz>
    </axis>
</joint>

父坐标系：chassis
子坐标系：gimbal_yaw_odom
转轴：+Z

然後咱們發

name = "gimbal_yaw_odom_joint";
position = joint_yaw;

robot_state_publisher 就自動算：

chassis
   ↓ joint_yaw
gimbal_yaw_odom

併發布動態 TF。

STM32
├── joint_yaw
├── joint_pitch
│
│
▼
serial_comm
│
└── /serial/gimbal_joint_state
          ↓
joint_state_publisher
          ↓
     /joint_states
          ↓
robot_state_publisher
          ↓
        /**tf**

1.2.2.這些變量的範圍和方向

至於這些角到底怎麼定義，重點不是 IMU 的世界系 RPY，而是 joint 自己的座標軸和零位。

yaw這裏：

<xyz>0 0 1</xyz>

所以 yaw 是繞父座標系的 +Z 轉。 ROS 採用右手系：

X 前
Y 左
Z 上

從機械人上方往下看：

joint_yaw > 0

一般就是逆時針轉，也就是向左轉。 所以建議你下位機定義： 雲台正對底盤 X+ 前方：

joint_yaw = 0

向左转：
joint_yaw > 0

向右转：
joint_yaw < 0

例如：

正前方      0 rad
左 90°      +π/2
右 90°      -π/2
后方        ±π

這個和 ROS 的右手系最自然。