Linux与MCU数据对接

1.Linux与MCU数据对接

1.1.Data from Linux to MCU

The navigation side mainly issues data related to cmd_vel, primarily including vx, vy, and wz. Here wz is wz_navigation, while the final robot spin speed iswz_final = wz_navigation + wz_spin;(final spin speed = navigation speed + spin speed)

On the Polar Bear Navigation side, the spin speed is determined by the host computer.

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

Here, the final velocity topic from nav2 is named cmd_vel_nav2_result. This node combines it with the spinning velocity to produce cmd_vel, whose wz is wz_final.

Therefore, what we send down to the lower-level controller is vx, vy,wz_final.

关于fake_vel_transform的详细说明请看云台系速度转换与底盘自旋

1.2.Data from MCU to Linux

1.2.1.Overview

Original Polar Bear version (large and small Yaw gimbals)

以下双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

The Polar Bear official navigation README explicitly refers to the joints provided by the real vehicle serial port as:

gimbal_yaw_odom   = 大 yaw
gimbal_yaw        = 小 yaw
gimbal_pitch      = pitch

gimbal_pitch_odom_joint: It can be considered as 0, because in your current description it is already fixed. gimbal_yaw_odom_joint: It can be 0 at the moment of power-on, but it cannot remain fixed at 0 during operation, provided that we have the same "large and small yaw" mechanism as the polar bear. It should update according to the actual angle of the large yaw.

YSU没有大小yaw结构。当前模型把实际yaw放在gimbal_yaw_joint，实际pitch放在gimbal_pitch_joint，两个odom关节固定。下文rel_pitch、rel_yaw分别表示机械相对角relative_pitch、relative_yaw，不是MCU世界IMU姿态。

The official Polar Bear standard_robot_pp_ros2 implementation actually carries two types of MCU→Linux data.

Since the chassis needs to stay in a spinning-top state at all times, let me infer the following:

JointState records 0° → −90° → −180° in order to tell TF: the relative relationship between the chassis and the gimbal is changing.

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

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

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

The first category is IMU data:

yaw, pitch, roll, and then after Linux receives it, publish 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

The second type carries JointState and gimbal reference position data:

This is very important

joint_state

pitch_odom   rad
yaw_odom     rad

After Linux receives it, it does not publish it immediately, but instead saves it:

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,
};

However, if we do not have the large and small yaw structure:

However, to avoid modifying the TF / Nav2 framework behind the polar bear as much as possible (since this angle is used for publishing TF), the most suitable compatibility method is:

真实机械 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

Finally, chassis → gimbal_yaw remains entirely correct.

In summary, what we truly need is:

MCU → Linux

roll       rad
pitch      rad
yaw        rad
joint_yaw  rad
joint_pitch rad

The RPY of the gimbal IMU is used to generate the orientation of /serial/imu.

The angle returned by the gimbal motor is used tojoint_yawandjoint_pitch.

But actually, if your radar is mounted on a yaw gimbal, then joint_pitch does not matter; it does not affect the final navigation, but it may be related to auto-aim.

Units, direction, and zero position can be derived from the current model:

We need to agree with the MCU that it must convert the angle of the gimbal motor:

• When the gimbal faces the forward direction of the chassis, it is 0.
• The range must be normalized, [-π, π) rad that is, [-180°, 180°)
• The rotation direction follows the standard coordinate system, that is, when viewed along the positive direction of an axis, counterclockwise around it is positive.

Then pitch_odom amplitude limiting must be performed:

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

If the mechanical horizontal position is defined as 0, then currently:

• lower: approximately −45°, tilting upward.
• upper: approximately +32.4°, tilting downward. The units are all rad.

This only limits the clamping of Gazebo in ROS2. (The actual limiting should be performed by the MCU based on the mechanical limits.)

Viewsensor_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

The data in thissensor_msgs/msg/JointStateis mainly used together withrobot_descriptionto publish dynamic TF.

For example:

<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

Then let's send it

name = "gimbal_yaw_odom_joint";
position = joint_yaw;

robot_state_publisher It is automatically calculated as:

chassis
   ↓ joint_yaw
gimbal_yaw_odom

and publish the dynamic TF.

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

1.2.2.The ranges and directions of these variables

As for how these angles are actually defined, the key point is not the IMU's world-frame RPY, but the joint's own coordinate axes and zero position.

Here for yaw:

<xyz>0 0 1</xyz>

So yaw is rotation about the parent coordinate system's +Z axis. ROS uses a right-handed coordinate system:

X 前
Y 左
Z 上

Viewing from above the robot:

joint_yaw > 0

Generally, it rotates counterclockwise, that is, turns to the left. Therefore, it is recommended that you define in the lower-level controller: The gimbal faces the chassis X+ forward:

joint_yaw = 0

向左转：
joint_yaw > 0

向右转：
joint_yaw < 0

For example:

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

This is most natural with ROS's right-handed coordinate system.