Linux与MCU数据对接

Linux与MCU数据对接

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的详细说明请看云台系速度转换与底盘自旋

MCU到Linux的数据

总概

北极熊原版本（大小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**

这些变量的范围和方向

至于这些角到底怎么定义，重点不是 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 的右手系最自然。