机器人旋转变形逻辑分析

机器人变形逻辑分析

概述

本文档从代码层面详细描述机器人执行"旋转变形"（kTransformateRotate，animation_id=8）的完整流程，从用户说出"变形"到机器人完成物理变形动作。

涉及的核心模块

模块	职责
Scheduler	ROS2 RPC 入口，接收外部请求
Checker (T1Checker)	前置条件校验
Dispatcher	任务分发
TaskFactory	创建 Task 对象
TaskDescriptionManager	管理不同机型(T1/Q1)的任务说明书
T1AnimationTaskDescription	T1 动画任务说明书，构造技能清单
WorkerManager	调度 Worker 执行任务，处理仲裁
Worker	持有 Task，驱动技能逐个执行
Task	持有技能清单(SkillParamList)，按顺序执行
ActionSkill	向 MC 发送动作切换指令
AudioSkill	播放音频文件
MotorSkill	切换舵机（四足/双足）
StateManager	全局机器人状态（形态、动作ID、电量等）

关键数据结构

T1Animation 枚举:
  kTransformateRotate = 8    // 旋转变形

MC Action ID:
  101 = QUADRUPED_STAND_DEFAULT       // 四足站立
  102 = QUADRUPED_LOCOMOTION_DEFAULT  // 四足行走态
  110 = QUADRUPED_GET_DOWN_DEFAULT    // 四足趴下
  111 = QUADRUPED_SIT_DOWN_DEFAULT    // 四足坐下
  300 = BIPED_LOCOMOTION_WBC          // 双足WBC控制
   21 = BIPED_TO_QUADRUPED_ROTATE     // 双足转四足（旋转）

RobotForm:
  kQuadruped = 四足形态
  kBiped     = 双足形态

---

第一阶段：RPC 入口与前置校验

步骤 1：ROS2 RPC 回调触发

文件 : scheduler.cpp:254-267

aimrt::rpc::Status Scheduler::PlayAnimationService(
    const aimdk_msgs::srv::PlayAnimation::Request &req,
    aimdk_msgs::srv::PlayAnimation::Response &res) {

  AIMRTE_INFO("Received PlayAnimation request with animation_id：{}", req.animation_id);
  // 日志: [20:45:29.299] Received PlayAnimation request with animation_id：8

当 App/语音/遥控器通过 ROS2 调用 PlayAnimation RPC 服务时，Scheduler 的 PlayAnimationService 回调被触发。req.animation_id = 8。

步骤 2：前置校验 --- CheckAnimation

文件 : scheduler.cpp:257

if (dispatcher_->CheckAnimation(req.animation_id) != kSuccess) {
    res.success = false;
    return {};
}

文件 : t1_checker.cpp:25-58

Checker 按顺序执行 5 道检查：

第 1 关：形态是否支持此动画？

if (not CheckAnimationSupport(animation)) return kFormNotSupportCurrAnimation;

• 四足支持列表：kGreet, kHandshake, kSitDown, kLieDown, kStandUp, kTransformateRotate, kProud, kJoy, kStretch, kDance
• 双足支持列表：kGreet, kHandshake, kBothHandsMakeHeart, kDance, kTransformateRotate, kPhotoPose_1~4, kPhotoThreeShot, kTalking, kProud, kJoy, kRightHandPunch, kLeftHandPunch, kBipedDefaultPos
• kTransformateRotate(8) 在两个列表中都有，无论当前什么形态都能通过

第 2 关：四足变形条件（四足形态 + kTransformateRotate 时才检查）

if (not CheckQuadrupedTransformateCondition(animation)) return kTransformateConditionNotMet;

只在 curr_robot_form == kQuadruped 且 animation == kTransformateRotate 时检查。要求当前 curr_action_id 为以下之一：

• QUADRUPED_STAND_DEFAULT (101)
• QUADRUPED_LOCOMOTION_DEFAULT (102)
• QUADRUPED_LOCOMOTION_TERRAIN (103)
• QUADRUPED_LOCOMOTION_RUN (112)
• QUADRUPED_GET_DOWN_DEFAULT (110)
• QUADRUPED_SIT_DOWN_DEFAULT (111)

这些是"稳定姿态"，不在这些状态下禁止变形（防止运动中变形摔倒）。

第 3 关：双足状态前置 action 检查

if (not CheckBipedAction()) return kBipedActionNotSupportAni;

只在双足形态下检查，要求当前 curr_action_id 为：

• QUADRUPED_TO_BIPED (10)
• QUADRUPED_TO_BIPED_ROTATE (11)
• BIPED_LOCOMOTION_DEFAULT (301)
• BIPED_LOCOMOTION_TERRAIN (302)
• BIPED_LOCOMOTION_WBC (300)

第 4 关：保底动作豁免（双足变形直接放行！）

if (animation == kSitDown || animation == kLieDown ||
    (animation == kTransformateRotate && curr_robot_form == kBiped)) {
    return kSuccess;  // 直接通过，跳过电量检查！
}

双足形态下的 kTransformateRotate 在此处直接返回成功，跳过第 5/6 关的电量检查。 这是设计意图：双足变形被视为必须响应的"保底动作"。

第 5 关：电量/充电拦截（全部动画）

bool is_easy_limit = (!IsIgnoreLowBatteryLimit() && last_battery_level_audio >= 4)
                     || is_charging;
if (is_easy_limit) return kFormNotSupportCurrAnimation;

第 6 关：电量/充电拦截（仅复杂动作）

bool is_complex_limit = (同上条件);
if (is_complex_limit && !IsEasyMotion(animation)) return kFormNotSupportCurrAnimation;

只有"简单动作"（kGreet, kHandshake, kBothHandsMakeHeart, 拍照系列, 碰拳）在低电量/充电时允许执行。kTransformateRotate 不是简单动作，但如果走到这里（四足形态），充电时会被拦截。

步骤 3：投递到线程池

文件 : scheduler.cpp:262-264

option_.exe_.Post([this, animation_id = req.animation_id, interrupt = req.interrupt]() {
    dispatcher_->DispatchAnimation(animation_id, interrupt);
});
res.success = true;  // 立即返回成功（异步执行）

RPC 立即返回成功给调用方，实际动画执行在线程池中异步处理。

---

第二阶段：任务创建

步骤 4：Dispatcher 分发

文件 : dispatcher.cpp:350-359

void Dispatcher::DispatchAnimation(int32_t animation_id, bool interrupt) {
    auto task = TaskFactory::GetInstance()->CreateTaskAnimation(animation_id, interrupt);
    if (task == nullptr) {
        AIMRTE_WARN("创建Animation任务失败");
        return;
    }
    AIMRTE_INFO("创建Animation task成功， animation_id: {}, interrupt: {}", animation_id, interrupt);
    worker_manager_->ExecTask(task);
}

步骤 5：TaskFactory 创建 Task

文件 : task_factory.cpp:110-127

std::shared_ptr<Task> TaskFactory::CreateTaskAnimation(const int32_t& animation_id, bool interrupt) {
    // 第1步：从 TaskDescriptionManager 获取"说明书"
    auto task_description = TaskDescriptionManager::GetInstance()
        ->GetTaskDescription(robot_type_, TaskDescriptionType::Animation);

    // 第2步：查说明书，生成技能清单
    auto skill_param_list = task_description->GetSkillParamList(animation_id, interrupt);

    // 第3步：打包成 Task 对象
    auto task = std::make_shared<Task>();
    task->SetType(TaskType::AnimationTask);
    task->SetSkillParamList(*skill_param_list);  // 技能清单元塞入 Task
    SetDefaultPriorities(task);
    return task;
}

步骤 6：TaskDescriptionManager 两层查找

文件 : task_description_manager.cpp:50-87

数据结构：

TaskDescriptionManager (全局单例)
  └─ root_task_description_: map<RobotType, RobotTaskDescription>
       ├─ [T1] → T1TaskDescription
       │    └─ task_description_map_: map<TaskDescriptionType, TaskDescription>
       │         ├─ [Animation]   → T1AnimationTaskDescription  ← 变形用这个
       │         ├─ [Move]        → T1MoveTaskDescription
       │         ├─ [Interaction] → T1InteractionTaskDescription
       │         └─ ...
       └─ [Q1] → Q1TaskDescription
            └─ task_description_map_:
                 ├─ [Animation]   → Q1AnimationTaskDescription
                 └─ ...

查找过程（第 80-87 行）：

GetTaskDescription(RobotType::T1, TaskDescriptionType::Animation) {
    // 第一层：按机器人类型找
    auto it = root_task_description_.find(T1);       // → T1TaskDescription
    // 第二层：按任务类型找
    return it->second->GetTaskDescription(Animation); // → T1AnimationTaskDescription
}

初始化注册（第 50-57 行）：

void TaskDescriptionManager::Init() {
    root_task_description_[T1] = new T1TaskDescription();
    root_task_description_[Q1] = new Q1TaskDescription();

    root_task_description_[T1]->RegisterTaskDescription(
        Animation, new T1AnimationTaskDescription());  // T1 注册动画说明书
    // ... 其他类型
}

---

第三阶段：构造技能清单（关键分叉点）

步骤 7：GetSkillParamList --- 根据形态分叉

文件 : t1_animation_task_description.cpp:35-47

std::shared_ptr<SkillParamList> GetSkillParamList(int32_t animation_id, bool interrupt) {
    auto animation = static_cast<T1Animation>(animation_id);  // 8 → kTransformateRotate
    auto skill_param_list = std::make_shared<SkillParamList>();

    if (curr_robot_form == kQuadruped) {
        GetQuadrupedAnimationParam(animation, skill_param_list);  // 路径 A
    } else if (curr_robot_form == kBiped) {
        GetBipedAnimationParam(animation, skill_param_list);      // 路径 B
    }
    return skill_param_list;
}

这是整个变形流程最关键的分叉点！ curr_robot_form 的值决定走哪条路径，而 curr_robot_form 是 ROS2 异步回调更新的（见步骤 12），可能存在延迟。

步骤 8A：路径 A --- 四足变双足

文件 : t1_animation_task_description.cpp:73-135

case kTransformateRotate:
    target_action_id = 300;  // BIPED_LOCOMOTION_WBC（最终目标）

然后执行注入逻辑（第 119-134 行），按顺序 Push 技能：

Skill 序列:
┌─────────────────────────────────────────────────────────┐
│ 1. ActionParams                                         │
│    action_id = 102 (QUADRUPED_LOCOMOTION_DEFAULT)       │
│    check_set = true    ← 阻塞等待 MC 确认！              │
│    作用：确保机器人先站起到行走态，再变形                  │
│                                                         │
│ 2. AudioParams                                          │
│    quadruped_audio = "transform4to2.wav" (四变双音效)    │
│    biped_audio     = "transform2to4.wav" (双变四音效)    │
│    实际播哪个由 AudioSkill 根据当前形态决定               │
│                                                         │
│ 3. MotorParams                                          │
│    motor_type = Biped (切双足舵机)                       │
│                                                         │
│ 4. ActionParams                                         │
│    action_id = 300 (BIPED_LOCOMOTION_WBC)               │
│    check_set = true    ← 阻塞等待 MC 确认！              │
│    作用：最终目标，进入双足 WBC 控制                      │
└─────────────────────────────────────────────────────────┘

音频延迟的关键原因：Action(102, check_set=true) 排在 Audio 前面。ActionSkill 会用 while+sleep(100ms) 循环轮询等待 MC 确认 action 切换完成。如果 MC 从非行走态（如趴下 110）切换到行走态 102 需要 ~3 秒，音频就被卡了 3 秒。

步骤 8B：路径 B --- 双足变四足

文件 : t1_animation_task_description.cpp:180-205

case kTransformateRotate: {
    // 如果当前不是 WBC，先切到 WBC
    if (curr_action_id != 300) {
        Push ActionParams(300, check_set=false);  // 不阻塞！
    }
    // 旋转变形动作
    Push ActionParams(21, check_set=false);       // 不阻塞！(BIPED_TO_QUADRUPED_ROTATE)
    // 音频
    Push AudioParams(transform4to2/transform2to4);
    // 电机切四足
    Push MotorParams(Quad);
    return;  // 注意：直接 return，不走后面的通用逻辑
}

Skill 序列:
┌─────────────────────────────────────────────────────────┐
│ 1. ActionParams (条件注入)                               │
│    action_id = 300 (BIPED_LOCOMOTION_WBC)               │
│    check_set = false   ← 不阻塞！发完 RPC 直接返回        │
│                                                         │
│ 2. ActionParams                                         │
│    action_id = 21 (BIPED_TO_QUADRUPED_ROTATE)           │
│    check_set = false   ← 不阻塞！                        │
│                                                         │
│ 3. MotorParams                                          │
│    motor_type = Quad (切四足舵机)                        │
│                                                         │
│ 4. AudioParams                                          │
│    quadruped_audio = "transform4to2.wav"                │
│    biped_audio     = "transform2to4.wav"                │
│    实际播哪个由 AudioSkill 根据当前形态决定               │
└─────────────────────────────────────────────────────────┘

双足变形没有延迟的原因：所有 Action 都没设 check_set=true，发完 RPC 直接过。音频虽然排在最后，但前面没有阻塞等待。

两条路径的核心差异

	路径 A（四足→双足）	路径 B（双足→四足）
中间过渡	Action 102 (check_set=true)	Action 300 (check_set=false)
变形动作	Action 300 (check_set=true)	Action 21 (check_set=false)
音频位置	第 2 位，排在阻塞 Action 后	第 4 位，前面无阻塞
阻塞 Action 数	2 个	0 个
音频延迟	0~3.2 秒（取决于当前状态）	~80ms

---

第四阶段：Worker 调度与任务执行

步骤 9：WorkerManager 仲裁与分发

文件 : worker_manager.cpp:52-149

bool WorkerManager::ExecTask(const std::shared_ptr<Task>& task) {
    // 1. 规则检查
    if (!ApplyRule(task)) return false;

    // 2. 仲裁：与当前运行的 Worker 比较优先级
    //    如果新任务优先级更高 → 停止旧 Worker
    //    如果被拒绝 → 返回 false

    // 3. 创建新 Worker，绑定 Task
    auto worker = std::make_shared<Worker>();
    worker->SetCurrentTask(task);
    active_workers_[AnimationTask] = worker;

    // 4. 投递到线程池执行
    option_.exe_.Post([worker, task]() {
        worker->ExecTask(task);
    });
}

步骤 10：Worker 驱动 Task 执行

文件 : worker.cpp:17-36

void Worker::ExecTask(const std::shared_ptr<Task>& task) {
    current_task_ = task;
    task->Run();  // 同步执行，阻塞到全部技能完成或被打断
    current_task_ = nullptr;
}

步骤 11：Task::Run --- 按序执行技能清单

文件 : task.cpp:37-73

void Task::Run() {
    auto skill_params = skill_param_list_.GetSkillsParam();
    task_state_ = TaskState::Running;
    param_index_ = 0;

    while (param_index_ < skill_params.size()) {
        if (task_state_ == TaskState::Stop) {
            sleep(100ms);  // 被中断时等待
            continue;
        }

        auto skill_param = skill_params[param_index_];
        auto skill = SkillManager::GetInstance()->GetSkill(skill_param->GetType());

        if (skill) {
            skill->Exec(skill_param.get());  // ← 同步调用，逐个执行
        }

        ++param_index_;
    }
    task_state_ = TaskState::Ready;
}

Worker 在一个单独的线程中同步执行 Task::Run()，按 SkillParamList 的顺序逐个调用 skill->Exec()。 这是同步阻塞模型------每个 skill 的 Exec() 必须返回，下一个才能执行。

---

第五阶段：技能执行

步骤 12A：ActionSkill::Exec --- 动作切换

文件 : action_skill.cpp:24-84

bool ActionSkill::Exec(SkillParam* param) {
    ActionParams* action_param = dynamic_cast<ActionParams*>(param);
    int32_t target_action_id = action_param->GetActionId();  // 例如 102

    auto& motion_state = state_manager_->GetMotionState();

    // 快速路径：如果 MC 已经是目标 action，直接返回
    if (motion_state.curr_action_id == target_action_id) {
        AIMRTE_INFO("当前mc已是目标Action, 无需执行");
        return true;  // O(1) 返回
    }

    // 发 RPC 给 MC：请切换到 target_action_id
    req.command.action.value = target_action_id;
    auto res = option_.set_mc_action_proxy.Func.Call(rpc_ctx, req, resp).Sync();

    // 关键：如果 check_set=true，进入同步轮询等待
    if (action_param->GetCheckSet()) {
        uint32_t index = 0;
        sleep(100ms);
        while (index < 50) {  // 最多 50 次 = 5 秒超时
            motion_state = state_manager_->GetMotionState();
            if (motion_state.curr_action_id == target_action_id) {
                break;  // MC 确认了，退出
            }
            // 没确认？重发 RPC，睡 100ms，再检查
            ++index;
            sleep(100ms);
        }
        // 超时检查
        if (motion_state.curr_action_id != target_action_id) {
            AIMRTE_ERROR("超时未能设置成功");
            return false;
        }
    }
    AIMRTE_INFO("执行ActionSkill成功, action_id: {}, curr_action_id: {}",
                target_action_id, motion_state.curr_action_id);
    return true;
}

执行逻辑：

1. 先检查 curr_action_id（StateManager 缓存）是否已经是目标值，是则直接返回
2. 向 MC 发送 SetMcAction RPC
3. 如果 check_set=true，进入 while+sleep(100ms) 轮询，等待 MC 通过 ROS2 topic 确认切换完成
4. MC 确认后返回，否则最多等 5 秒超时

curr_action_id 缓存的更新路径：

MC 执行完动作切换后，通过 ROS2 topic 发布 McCommonState 消息 → Scheduler 订阅回调 OnMcCommonStatus（scheduler.cpp:105）→ 投递到线程池执行 DispatchMcCommonStatus（dispatcher.cpp:100）→ 更新 motion_state.curr_action_id。

步骤 12B：AudioSkill::Exec --- 音频播放

文件 : audio_skill.cpp:24-82

bool AudioSkill::Exec(SkillParam* param) {
    AudioParams* audio_param = dynamic_cast<AudioParams*>(param);

    // 关键：根据当前形态选择音频文件
    std::string audio_file;
    if (motion_state.curr_robot_form == kQuadruped) {
        audio_file = audio_param->GetAudioFileNameQuadruped();  // "transform4to2.wav"
    } else {
        audio_file = audio_param->GetAudioFileNameBiped();      // "transform2to4.wav"
    }

    // 构造 RPC 请求，发给 HAL 音频服务播放
    req.file.file_name = audio_file;
    req.file.file_path = "/robot/data/var/hal_audio/file/";
    auto res = option_.play_audio_file_proxy.Func.Call(rpc_ctx, req, resp).Sync();

    AIMRTE_INFO("播放音频文件成功, audio_file: {}", audio_file);
    return true;
}

音频选择依赖 curr_robot_form 的当前值------如果该值因 ROS2 异步延迟而滞后，就会选错音频文件。

步骤 12C：MotorSkill::Exec --- 舵机切换

文件 : motor_skill.cpp:23-60

bool MotorSkill::Exec(SkillParam* param) {
    MotorParams* motor_param = dynamic_cast<MotorParams*>(param);
    Q1MotorType motor_type = motor_param->GetMotorType();

    if (motor_type == Quad) {
        req.position = 0;    // 舵机位置 0° = 四足模式
    } else if (motor_type == Biped) {
        req.position = 90;   // 舵机位置 90° = 双足模式
    }

    auto res = option_.set_servo_proxy.Func.Call(rpc_ctx, req, resp).Sync();
    AIMRTE_INFO("执行MotorSkill成功, motor_type: {}", motor_type);
    return true;
}

发送舵机位置指令给硬件，90° 对应双足模式，0° 对应四足模式。

---

第六阶段：状态同步

步骤 13：MC 状态回调

文件 : dispatcher.cpp:100-109

void Dispatcher::DispatchMcCommonStatus(int32_t action_id, uint32_t current_form, ...) {
    auto& motion_state = state_manager_->GetMotionState();

    // 去重：与上次一致则忽略
    if (motion_state.curr_action_id == action_id &&
        motion_state.curr_robot_form == current_form) {
        return;
    }

    // 更新状态缓存
    motion_state.curr_robot_form = static_cast<RobotForm>(current_form);
    motion_state.curr_action_id  = action_id;
}

MC 执行完动作后通过 ROS2 topic 异步推送状态更新。此回调更新 StateManager 中的 curr_robot_form 和 curr_action_id。

---

完整时序图

 用户说"变形"
   │
   ▼
 PlayAnimationService          [scheduler.cpp:254]  ROS2 RPC 回调
   │
   ├─ CheckAnimation           [t1_checker.cpp:25]   6道前置检查
   │    ├─ 形态支持？
   │    ├─ 变形条件？
   │    ├─ 双足action合法？
   │    ├─ 双足变形？→ 直接放行（跳过电量检查）
   │    ├─ 电量/充电拦截
   │    └─ 复杂动作限制
   │
   ├─ Post 到线程池             [scheduler.cpp:262]
   │
   ▼
 DispatchAnimation             [dispatcher.cpp:350]
   │
   ├─ CreateTaskAnimation      [task_factory.cpp:110]
   │    ├─ GetTaskDescription  [task_description_manager.cpp:80]  两层map查找
   │    │    → T1AnimationTaskDescription
   │    │
   │    └─ GetSkillParamList   [t1_animation_task_description.cpp:35]
   │         │
   │         ├─ curr_robot_form == kQuadruped？
   │         │    └─ GetQuadrupedAnimationParam
   │         │         Skill序列: [Action102阻塞, Audio, MotorBiped, Action300阻塞]
   │         │
   │         └─ curr_robot_form == kBiped？
   │              └─ GetBipedAnimationParam
   │                   Skill序列: [Action300不阻塞, Action21, MotorQuad, Audio]
   │
   ├─ ExecTask(task)            [worker_manager.cpp:52]
   │    ├─ 规则检查
   │    ├─ 仲裁（优先级比较）
   │    └─ Post Worker 到线程池
   │
   ▼
 Worker::ExecTask              [worker.cpp:17]
   │
   └─ Task::Run()              [task.cpp:37]
        │
        └─ while 循环逐 Skill 执行:
             │
             ├─ [Skill1] ActionSkill::Exec()    [action_skill.cpp:24]
             │    ├─ MC已到目标? → 跳过
             │    ├─ 发SetMcAction RPC
             │    └─ check_set=true? → while+sleep(100ms)轮询 ← 阻塞点！
             │
             ├─ [Skill2] AudioSkill::Exec()     [audio_skill.cpp:24]
             │    ├─ 读 curr_robot_form → 选音频
             │    └─ 发PlayAudioFile RPC → HAL播放
             │
             ├─ [Skill3] MotorSkill::Exec()     [motor_skill.cpp:23]
             │    └─ 发SetServo RPC → 切舵机
             │
             └─ [Skill4] ActionSkill::Exec()    [action_skill.cpp:24]
                  └─ 最终目标action

                    （异步）MC 执行物理动作
                       │
                       └─ ROS2 topic → OnMcCommonStatus
                            └─ DispatchMcCommonStatus [dispatcher.cpp:100]
                                 └─ 更新 curr_robot_form / curr_action_id

---

涉及的全部源文件

文件		作用
scheduler.cpp		RPC 入口
t1_checker.cpp		前置校验
dispatcher.cpp		状态更新 + 动画分发
task_factory.cpp		Task 创建
task_description_manager.cpp		说明书注册与查找
t1_animation_task_description.cpp		技能清单构造
task.cpp		技能逐个执行
worker_manager.cpp		Worker 仲裁与调度
worker.cpp		Worker 驱动 Task
action_skill.cpp		动作切换（含阻塞轮询）
audio_skill.cpp		音频播放
motor_skill.cpp		舵机切换