机器人旋转变形逻辑分析

机器人变形逻辑分析概述本文档从代码层面详细描述机器人执行旋转变形kTransformateRotateanimation_id8的完整流程从用户说出变形到机器人完成物理变形动作。涉及的核心模块模块职责SchedulerROS2 RPC 入口接收外部请求Checker (T1Checker)前置条件校验Dispatcher任务分发TaskFactory创建 Task 对象TaskDescriptionManager管理不同机型(T1/Q1)的任务说明书T1AnimationTaskDescriptionT1 动画任务说明书构造技能清单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 入口与前置校验步骤 1ROS2 RPC 回调触发文件: scheduler.cpp:254-267aimrt::rpc::StatusScheduler::PlayAnimationService(constaimdk_msgs::srv::PlayAnimation::Requestreq,aimdk_msgs::srv::PlayAnimation::Responseres){AIMRTE_INFO(Received PlayAnimation request with animation_id{},req.animation_id);// 日志: [20:45:29.299] Received PlayAnimation request with animation_id8当 App/语音/遥控器通过 ROS2 调用PlayAnimationRPC 服务时Scheduler 的PlayAnimationService回调被触发。req.animation_id 8。步骤 2前置校验 — CheckAnimation文件: scheduler.cpp:257if(dispatcher_-CheckAnimation(req.animation_id)!kSuccess){res.successfalse;return{};}文件: t1_checker.cpp:25-58Checker 按顺序执行 5 道检查第 1 关形态是否支持此动画if(notCheckAnimationSupport(animation))returnkFormNotSupportCurrAnimation;四足支持列表kGreet, kHandshake, kSitDown, kLieDown, kStandUp,kTransformateRotate, kProud, kJoy, kStretch, kDance双足支持列表kGreet, kHandshake, kBothHandsMakeHeart, kDance,kTransformateRotate, kPhotoPose_1~4, kPhotoThreeShot, kTalking, kProud, kJoy, kRightHandPunch, kLeftHandPunch, kBipedDefaultPoskTransformateRotate(8) 在两个列表中都有无论当前什么形态都能通过第 2 关四足变形条件四足形态 kTransformateRotate 时才检查if(notCheckQuadrupedTransformateCondition(animation))returnkTransformateConditionNotMet;只在 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(notCheckBipedAction())returnkBipedActionNotSupportAni;只在双足形态下检查要求当前 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(animationkSitDown||animationkLieDown||(animationkTransformateRotatecurr_robot_formkBiped)){returnkSuccess;// 直接通过跳过电量检查}双足形态下的 kTransformateRotate 在此处直接返回成功跳过第 5/6 关的电量检查。这是设计意图双足变形被视为必须响应的保底动作。第 5 关电量/充电拦截全部动画boolis_easy_limit(!IsIgnoreLowBatteryLimit()last_battery_level_audio4)||is_charging;if(is_easy_limit)returnkFormNotSupportCurrAnimation;第 6 关电量/充电拦截仅复杂动作boolis_complex_limit(同上条件);if(is_complex_limit!IsEasyMotion(animation))returnkFormNotSupportCurrAnimation;只有简单动作kGreet, kHandshake, kBothHandsMakeHeart, 拍照系列, 碰拳在低电量/充电时允许执行。kTransformateRotate 不是简单动作但如果走到这里四足形态充电时会被拦截。步骤 3投递到线程池文件: scheduler.cpp:262-264option_.exe_.Post([this,animation_idreq.animation_id,interruptreq.interrupt](){dispatcher_-DispatchAnimation(animation_id,interrupt);});res.successtrue;// 立即返回成功异步执行RPC 立即返回成功给调用方实际动画执行在线程池中异步处理。第二阶段任务创建步骤 4Dispatcher 分发文件: dispatcher.cpp:350-359voidDispatcher::DispatchAnimation(int32_tanimation_id,boolinterrupt){autotaskTaskFactory::GetInstance()-CreateTaskAnimation(animation_id,interrupt);if(tasknullptr){AIMRTE_WARN(创建Animation任务失败);return;}AIMRTE_INFO(创建Animation task成功 animation_id: {}, interrupt: {},animation_id,interrupt);worker_manager_-ExecTask(task);}步骤 5TaskFactory 创建 Task文件: task_factory.cpp:110-127std::shared_ptrTaskTaskFactory::CreateTaskAnimation(constint32_tanimation_id,boolinterrupt){// 第1步从 TaskDescriptionManager 获取说明书autotask_descriptionTaskDescriptionManager::GetInstance()-GetTaskDescription(robot_type_,TaskDescriptionType::Animation);// 第2步查说明书生成技能清单autoskill_param_listtask_description-GetSkillParamList(animation_id,interrupt);// 第3步打包成 Task 对象autotaskstd::make_sharedTask();task-SetType(TaskType::AnimationTask);task-SetSkillParamList(*skill_param_list);// 技能清单元塞入 TaskSetDefaultPriorities(task);returntask;}步骤 6TaskDescriptionManager 两层查找文件: task_description_manager.cpp:50-87数据结构TaskDescriptionManager (全局单例) └─ root_task_description_: mapRobotType, RobotTaskDescription ├─ [T1] → T1TaskDescription │ └─ task_description_map_: mapTaskDescriptionType, TaskDescription │ ├─ [Animation] → T1AnimationTaskDescription ← 变形用这个 │ ├─ [Move] → T1MoveTaskDescription │ ├─ [Interaction] → T1InteractionTaskDescription │ └─ ... └─ [Q1] → Q1TaskDescription └─ task_description_map_: ├─ [Animation] → Q1AnimationTaskDescription └─ ...查找过程第 80-87 行GetTaskDescription(RobotType::T1,TaskDescriptionType::Animation){// 第一层按机器人类型找autoitroot_task_description_.find(T1);// → T1TaskDescription// 第二层按任务类型找returnit-second-GetTaskDescription(Animation);// → T1AnimationTaskDescription}初始化注册第 50-57 行voidTaskDescriptionManager::Init(){root_task_description_[T1]newT1TaskDescription();root_task_description_[Q1]newQ1TaskDescription();root_task_description_[T1]-RegisterTaskDescription(Animation,newT1AnimationTaskDescription());// T1 注册动画说明书// ... 其他类型}第三阶段构造技能清单关键分叉点步骤 7GetSkillParamList — 根据形态分叉文件: t1_animation_task_description.cpp:35-47std::shared_ptrSkillParamListGetSkillParamList(int32_tanimation_id,boolinterrupt){autoanimationstatic_castT1Animation(animation_id);// 8 → kTransformateRotateautoskill_param_liststd::make_sharedSkillParamList();if(curr_robot_formkQuadruped){GetQuadrupedAnimationParam(animation,skill_param_list);// 路径 A}elseif(curr_robot_formkBiped){GetBipedAnimationParam(animation,skill_param_list);// 路径 B}returnskill_param_list;}这是整个变形流程最关键的分叉点curr_robot_form的值决定走哪条路径而curr_robot_form是 ROS2 异步回调更新的见步骤 12可能存在延迟。步骤 8A路径 A — 四足变双足文件: t1_animation_task_description.cpp:73-135casekTransformateRotate:target_action_id300;// 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_settrue) 排在 Audio 前面。ActionSkill 会用 whilesleep(100ms) 循环轮询等待 MC 确认 action 切换完成。如果 MC 从非行走态如趴下 110切换到行走态 102 需要 ~3 秒音频就被卡了 3 秒。步骤 8B路径 B — 双足变四足文件: t1_animation_task_description.cpp:180-205casekTransformateRotate:{// 如果当前不是 WBC先切到 WBCif(curr_action_id!300){PushActionParams(300,check_setfalse);// 不阻塞}// 旋转变形动作PushActionParams(21,check_setfalse);// 不阻塞(BIPED_TO_QUADRUPED_ROTATE)// 音频PushAudioParams(transform4to2/transform2to4);// 电机切四足PushMotorParams(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_settrue发完 RPC 直接过。音频虽然排在最后但前面没有阻塞等待。两条路径的核心差异路径 A四足→双足路径 B双足→四足中间过渡Action 102 (check_settrue)Action 300 (check_setfalse)变形动作Action 300 (check_settrue)Action 21 (check_setfalse)音频位置第 2 位排在阻塞 Action 后第 4 位前面无阻塞阻塞 Action 数2 个0 个音频延迟0~3.2 秒取决于当前状态~80ms第四阶段Worker 调度与任务执行步骤 9WorkerManager 仲裁与分发文件: worker_manager.cpp:52-149boolWorkerManager::ExecTask(conststd::shared_ptrTasktask){// 1. 规则检查if(!ApplyRule(task))returnfalse;// 2. 仲裁与当前运行的 Worker 比较优先级// 如果新任务优先级更高 → 停止旧 Worker// 如果被拒绝 → 返回 false// 3. 创建新 Worker绑定 Taskautoworkerstd::make_sharedWorker();worker-SetCurrentTask(task);active_workers_[AnimationTask]worker;// 4. 投递到线程池执行option_.exe_.Post([worker,task](){worker-ExecTask(task);});}步骤 10Worker 驱动 Task 执行文件: worker.cpp:17-36voidWorker::ExecTask(conststd::shared_ptrTasktask){current_task_task;task-Run();// 同步执行阻塞到全部技能完成或被打断current_task_nullptr;}步骤 11Task::Run — 按序执行技能清单文件: task.cpp:37-73voidTask::Run(){autoskill_paramsskill_param_list_.GetSkillsParam();task_state_TaskState::Running;param_index_0;while(param_index_skill_params.size()){if(task_state_TaskState::Stop){sleep(100ms);// 被中断时等待continue;}autoskill_paramskill_params[param_index_];autoskillSkillManager::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() 必须返回下一个才能执行。第五阶段技能执行步骤 12AActionSkill::Exec — 动作切换文件: action_skill.cpp:24-84boolActionSkill::Exec(SkillParam*param){ActionParams*action_paramdynamic_castActionParams*(param);int32_ttarget_action_idaction_param-GetActionId();// 例如 102automotion_statestate_manager_-GetMotionState();// 快速路径如果 MC 已经是目标 action直接返回if(motion_state.curr_action_idtarget_action_id){AIMRTE_INFO(当前mc已是目标Action, 无需执行);returntrue;// O(1) 返回}// 发 RPC 给 MC请切换到 target_action_idreq.command.action.valuetarget_action_id;autoresoption_.set_mc_action_proxy.Func.Call(rpc_ctx,req,resp).Sync();// 关键如果 check_settrue进入同步轮询等待if(action_param-GetCheckSet()){uint32_tindex0;sleep(100ms);while(index50){// 最多 50 次 5 秒超时motion_statestate_manager_-GetMotionState();if(motion_state.curr_action_idtarget_action_id){break;// MC 确认了退出}// 没确认重发 RPC睡 100ms再检查index;sleep(100ms);}// 超时检查if(motion_state.curr_action_id!target_action_id){AIMRTE_ERROR(超时未能设置成功);returnfalse;}}AIMRTE_INFO(执行ActionSkill成功, action_id: {}, curr_action_id: {},target_action_id,motion_state.curr_action_id);returntrue;}执行逻辑先检查 curr_action_idStateManager 缓存是否已经是目标值是则直接返回向 MC 发送 SetMcAction RPC如果 check_settrue进入 whilesleep(100ms) 轮询等待 MC 通过 ROS2 topic 确认切换完成MC 确认后返回否则最多等 5 秒超时curr_action_id 缓存的更新路径MC 执行完动作切换后通过 ROS2 topic 发布McCommonState消息 → Scheduler 订阅回调OnMcCommonStatusscheduler.cpp:105→ 投递到线程池执行DispatchMcCommonStatusdispatcher.cpp:100→ 更新motion_state.curr_action_id。步骤 12BAudioSkill::Exec — 音频播放文件: audio_skill.cpp:24-82boolAudioSkill::Exec(SkillParam*param){AudioParams*audio_paramdynamic_castAudioParams*(param);// 关键根据当前形态选择音频文件std::string audio_file;if(motion_state.curr_robot_formkQuadruped){audio_fileaudio_param-GetAudioFileNameQuadruped();// transform4to2.wav}else{audio_fileaudio_param-GetAudioFileNameBiped();// transform2to4.wav}// 构造 RPC 请求发给 HAL 音频服务播放req.file.file_nameaudio_file;req.file.file_path/robot/data/var/hal_audio/file/;autoresoption_.play_audio_file_proxy.Func.Call(rpc_ctx,req,resp).Sync();AIMRTE_INFO(播放音频文件成功, audio_file: {},audio_file);returntrue;}音频选择依赖curr_robot_form的当前值——如果该值因 ROS2 异步延迟而滞后就会选错音频文件。步骤 12CMotorSkill::Exec — 舵机切换文件: motor_skill.cpp:23-60boolMotorSkill::Exec(SkillParam*param){MotorParams*motor_paramdynamic_castMotorParams*(param);Q1MotorType motor_typemotor_param-GetMotorType();if(motor_typeQuad){req.position0;// 舵机位置 0° 四足模式}elseif(motor_typeBiped){req.position90;// 舵机位置 90° 双足模式}autoresoption_.set_servo_proxy.Func.Call(rpc_ctx,req,resp).Sync();AIMRTE_INFO(执行MotorSkill成功, motor_type: {},motor_type);returntrue;}发送舵机位置指令给硬件90° 对应双足模式0° 对应四足模式。第六阶段状态同步步骤 13MC 状态回调文件: dispatcher.cpp:100-109voidDispatcher::DispatchMcCommonStatus(int32_taction_id,uint32_tcurrent_form,...){automotion_statestate_manager_-GetMotionState();// 去重与上次一致则忽略if(motion_state.curr_action_idaction_idmotion_state.curr_robot_formcurrent_form){return;}// 更新状态缓存motion_state.curr_robot_formstatic_castRobotForm(current_form);motion_state.curr_action_idaction_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_settrue? → whilesleep(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.cppRPC 入口t1_checker.cpp前置校验dispatcher.cpp状态更新 动画分发task_factory.cppTask 创建task_description_manager.cpp说明书注册与查找t1_animation_task_description.cpp技能清单构造task.cpp技能逐个执行worker_manager.cppWorker 仲裁与调度worker.cppWorker 驱动 Taskaction_skill.cpp动作切换含阻塞轮询audio_skill.cpp音频播放motor_skill.cpp舵机切换