机器人移动控制升级：从按时间盲走到按里程计精走

Interaction 模块里程计（Odom）集成完整总结

分支：feat/adapt_odom

核心改动：将机器人移动从"按时间盲走"升级为"按里程计精走"

1. 背景与目的

之前的问题：按时间盲走

机器人所有移动（前进、后退、横移、转向）都是按固定时间控制的：

cpp 复制代码

// 时间 = 距离 ÷ 速度 × 折扣系数
duration = 1.0 / 0.25 × 1000 × 0.6 = 2400ms
// 实际走：0.25 × 2.4 = 0.6 米

这带来了三个致命问题：

1. 精度差。 理论上走 0.6 米（被 duration_cap_factor: 0.6 打了折），但 MC 实际速度不一定到 0.25m/s。实测 T1 实际速度只有 ~0.17m/s，走 1 米命令实际只挪了 0.7 米。

2. 没有反馈。 时间到了就停------不管机器人是被堵墙了、打滑了、还是路不平走得慢。没有任何机制知道"到底走了多远"。

3. 转动也不精确。 左转 45° 靠纯时间------角速度 × 时间 = 角度。如果地面摩擦力不同、电机响应延迟，实际角度跟命令偏差大。

解决方案：里程计为主，时间兜底

里程计（odometry）通过 IMU 和关节编码器实时计算机器人的位置（x, y）和朝向（yaw）。/odom 话题以 100Hz 频率发布这些数据。

Interaction 模块订阅 /odom，在每次移动时：

复制代码

直线移动：每 20ms 读 odom x/y → 算走了多远 → 走够了停
转动：每 20ms 读 odom yaw → 累积转角 → 转够了停
时间只在 odom 异常时兜底（超时强制停，防止无限走）

对比

	之前	现在
控制方式	时间为主，odom 辅助提前停	odom 为主，时间兜底
前进 1 米	固定走 4 秒（或 0.6 折扣），不管到没到	走到 odom 说 1 米为止
左转 45°	固定转 1.5 秒，不管转到没	转到 odom 说 45° 为止
精度	依赖 MC 速度准确度	闭环控制，自适应
堵墙/打滑	时间到停，不管走了多少	走到目标才停，走不到超时报警

适用范围

T1 机器人：全部移动类型启用里程计控制（直线 + 转动 + 唤醒转向）
Q1 机器人：保持不变，纯时间控制（Q1 有自己的 move_task_description，不调 SetDistance/SetAngularTargetDeg）

2. 里程计是什么

通俗理解

里程计就是机器人的"计步器 + 指南针"------手机上地图那个蓝色箭头怎么知道面朝哪，机器人就怎么知道。

闭上眼睛走路，你也能大概感觉到自己走了多远、面朝哪------因为你的脚、身体、前庭系统在不断感知。机器人的 IMU 和关节编码器就是它的"感觉器官"。

物理传感器

里程计依赖两类物理传感器：

传感器	装在哪	测什么	怎么算
IMU（陀螺仪）	机器人身体内	角速度（每秒转多少弧度）	角速度×时间积分 → 转了多大角度 → yaw 朝向
关节编码器	每条腿的关节处	电机转了多少角度	腿的运动学推算 → 身体移动了多少 → 位置 x, y

数据流程

复制代码

         ┌─────────────────────────────────────┐
         │         legged_odom 模块              │
         │                                      │
IMU ────→│  陀螺仪角速度 → 积分 → 朝向 yaw      │
         │                                      │──→ /odom (100Hz)
编码器──→│  腿关节角度 → 运动学推算 → 位置 x, y  │
         │                                      │
         └─────────────────────────────────────┘

legged_odom 模块启动后持续发布 /odom 话题，频率约 100Hz（每秒 100 帧）。

坐标系

里程计用自己的坐标系，开机点为原点：

复制代码

        y
        ↑
  (-1,1) │  (1,1)
        │
────────○────────→ x
        │ (0,0) 开机点
  (-1,-1)│  (1,-1)
        │

x, y ：机器人在地面上的位置，单位米
yaw ：机器人的朝向角，单位弧度。0 = 正前方，π/2 = 左，-π/2 = 右，范围 (-π, π]
开机时：不管机器人面朝哪，odom 初始值为 x=0, y=0, yaw=0

Interaction 订阅了什么

cpp 复制代码

// scheduler.cpp OnOdom 回调
void Scheduler::OnOdom(const nav_msgs::msg::Odometry &msg) {
  auto &vel_state = StateManager::GetInstance()->GetVelocityState();
  const auto &p   = msg.pose.pose;        // PoseWithCovariance → Pose

  double yaw = QuatToYaw(                 // 四元数 (qx,qy,qz,qw) → 偏航角（弧度）
      p.orientation.x, p.orientation.y,
      p.orientation.z, p.orientation.w);

  std::lock_guard<std::mutex> lk(vel_state.odom_mutex);
  vel_state.odom_x   = p.position.x;       // X 坐标（米）
  vel_state.odom_y   = p.position.y;       // Y 坐标（米）
  vel_state.odom_yaw = yaw;                // 朝向角（弧度）
}

Odometry 完整消息结构

cpp 复制代码

// nav_msgs/msg/Odometry
struct Odometry {
    Header header;                 // 时间戳 + frame_id ("odom")
    string child_frame_id;         // 子坐标系 ("base_link")

    PoseWithCovariance pose;       // ← 我们用这部分
    //   ├── pose.position.x       // → odom_x（米）
    //   ├── pose.position.y       // → odom_y（米）
    //   ├── pose.position.z       // → 未使用
    //   └── pose.orientation      // → 四元数 (x,y,z,w) → 转成 odom_yaw（弧度）
    //       (带 6×6 协方差矩阵，表示位置+朝向的不确定度)

    TwistWithCovariance twist;     // ← 未使用
    //   ├── linear  {x, y, z}     // → 估计的线速度
    //   └── angular {x, y, z}     // → 估计的角速度
    //       (带 6×6 协方差矩阵)
};

四元数 → yaw 转换

cpp 复制代码

// scheduler.cpp
static double QuatToYaw(double qx, double qy, double qz, double qw) {
    return std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
}

ROS2 标准公式。四元数 (x,y,z,w) 表示 3D 旋转，我们只需要绕 Z 轴的旋转角（偏航角 yaw）。

在 MoveSkill 中如何使用 odom 数据

直线移动 --- 通过 x, y 变化量算走了多远：

cpp 复制代码

double dx = cur.x - start_pose.x;                   // X 方向偏移
double dy = cur.y - start_pose.y;                   // Y 方向偏移
double travelled = std::sqrt(dx * dx + dy * dy);    // 勾股定理算总位移

不管机器人往哪个方向走，起点到终点的直线距离就是实际走的距离。

转动 --- 通过 yaw 的变化量累积转角：

cpp 复制代码

double dyaw = cur.yaw - prev_yaw;         // 这一帧转了多大角度
if (dyaw > M_PI)  dyaw -= 2.0 * M_PI;    // 修正 -π↔+π 跳变
if (dyaw <= -M_PI) dyaw += 2.0 * M_PI;
accumulated_yaw += dyaw;                  // 累加总转角

yaw 的范围是 (-π, +π]，从 179° 转到 -179° 时 yaw 会瞬间从 +3.12 跳到 -3.12------实际只转了 2°，但差值显示 -6.26 rad。通过 ±2π 修正来识别这种跳变。

限制

相对定位：开机点为原点，不知道自己在房间里的绝对位置
累积误差：长时间运行会漂移（几厘米/秒级别）
只适用于地面：机器人被抱起来/架空时，编码器和 IMU 数据会失准

是否所有机器人都用？

T1 ：✅ 启用，legged_odom 模块正常发布 /odom
Q1 ：❌ 不使用，MoveSkill 走纯时间分支。/odom 订阅仍然运行但不影响 Q1 行为

3. 坐标系约定

从原代码注释：

复制代码

// Quadruped: 0°=forward, 90°=left, 180°=backward, 270°=right

这是 CCW（逆时针） 约定：

复制代码

          0°(前)
           ↑
90°(左) ←── ○ ──→ 270°(右)
           ↓
         180°(后)

velocity_state.h 也对应：

cpp 复制代码

kTurnLeft:  velocity = 0.5   // 正 = CCW = 左转
kTurnRight: velocity = -0.5  // 负 = CW  = 右转

X、Y 是标准平面坐标（米），原点在机器人开机位置。

4. 整体架构

三层分工：

复制代码

┌─────────────────────────────────────────────────────┐
│  订阅者（Scheduler::OnInitialize）                   │
│    订阅 /odom → OnOdom → 写入 VelocityState         │
├─────────────────────────────────────────────────────┤
│  存储层（VelocityState）                             │
│    odom_x, odom_y, odom_yaw + odom_mutex            │
├─────────────────────────────────────────────────────┤
│  使用者（读 odom 数据）                              │
│    MoveSkill::Exec()           → x/y 算距离         │
│                                → yaw 累积转角       │
│    T1MoveTaskDescription       → yaw 算唤醒转向角度 │
│    (kWakeTurnAround 分支)                            │
└─────────────────────────────────────────────────────┘

cpp 复制代码

// velocity_state.h 第44-48行：共享存储
// Odometry pose (updated by Scheduler via /odom subscription)
mutable std::mutex odom_mutex;
double odom_x{0.0};
double odom_y{0.0};
double odom_yaw{0.0};

5. 完整数据流

复制代码

legged_odom 模块
    │
    │ 发布 /odom (100Hz)
    ▼
Scheduler::OnInitialize()
    │ 订阅 /odom
    ▼
Scheduler::OnOdom(msg)
    │ 四元数 → yaw
    │ lock_guard
    │ 写入 VelocityState
    ▼
┌──────────────────────────────────────────────┐
│                VelocityState                  │
│  odom_x  odom_y  odom_yaw                    │
└──────┬───────────────────────┬───────────────┘
       │                       │
       ▼                       ▼
  MoveSkill::Exec     T1MoveTaskDescription
  (x/y 算距离         (kWakeTurnAround 分支
   yaw 累积转角)       yaw 算唤醒转向角度)

6. 框架生命周期------为什么不能在 OnConfigure 订阅

cpp 复制代码

// build/_deps/aimrte-src/src/module/module_base.cpp

// 阶段1: 配置
ModuleCfg ModuleBase::Configure(...) {   // 第13行
    OnConfigure(module_cfg);             // ← g_thread_ctx.ctx_ptr 还是空！
}

// 阶段2: 初始化
bool ModuleBase::Initialize(...) {       // 第28行
    ctx_ptr_ = ctx::Init(core);          // 第31行 ← 这里才创建线程上下文！
    g_thread_ctx = {ctx_ptr_->weak_from_this()}; // init.h 第30行

    ctx_res_manager_.Initialize();       // 第54行
    return OnInitialize();               // 第60行 ← 上下文就绪，可以订阅了
}

调用链：

复制代码

OnConfigure → SkillManager::Init → MoveSkill::Init → SubscribeInline()
                                                                  ↓
                                                    ctx::init::SubscribeInline()
                                                                  ↓
                                                    ExpectContext(loc)
                                                                  ↓
                                         g_thread_ctx.ctx_ptr.lock() → nullptr
                                                                  ↓
                                                    panic("Broken context !")

结论：.Def() 在 OnConfigure 做，.WhenInit().SubscribeOn() 在 OnInitialize 做。

订阅写法

cpp 复制代码

// scheduler.cpp OnConfigure 第39行 --- 声明
.Def(option_.odom_sub_, "/odom")

// scheduler.cpp OnInitialize 第76行 --- 真正订阅
option_.odom_sub_.WhenInit().SubscribeOn(option_.exe_,
    std::bind(&Scheduler::OnOdom, this, std::placeholders::_1));

跟其他 11 个订阅完全一样的模式。

7. 直线移动逻辑

7a. 目标距离怎么来的

复制代码

PlayMove(distance=1)  ← distance 单位：米
    ↓
T1MoveTaskDescription::GetSkillParamList
    │  SetDistance(distance * 1000)   ← 存为毫米：1000mm
    ↓
MoveSkill::Exec
    │  target_distance_m = GetDistance() / 1000.0   ← 毫米转回米：1.0m
    │  has_linear_target = target_distance_m > 0    ← true

7b. 每帧计算位移

cpp 复制代码

// move_skill.cpp Exec() --- while 循环内，每 20ms 执行一次
if (has_linear_target) {
    const double dx = cur.x - start_pose.x;        // X 方向偏移（米）
    const double dy = cur.y - start_pose.y;        // Y 方向偏移（米）
    const double travelled = std::sqrt(dx*dx + dy*dy);  // 总位移（米）

不管机器人往哪个方向走------前进、后退、左移、右移------最终位移都等于起点到当前位置的直线距离。

勾股定理：distance = √(Δx² + Δy²)。

7c. 停止判断

cpp 复制代码

    if (travelled >= target_distance_m) {
        // ✅ 正常：里程计说到了 → 停
        should_stop = true;
    } else if (elapsed >= timeout_ms) {
        // ⚠️ 异常：超时了还没到 → 强制停（安全兜底）
        should_stop = true;
    }
}

7d. 实例计算

前进 1 米，速度 0.25m/s：

复制代码

T=0ms:    start_pose = (0.100, 0.050)  ← 记起点
T=500ms:  cur = (0.225, 0.050) → dx=0.125 → travelled=0.125m
T=1000ms: cur = (0.350, 0.049) → dx=0.250 → travelled=0.250m
T=1500ms: cur = (0.475, 0.051) → dx=0.375 → travelled=0.375m
T=2000ms: cur = (0.600, 0.050) → dx=0.500 → travelled=0.500m
...
T=4000ms: cur = (1.100, 0.050) → dx=1.000 → travelled=1.000m ✅ 到了！停

时间	cur.x	travelled	判断
0ms	0.100	0.000	开始
500ms	0.225	0.125	继续
1000ms	0.350	0.250	继续
1500ms	0.475	0.375	继续
2000ms	0.600	0.500	继续
2500ms	0.725	0.625	继续
3000ms	0.850	0.750	继续
3500ms	0.975	0.875	继续
4000ms	1.100	1.000	✅ 停止

7e. 超时兜底

cpp 复制代码

int64_t timeout_ms = total_duration * 2;     // 前进1米：4000×2=8000ms
if (timeout_ms < 5000) timeout_ms = 5000;    // 最少5秒

如果 odom 坏了/堵墙了/被架空了------odom_x 不动，travelled 永远不会到 1.0。8 秒后强制停，不会无限走。

7f. 适用范围

方向	has_linear_target	用 odom x/y？
前进 distance=1	✅	✅
后退 distance=1	✅	✅
左移 distance=1	✅	✅
右移 distance=1	✅	✅
前进 distance=0	❌	❌ 走纯时间
Q1 所有移动	❌	❌ 走纯时间

8. 转动逻辑

转动逻辑分两层：目标角度计算 （在 T1MoveTaskDescription）+ 角度检测（在 MoveSkill::Exec）。

8a. 目标角度计算 --- 在 T1MoveTaskDescription

cpp 复制代码

// t1_move_task_description.cpp switch(direction)

case Direction::kTurnLeft:
    angular_target = angle > 0 ? angle : step * 30;  // 如 angle=45 → 45°
    velocity = (0, 0, 0.5);                           // 正=左转

case Direction::kTurnRight:
    angular_target = angle > 0 ? angle : step * 30;  // 如 angle=45 → 45°
    velocity = (0, 0, -0.5);                          // 负=右转

case Direction::kTurnAround:
    angular_target = 180;                              // 固定 180°
    velocity = (0, 0, -0.5);

case Direction::kWakeTurnAround:
    // 读 odom yaw 算目标角度（详见 8b）
    angular_target = delta;                            // 里程计算出的
    velocity = 根据最短路径方向 ±0.5;

8b. kWakeTurnAround 目标角度计算 --- 用里程计

cpp 复制代码

// t1_move_task_description.cpp kWakeTurnAround case

// 读里程计当前朝向
auto& vel_state = StateManager::GetInstance()->GetVelocityState();
{
    std::lock_guard<std::mutex> lk(vel_state.odom_mutex);
    current_yaw_rad = vel_state.odom_yaw;           // 当前朝向（弧度）
}
double H = current_yaw_rad * 180.0 / M_PI;           // 转成度，如 90°
double T = static_cast<double>(angle);                // 目标方向（度），从语音模块来，如 300°

// 两条路径，选短的
double ccw_turn = fmod(T - H + 360.0, 360.0);       // 逆时针：如 (300-90+360)%360 = 210°
double cw_turn  = fmod(H - T + 360.0, 360.0);       // 顺时针：如 (90-300+360)%360 = 150°

if (cw_turn <= ccw_turn) {                           // 150 < 210 → 顺时针更短
    delta = cw_turn;           // 150°
    turn_velocity = -0.5;      // 右转(CW)
} else {
    delta = ccw_turn;          // 逆时针更短 → 左转
    turn_velocity = +0.5;      // 左转(CCW)
}
angular_target = delta;                              // 传给 MoveSkill

复制代码

例子：人在世界 300° 方向，里程计说机器人朝 90°
  H=90, T=300
  ccw路径 = (300-90+360)%360 = 210°  （左转 210°）
  cw路径  = (90-300+360)%360  = 150°  （右转 150°）← 更短
  → 选右转 150°，angular_target=150°

例子：人在 60° 方向，机器人朝 0°（正前方）
  H=0, T=60
  ccw路径 = (60-0+360)%360 = 60°    （左转 60°）← 更短
  cw路径  = (0-60+360)%360  = 300°  （右转 300°）
  → 选左转 60°，angular_target=60°

8c. 角度检测 --- 在 MoveSkill::Exec（每 20ms）

所有转动（kTurnLeft、kTurnRight、kTurnAround、kWakeTurnAround）都走同一套检测逻辑：

cpp 复制代码

// move_skill.cpp Exec() while 循环内

// 第一步：每帧累积 yaw 变化量
double dyaw = cur.yaw - prev_yaw;          // 这一帧转了多少（弧度）
if (dyaw > M_PI)  dyaw -= 2.0 * M_PI;     // 修正 -π→+π 跳变
if (dyaw <= -M_PI) dyaw += 2.0 * M_PI;    // 修正 +π→-π 跳变
accumulated_yaw += dyaw;                   // 累加总转角
prev_yaw = cur.yaw;

// 第二步：判断是否转够了
if (has_angular_target) {
    double turned_deg = abs(accumulated_yaw) * 180.0 / M_PI;  // 弧度→度

    if (turned_deg >= angular_target) {          // ✅ 转够了 → 停
        should_stop = true;
    } else if (elapsed >= timeout_ms) {          // ⚠️ 超时 → 强制停
        should_stop = true;
    }
}

8d. yaw 跳变修正详解

yaw（朝向角）的范围是 (-π, +π]，即 -180° 到 +180°。

复制代码

机器人从 179° 继续左转 2°：
  转前 yaw = +179° = +3.12 rad
  转后 yaw = -179° = -3.12 rad
  差值 dyaw = -3.12 - 3.12 = -6.24 rad  ← 看起来像转了356°！

实际只转了 2° = 0.035 rad
需要修正：-6.24 + 2π = -6.24 + 6.28 = +0.035 rad ✅

修正规则：

dyaw > π（单帧正向变化超过半圈）→ 减去 2π（跳变，不是真转了这么大）
dyaw ≤ -π（单帧负向变化超过半圈）→ 加上 2π

累积而不是直接做差------这样可以避免一次跳变就永远错位。

8e. 实例计算

左转 45°，角速度 0.5 rad/s：

复制代码

T=0ms:    prev_yaw = 0.500 rad → accumulated_yaw = 0
T=500ms:  cur.yaw = 0.750 → dyaw = 0.250 → acc = 0.250 → 14.3°
T=1000ms: cur.yaw = 1.000 → dyaw = 0.250 → acc = 0.500 → 28.6°
T=1500ms: cur.yaw = 1.250 → dyaw = 0.250 → acc = 0.750 → 43.0°
T=1570ms: cur.yaw = 1.285 → dyaw = 0.036 → acc = 0.785 → 45.0° ✅ 停

时间	cur.yaw(rad)	dyaw	accumulated	角度	判断
0ms	0.500	---	0.000	0°	开始
500ms	0.750	0.250	0.250	14.3°	继续
1000ms	1.000	0.250	0.500	28.6°	继续
1500ms	1.250	0.250	0.750	43.0°	继续
1570ms	1.285	0.036	0.785	45.0°	✅ 停止

8f. 超时兜底

cpp 复制代码

// 左转45°理论1570ms → 超时 2×1570=3140ms → 不足5秒 → 取5000ms
int64_t timeout_ms = total_duration * 2;
if (timeout_ms < 5000) timeout_ms = 5000;

如果 odom yaw 不更新（里程计挂了），accumulated_yaw 永远是 0，5 秒后强制停。

8g. 适用范围

转动类型	has_angular_target	目标从哪来	用 odom yaw？
kTurnLeft	✅	PlayMove 的 angle 参数	✅
kTurnRight	✅	PlayMove 的 angle 参数	✅
kTurnAround	✅	固定 180°	✅
kWakeTurnAround	✅	odom yaw 计算	✅
Q1 所有转动	❌	不走这里	❌ 纯时间

9. 时间 vs 里程计的主辅关系

cpp 复制代码

// move_skill.cpp while 循环

if (has_linear_target) {
    // ========== odom 为主 ==========
    if (travelled >= target_distance_m) {
        break;  // 里程计说到了 → 停！
    }
    if (elapsed >= timeout_ms) {
        break;  // 超时 → 强制停（兜底）
    }
} else if (has_angular_target) {
    // ========== odom 为主 ==========
    if (turned_deg >= angular_target) {
        break;  // 转够了 → 停！
    }
    if (elapsed >= timeout_ms) {
        break;  // 超时 → 强制停（兜底）
    }
} else {
    // ========== 纯时间 ==========
    if (elapsed >= total_duration) {
        break;  // 时间到 → 停
    }
}

之前： 走固定时间，odom 辅助提前停

现在： 走够为止，时间只做安全兜底

9b. MoveSkill::Exec 完整代码逐行讲解

取参数

cpp 复制代码

int64_t duration = move_param->GetDuration();
// 理论时长(ms)。T1MoveTaskDescription 算的。前进 1 米 → 4000ms

auto publish_interval_ms = move_param->GetPublishInterval().count();
// 每轮循环睡多久。配置里 publish_interval: 0.02 → 20ms → 每秒发 50 帧

auto velocity = move_param->GetVelocity();
// tuple<float,float,float> = (前进速度, 横移速度, 角速度)
// 前进：(0.25, 0, 0)    后退：(-0.25, 0, 0)
// 左移：(0, 0.2, 0)      右移：(0, -0.2, 0)
// 左转：(0, 0, 0.5)      右转：(0, 0, -0.5)

判断走哪个分支（最核心的三选一）

cpp 复制代码

const double target_distance_m = move_param->GetDistance() > 0
    ? move_param->GetDistance() / 1000.0   // mm → m，如 1000 → 1.0m
    : -1.0;                                  // 没设 → 不启用

const bool has_linear_target = target_distance_m > 0.0;
// T1 前后左右横移 → SetDistance 被调 → has_linear_target=true
// 转动 → distance=0 → has_linear_target=false
// Q1 → 不调 SetDistance、distance_ 默认为 0 → has_linear_target=false

cpp 复制代码

const uint32_t angular_target = move_param->GetAngularTargetDeg();
// T1MoveTaskDescription 设的转动目标角度（度）
// kTurnLeft: 45°, kTurnRight: 45°, kTurnAround: 180°, kWakeTurnAround: odom 算出
// Q1: 不调 SetAngularTargetDeg → 默认为 0

const bool has_angular_target = (angular_target > 0u);
// 有角度目标 → 走转动分支
// 没有 → 走纯时间

cpp 复制代码

const Pose2D start_pose = GetCurrentPose();
// 记下起点！后面每帧跟它比：
// 直线：start_pose.x/y vs cur.x/y → 走了多远
// 转动：start_pose.yaw + 每帧累加 → 转了多少

has_linear	has_angular	走哪个分支	什么时候停
✅	❌	T1 前后左右	odom 说距离到了
❌	✅	T1 所有转动	odom yaw 说角度到了
❌	❌	Q1、无目标移动	设定时间到

构造发给 MC 的消息

cpp 复制代码

msg.forward_velocity = std::get<0>(velocity);
msg.lateral_velocity = std::get<1>(velocity);
msg.angular_velocity = std::get<2>(velocity);
msg.source = "interaction";

// 只有三个值，MC 拿到后驱动电机：
// forward_velocity > 0 → 前进
// forward_velocity < 0 → 后退
// angular_velocity > 0 → 左转(CCW)
// angular_velocity < 0 → 右转(CW)

超时和安全兜底

cpp 复制代码

int64_t timeout_ms = total_duration * 2;     // 理论时间的两倍
if (timeout_ms < 5000) timeout_ms = 5000;    // 最少 5 秒

// 比如前进 1 米理论 4 秒 → 超时 8 秒
// 左转 45° 理论 1.5 秒 → 超时 5 秒（不足 5 秒取 5 秒）
// 防止 odom 坏了/堵墙了/机器人被架空了 → 无限走

while 循环 --- 每 20ms 一轮

cpp 复制代码

while (cancel_requested_ == 0                    // 没被 Stop() 取消
    && move_direction != kStopWalk                // 没被别的任务叫停
    && elapsed < timeout_ms) {                    // 没超时

    Publish(msg);      // 发一帧速度给 MC → 电机转
    sleep(20ms);       // 等 20 毫秒
    elapsed += 20;     // 计时

三个退出条件，任意一个不满足就停。

每帧读 odom + 累积 yaw

cpp 复制代码

    const Pose2D cur = GetCurrentPose();           // 读最新位姿（锁保护）

    double dyaw = cur.yaw - prev_yaw;              // 这一帧转了多大角度
    if (dyaw > M_PI)  dyaw -= 2.0 * M_PI;         // 从 -π 跳到 +π，修正
    if (dyaw <= -M_PI) dyaw += 2.0 * M_PI;        // 从 +π 跳到 -π，修正
    accumulated_yaw += dyaw;                       // 累加总转角
    prev_yaw = cur.yaw;                            // 记下，下帧用

yaw（朝向角）范围是 (-π, π]。机器人从 179°（+3.12rad）转到 -179°（-3.12rad）时，yaw 一瞬间跳了 -6.24rad------但实际只转了 2°。如果单帧变化超过半圈（π），就是跳变，通过 ±2π 修正。

每帧都在累积 yaw，不管是哪个分支。 这样转动分支能实时拿到累计转角。

分支 A --- 直线移动

cpp 复制代码

    if (has_linear_target) {
      double dx = cur.x - start_pose.x;            // X 偏移
      double dy = cur.y - start_pose.y;            // Y 偏移
      double travelled = sqrt(dx*dx + dy*dy);       // 勾股算总位移

      if (travelled >= target_distance_m) {         // odom 说到了
        停！                                        // 正常
      } else if (elapsed >= timeout_ms) {           // 超时还没到
        强制停！                                     // 兜底
      }
    }

odom x/y 为主，timeout 兜底。

分支 B --- 转动

cpp 复制代码

    else if (has_angular_target) {
      double turned_deg = abs(accumulated_yaw) * 180.0 / M_PI;
      // 弧度 → 度，取绝对值（不管左转还是右转，只关心转了多少）

      if (turned_deg >= angular_target) {           // 转够了
        停！
      } else if (elapsed >= timeout_ms) {           // 超时
        强制停！
      }
    }

odom yaw 为主，timeout 兜底。

分支 C --- 纯时间

cpp 复制代码

    else {
      if (elapsed >= total_duration) {              // 设定时间到了
        停！
      }
    }

Q1 全部走这里。 不读 odom，纯时间控制。

结束

cpp 复制代码

  if (被打断)  "被外部打断"
  else         "持续时间已到"

  PublishStop();   // 不管怎么停的，都发全零速度给 MC

10. 安全兜底

cpp 复制代码

int64_t timeout_ms = total_duration * 2;  // 理论时长的 2 倍
if (timeout_ms < 5000) timeout_ms = 5000; // 最少 5 秒

while (elapsed < timeout_ms  // 超时强制停
    && cancel_requested_ == 0             // 被 Stop() 打断
    && move_direction != kStopWalk) {     // 被别的任务叫停
    ...
}

退出条件	触发场景
`travelled >= target`	正常：里程计说到了
`turned_deg >= angular_target`	正常：转够了
`elapsed >= timeout_ms`	异常：里程计坏了/堵墙，强制停
`cancel_requested_ != 0`	外部调 Stop()，立刻停
`move_direction == kStopWalk`	被别的任务停掉

10b. 里程计容错 --- 自动退回纯时间

背景

timeout 兜底解决的是"里程计数据不正确"的问题（odom 全是 0、堵墙）。但如果里程计进程根本没有启动，或运行中途挂掉，odom 数据不再更新，机器人会一直走到 timeout 才停。

为此增加了实时检测机制：

从未收到过里程计数据 → 退回纯时间
超过 1 秒没有新的里程计数据 → 判断为已挂掉 → 退回纯时间

VelocityState 增加标记和时间戳

cpp 复制代码

// velocity_state.h
#include <chrono>

struct VelocityState {
  // ... 原有字段 ...

  bool odom_received{false};                                 // 是否收到过
  std::chrono::steady_clock::time_point odom_last_time{};    // 最后收包时间
};

OnOdom 每次刷新

cpp 复制代码

// scheduler.cpp OnOdom 回调
std::lock_guard<std::mutex> lk(vel_state.odom_mutex);
vel_state.odom_x         = p.position.x;
vel_state.odom_y         = p.position.y;
vel_state.odom_yaw       = yaw;
vel_state.odom_received  = true;                              // 标记已收到
vel_state.odom_last_time = std::chrono::steady_clock::now();  // 刷新时间

Exec 执行前检查

cpp 复制代码

// move_skill.cpp Exec() --- 在 set has_linear/has_angular 之后
if (has_linear_target || has_angular_target) {
    auto& vel_state = StateManager::GetInstance()->GetVelocityState();
    bool odom_stale = false;
    if (!vel_state.odom_received) {
        odom_stale = true;                                       // 从没收过
    } else {
        auto now   = std::chrono::steady_clock::now();
        odom_stale = (now - vel_state.odom_last_time) > std::chrono::seconds(1);  // 超过 1 秒
    }
    if (odom_stale) {
        AIMRTE_WARN("MoveSkill: 里程计数据不可用，退回纯时间控制");
        has_linear_target  = false;    // 不走 odom 分支
        has_angular_target = false;    // 不走 odom 分支
    }
    // → 落入 else 分支 → 纯时间控制
}

生效场景

场景	检测方式	行为
legged_odom 没启动	`odom_received=false`	退回到纯时间
legged_odom 运行中途挂掉	`now - odom_last_time > 1s`	退回到纯时间
legged_odom 正常运行	都不触发	走 odom 分支
Q1	has_linear/has_angular=false	不检查，走纯时间

正常时 odom 100Hz（每 10ms 一帧），odom_last_time 每 10ms 刷新，与当前时间差永远 < 1s。里程计停发 1 秒后自动检测到并退回。

11. 线程安全

写入线程： Scheduler 的 executor 线程（OnOdom）

读取线程： Worker 线程（MoveSkill::Exec）、主线程（kWakeTurnAround）

同一把锁保护：

cpp 复制代码

// 写 --- scheduler.cpp
std::lock_guard<std::mutex> lk(vel_state.odom_mutex);
vel_state.odom_x   = p.position.x;
vel_state.odom_y   = p.position.y;
vel_state.odom_yaw = yaw;

// 读 --- move_skill.cpp
std::lock_guard<std::mutex> lk(vel_state.odom_mutex);
return {vel_state.odom_x, vel_state.odom_y, vel_state.odom_yaw};

// 读 --- t1_move_task_description.cpp (kWakeTurnAround)
std::lock_guard<std::mutex> lk(vel_state.odom_mutex);
current_yaw_rad = vel_state.odom_yaw;

同一个 mutex 保证读到的 x、y、yaw 属于同一帧消息。

12. CMake 集成

关键区别：

关键字	用途	谁用
`DEPEND`	工程内部目标，aimrte 会尝试安装	自己的模块
`IMPORT`	第三方库（ROS2 标准包），只链接不安装	nav_msgs 等

cmake 复制代码

# src/scheduler/CMakeLists.txt --- 谁用谁声明
find_package(nav_msgs REQUIRED)
aimrte_library(
  DEPEND aimrte aimdk-protocol-ROS aima::interaction::state ...
  IMPORT nav_msgs    # ← 第三方库用 IMPORT
)

cmake 复制代码

# src/skill/CMakeLists.txt --- 不用 nav_msgs 了，不声明
aimrte_library(
  DEPEND aimrte aimdk-protocol-ROS aima::interaction::state ...
)

13. 配置参数

yaml 复制代码

# t1_move_cfg.yaml

# 室内模式
indoor:
  forward:
    velocity: 0.25            # 前进速度 m/s
    duration_cap_factor: 1.0  # 时间折扣（1.0 = 不折扣）
  backward:
    velocity: -0.25
    duration_cap_factor: 1.0
  turn_around:
    velocity: -0.5            # 后转/唤醒转向角速度 rad/s

# 通用配置
common:
  publish_interval: 0.02      # 发指令间隔（秒）= 50Hz
  enable_wake_turn_around: true  # 唤醒转向开关

duration_cap_factor 之前是 0.6（只走 60%），现在改 1.0。

14. Odometry 消息结构

cpp 复制代码

// nav_msgs/msg/Odometry
struct Odometry {
    Header header;                // 时间戳 + frame_id ("odom")
    string child_frame_id;        // 子坐标系 ("base_link")

    PoseWithCovariance pose;      // ← 我们用的
    //  └── Pose
    //       ├── position { x, y, z }          ← odom_x, odom_y
    //       └── orientation { x, y, z, w }    ← 转成 odom_yaw

    TwistWithCovariance twist;    // 没用
    //  ├── linear  { x, y, z }  --- 线速度
    //  └── angular { x, y, z }  --- 角速度
};

我们只用了 pose.pose.position.(x, y) 和 pose.pose.orientation（四元数转 yaw）。

15. 与其他模块的关系

复制代码

                ┌─────────────────┐
                │  legged_odom     │  发布 /odom (x, y, yaw)
                └────────┬────────┘
                         │
                         ▼
                ┌─────────────────┐
                │  interaction     │  订阅 /odom
                │    ↓            │
                │  VelocityState  │  存储 odom_x, odom_y, odom_yaw
                │    ↓            │
                │  MoveSkill      │  发 /aima/mc/locomotion/velocity
                └────────┬────────┘
                         │
                         ▼
                ┌─────────────────┐
                │      MC         │  收速度指令 → 驱动电机
                └─────────────────┘
                         ↑
                ┌─────────────────┐
                │  语音模块        │  发 PlayMove(direction, angle)
                └─────────────────┘

legged_odom → 发 /odom
语音模块 → 唤醒词检测 + 声源定位 → 发 PlayMove(angle=xxx)
MC（运动控制） → 收 McLocomotionVelocity → 驱动电机
interaction → 订阅 odom，消费 PlayMove，生成速度指令 → 居中调度

16. 涉及文件清单（10 个文件）

文件	改动
`src/state/velocity_state.h`	添加 `odom_x`, `odom_y`, `odom_yaw`, `odom_mutex`
`src/scheduler/scheduler.h`	添加 `odom_sub_`, `OnOdom()` 声明
`src/scheduler/scheduler.cpp`	OnConfigure `.Def(odom_sub_)`, OnInitialize `SubscribeOn`, `OnOdom` 实现
`src/scheduler/CMakeLists.txt`	添加 `find_package(nav_msgs)` + `IMPORT nav_msgs`
`src/skill/CMakeLists.txt`	移除 nav_msgs/geometry_msgs 依赖
`src/skill/atomic/move_skill.h`	移除 odom_sub, 添加 Pose2D, GetCurrentPose
`src/skill/atomic/move_skill.cpp`	odom 为主三分支循环（直线/转动/纯时间）
`src/skill/params/move_params.h`	添加 `angular_target_deg_`，`distance_`/`step_` 初始化为 0
`src/task_description/t1_move_task_description.cpp`	SetDistance, SetAngularTargetDeg, kWakeTurnAround odom 计算
`src/app/install/linux/bin/cfg/t1_move_cfg.yaml`	`enable_wake_turn_around: true`, `duration_cap_factor: 1.0`

17. 关键技术点

四元数 → yaw

cpp 复制代码

static double QuatToYaw(double qx, double qy, double qz, double qw) {
    return std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
}

ROS2 标准公式，把四元数转成偏航角（弧度）。

yaw ±π 跳变处理

cpp 复制代码

double dyaw = cur.yaw - prev_yaw;
if (dyaw > M_PI)  dyaw -= 2.0 * M_PI;   // 从 -π 跳到 +π → 补偿
if (dyaw <= -M_PI) dyaw += 2.0 * M_PI;  // 从 +π 跳到 -π → 补偿
accumulated_yaw += dyaw;

累积每帧小增量，防止 yaw 从 180° 跳到 -180° 时计算错误。

勾股定理算距离

cpp 复制代码

double travelled = std::sqrt(dx * dx + dy * dy);

不管机器人往哪个方向走，两点间直线距离就是总位移。

18. 调试手段

运行时保留的关键日志

cpp 复制代码

// 正常到达（里程计生效）
AIMRTE_INFO("MoveSkill: 里程计已到达目标距离 {:.3f}m (目标 {:.3f}m), 提前停止", ...);

// 超时兜底（里程计异常/堵墙）
AIMRTE_WARN("MoveSkill: 超时！{}ms 只走了 {:.3f}m (目标 {:.3f}m), 强制停止", ...);
AIMRTE_WARN("MoveSkill: 超时！{}ms 只转了 {:.1f}deg (目标 {}deg), 强制停止", ...);

开发调试时可加的详细日志

需要排查问题时，在 move_skill.cpp 对应位置加上以下日志：

Exec 入口 --- 打印入参：

cpp 复制代码

// move_skill.cpp 第47行附近
AIMRTE_INFO("MoveSkill: 入参 velocity=({:.3f},{:.3f},{:.3f}), duration={}ms, raw_distance={}mm",
    std::get<0>(velocity), std::get<1>(velocity), std::get<2>(velocity), duration, raw_distance);

分支判断 --- 确认走了哪个分支：

cpp 复制代码

// move_skill.cpp 第57行附近
AIMRTE_INFO("MoveSkill: odom判断 target={:.3f}m, angular_target={}deg, has_linear={}, has_angular_target={}, start=({:.3f},{:.3f},{:.3f}rad)",
    target_distance_m, angular_target, has_linear_target, has_angular_target,
    start_pose.x, start_pose.y, start_pose.yaw);

循环内 --- 每 500ms 打印当前位置：

cpp 复制代码

// move_skill.cpp 第107行附近（直线分支内）
if (elapsed % 500 < publish_interval_ms || travelled >= target_distance_m) {
    AIMRTE_INFO("MoveSkill: [{}/{}ms] odom=({:.3f},{:.3f}) travelled={:.3f}m target={:.3f}m",
        elapsed, total_duration, cur.x, cur.y, travelled, target_distance_m);
}

循环内 --- 每 500ms 打印当前转角：

cpp 复制代码

// move_skill.cpp 第121行附近（转动分支内）
if (elapsed % 500 < publish_interval_ms || turned_deg >= angular_target) {
    AIMRTE_INFO("MoveSkill: [{}/{}ms] yaw_acc={:.1f}deg target={}deg",
        elapsed, total_duration, turned_deg, angular_target);
}

结束时 --- 打印实际耗时和里程计记录：

cpp 复制代码

// move_skill.cpp 第144行附近
auto real_duration_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
    std::chrono::steady_clock::now() - ts_start).count();
AIMRTE_INFO("MoveSkill: 实际耗时={}ms (设定duration={}ms)", real_duration_ms, total_duration);

// move_skill.cpp 第151行附近（直线结束后）
AIMRTE_INFO("MoveSkill: odom记录总位移={:.3f}m, 目标={:.3f}m", total_travelled, target_distance_m);

// move_skill.cpp 第156行附近（转动结束后）
AIMRTE_INFO("MoveSkill: odom记录总转角={:.1f}deg, 目标={}deg", turned_deg, angular_target);

日志示例（前进 1 米）

正常情况：

复制代码

MoveSkill: 入参 velocity=(0.250,0.000,0.000), duration=4000ms, raw_distance=1000mm
MoveSkill: odom判断 target=1.000m, angular_target=0deg, has_linear=true, has_angular_target=false, start=(0.100,0.200,1.570rad)
MoveSkill: [500/4000ms]  odom=(0.225,0.200) travelled=0.125m target=1.000m
MoveSkill: [1000/4000ms] odom=(0.350,0.199) travelled=0.250m target=1.000m
MoveSkill: [1500/4000ms] odom=(0.475,0.201) travelled=0.375m target=1.000m
MoveSkill: [2000/4000ms] odom=(0.600,0.200) travelled=0.500m target=1.000m
MoveSkill: [2500/4000ms] odom=(0.725,0.198) travelled=0.625m target=1.000m
MoveSkill: [3000/4000ms] odom=(0.850,0.200) travelled=0.750m target=1.000m
MoveSkill: [3500/4000ms] odom=(0.975,0.201) travelled=0.875m target=1.000m
MoveSkill: 里程计已到达目标距离 1.000m (目标 1.000m), 提前停止
MoveSkill: 实际耗时=3875ms (设定duration=4000ms)
MoveSkill: odom记录总位移=1.000m, 目标=1.000m

odom 异常情况：

复制代码

MoveSkill: [4000/4000ms] odom=(0.000,0.000) travelled=0.000m target=1.000m
MoveSkill: [5000/4000ms] odom=(0.000,0.000) travelled=0.000m target=1.000m
MoveSkill: 超时！5000ms 只走了 0.000m (目标 1.000m), 强制停止
MoveSkill: 实际耗时=5000ms (设定duration=4000ms)
MoveSkill: odom记录总位移=0.000m, 目标=1.000m

19. 常见问题与排查

现象	可能原因	排查
走不到目标距离	MC 实际速度 < 命令速度	看每 500ms 日志，travelled 增长慢 → 调大配置文件 velocity
odom 始终为 0	legged_odom 未启动或 /odom 未发布	查看 legged_odom 模块日志
方向反了	坐标系约定搞反	确认 CCW 约定，正向=左
动一下就停	`distance_` 未初始化返回随机值	MoveParams 成员已加 `{0}` 默认初始化
转超过目标角度	yaw 跳变未正确处理	检查 dyaw 归一化逻辑
Q1 走 odom 分支	`distance_` 垃圾值 >0	`int32_t distance_{0}` 保证默认=0，不走 odom
Q1 机器人	q1_move_task_description 不调 SetDistance/SetAngularTargetDeg	走纯时间分支，行为不变

20. 扩展指南 --- 以后如何加新的 topic 订阅

以新增 /imu 为例：

步骤 1：CMake

cmake 复制代码

# scheduler/CMakeLists.txt
find_package(sensor_msgs REQUIRED)
IMPORT sensor_msgs

步骤 2：State 加字段

cpp 复制代码

// sys_state.h
mutable std::mutex imu_mutex;
double imu_angular_z{0.0};

步骤 3：Scheduler Option 加 Sub

cpp 复制代码

// scheduler.h
#include <sensor_msgs/msg/imu.hpp>
aimrte::Sub<sensor_msgs::msg::Imu> imu_sub_;
void OnImu(const sensor_msgs::msg::Imu &msg);

步骤 4：OnConfigure 声明

cpp 复制代码

// [Rpc::ros2] 之前
.Def(option_.imu_sub_, "/imu")

步骤 5：OnInitialize 订阅

cpp 复制代码

// SubscribeOn 区域
option_.imu_sub_.WhenInit().SubscribeOn(option_.exe_,
    std::bind(&Scheduler::OnImu, this, std::placeholders::_1));

步骤 6：回调写入

cpp 复制代码

void Scheduler::OnImu(const sensor_msgs::msg::Imu &msg) {
    auto &sys = StateManager::GetInstance()->GetSysState();
    std::lock_guard<std::mutex> lk(sys.imu_mutex);
    sys.imu_angular_z = msg.angular_velocity.z;
}

步骤 7：消费者读取

cpp 复制代码

auto &sys = StateManager::GetInstance()->GetSysState();
double az;
{ std::lock_guard<std::mutex> lk(sys.imu_mutex); az = sys.imu_angular_z; }

核心原则

复制代码

1. 所有订阅资源的声明（.Def）     → Scheduler::OnConfigure
2. 所有订阅的注册（SubscribeOn）  → Scheduler::OnInitialize
3. 所有回调的实现                 → Scheduler 的 private 方法
4. 共享数据存 State               → src/state/ 对应 struct
5. 消费者读 State                 → 加锁读
6. CMake 第三方库用 IMPORT        → 不用 DEPEND
7. Sub 放 [Rpc::ros2] 前面       → Service 放后面

机器人移动控制升级：从按时间盲走到按里程计精走

Interaction 模块里程计（Odom）集成完整总结

1. 背景与目的

之前的问题：按时间盲走

解决方案：里程计为主，时间兜底

对比

适用范围

2. 里程计是什么

通俗理解

物理传感器

数据流程

坐标系

Interaction 订阅了什么

Odometry 完整消息结构

四元数 → yaw 转换

在 MoveSkill 中如何使用 odom 数据

限制

是否所有机器人都用？

3. 坐标系约定

4. 整体架构

5. 完整数据流

6. 框架生命周期------为什么不能在 OnConfigure 订阅

当在 OnConfigure 里调 Subscribe 时会怎样？

订阅写法

7. 直线移动逻辑

7a. 目标距离怎么来的

7b. 每帧计算位移

7c. 停止判断

7d. 实例计算

7e. 超时兜底

7f. 适用范围

8. 转动逻辑

8a. 目标角度计算 --- 在 T1MoveTaskDescription

8b. kWakeTurnAround 目标角度计算 --- 用里程计

8c. 角度检测 --- 在 MoveSkill::Exec（每 20ms）

8d. yaw 跳变修正详解

8e. 实例计算

8f. 超时兜底

8g. 适用范围

9. 时间 vs 里程计的主辅关系

9b. MoveSkill::Exec 完整代码逐行讲解

取参数

判断走哪个分支（最核心的三选一）

构造发给 MC 的消息

超时和安全兜底

while 循环 --- 每 20ms 一轮

每帧读 odom + 累积 yaw

分支 A --- 直线移动

分支 B --- 转动

分支 C --- 纯时间

结束

10. 安全兜底

10b. 里程计容错 --- 自动退回纯时间

背景

VelocityState 增加标记和时间戳

OnOdom 每次刷新

Exec 执行前检查

生效场景

11. 线程安全

12. CMake 集成

13. 配置参数

14. Odometry 消息结构

15. 与其他模块的关系

16. 涉及文件清单（10 个文件）

17. 关键技术点

四元数 → yaw

yaw ±π 跳变处理

勾股定理算距离

18. 调试手段

运行时保留的关键日志

开发调试时可加的详细日志

日志示例（前进 1 米）

19. 常见问题与排查

20. 扩展指南 --- 以后如何加新的 topic 订阅

核心原则