R2 梅林区 Move / Fetch 代码详解

本文档详解 /mf_action_seq 数据流，以及 meilin_move.cpp 和 meilin_fetch.cpp 中每个函数的作用和关键代码。

一、数据流全景：`/mf_action_seq` → Move/Fetch

外部规划模块 (mf_action_planner / Web)
  │
  │  ros2 topic pub /mf_action_seq  std_msgs/Float32MultiArray
  │  每 8 个 float 一组: [action_type, row, col, arg3, yaw, 0,0,0]
  ▼
┌──────────────────────────────────────────────────────────────────┐
│ bt_engine_node  (bt_engine_node.cpp)                             │
│                                                                  │
│ mf_action_callback()  第 696 行                                   │
│   │                                                              │
│   ├─ parse_mf_action_seq()  第 586 行                             │
│   │    ├─ 校验: 长度 % 8 == 0, action_type 必须是 0/1             │
│   │    ├─ action_type=0 → make_move_segment()  第 463 行          │
│   │    │    ├─ 校验 grid 范围 (0-5,0-2)                           │
│   │    │    ├─ 校验 height_map 与 planner arg3 一致性             │
│   │    │    └─ 校验方向 (禁止后退)                                 │
│   │    └─ action_type=1 → make_fetch_segment()  第 523 行         │
│   │         ├─ 校验相邻、禁止后退                                 │
│   │         └─ 校验 yaw 与方向一致性                               │
│   │                                                              │
│   ├─ 追加 PLAN_DONE 哨兵 segment (第 709 行)                     │
│   │                                                              │
│   └─ 写入 blackboard:                                            │
│        set("segment_queue", queue)     ← SegmentQueue (deque)     │
│        set("execution_state", "MF_PLAN_READY")                   │
│        set("plan_id", ...)                                       │
│        set("current_segment_index", -1)                          │
│                                                                  │
│ tick_callback()  100Hz 定时器                                     │
│   └─ tree.tickOnce()  → BehaviorTree 遍历                        │
└──────────────────────────────────────────────────────────────────┘
  │
  │  BT XML 树 (full_match.xml / meilin_stage.xml):
  │
  │  <ForEachSegment>                          ← 循环直到 PLAN_DONE
  │    <Sequence>
  │      <PopNextMeilinSegment queue="{segment_queue}"  ← 从队列弹出一个
  │           segment_type="{segment_type}"
  │           move_row="{move_row}"          ─┐
  │           move_col="{move_col}"           │ 写入 blackboard 的
  │           target_height_mm="{target_height_mm}"  │ planner 原始值
  │           kfs_row="{kfs_row}"             │
  │           kfs_col="{kfs_col}"             │
  │           height_diff="{height_diff}"     │
  │           target_yaw="{target_yaw}"/>    ─┘
  │      <SwitchSegmentType>                  ← 按 segment_type 分发
  │        <Move  segment_type="move"         ← 读 blackboard 里 move_row 等
  │              move_row="{move_row}" ... />
  │        <Fetch segment_type="fetch"        ← 读 blackboard 里 kfs_row 等
  │              kfs_row="{kfs_row}" ... />
  │        <ForceSuccess segment_type="PLAN_DONE" />  ← 哨兵: 退出循环
  │      </SwitchSegmentType>
  │    </Sequence>
  │  </ForEachSegment>
  ▼
┌─────────────────────────────────────────┐
│ MeilinMove / MeilinFetch                │
│   onStart() → drive() → onRunning()     │
│                                          │
│   读 blackboard 输入端口                  │
│   计算 grid→world / 爬台 / 抓取位姿       │
│   发 Align / Suspension / Arm goal       │
│   状态机推进，完成后更新 blackboard        │
└─────────────────────────────────────────┘
  │
  │  rclcpp_action::async_send_goal()
  ▼
┌─────────────────────────────────────────┐
│ r2_hardware Action Server               │
│   move_to_pose / suspension_control     │
│   arm_action / spear_action             │
└─────────────────────────────────────────┘

关键点：bt_engine_node 的 mf_action_callback 一次性解析整条 /mf_action_seq 为 Segment 队列写入 blackboard，BT 树每 tick 通过 PopNextMeilinSegment 弹出一个 segment，把它的字段（row/col/height/yaw）写入 blackboard 端口，Move/Fetch 从 blackboard 端口读取输入执行。

二、中间节点：PopNextMeilinSegment

文件: src/TreeAction/r2_bt/src/nodes/pop_next_meilin_segment.cpp

作用

充当 BT 树和 segment_queue 之间的桥梁：从队列头部弹出一个 Segment，把字段写入 blackboard 端口。

`tryPop()` 函数

BT::NodeStatus PopNextMeilinSegment::tryPop()
{
  SegmentQueuePtr queue;
  if (!getInput("queue", queue) || !queue)
  {
    // 队列还不存在 → RUNNING（等待 mf_action_callback 写入）
    config().blackboard->set("execution_state", "WAITING_MF_ACTION_SEQ");
    return BT::NodeStatus::RUNNING;
  }

  std::unique_lock<std::mutex> lock(queue->mtx);
  if (queue->items.empty())
  {
    // 队列空了但还没收到 PLAN_DONE → RUNNING（等待新 plan）
    config().blackboard->set("execution_state", "WAITING_MF_ACTION_SEQ");
    return BT::NodeStatus::RUNNING;
  }

  // 弹出队首 segment
  const Segment segment = queue->items.front();
  queue->items.pop_front();
  lock.unlock();

  // 写入 blackboard 端口（Move/Fetch 通过 InputPort 读取）
  setOutput("segment_type", segment.segment_type);
  setOutput("move_row", segment.move_row);
  setOutput("move_col", segment.move_col);
  setOutput("target_height_mm", segment.target_height_mm);
  setOutput("kfs_row", segment.kfs_row);
  setOutput("kfs_col", segment.kfs_col);
  setOutput("height_diff", segment.height_diff);
  setOutput("target_yaw", segment.target_yaw);

  // 更新调试字段
  config().blackboard->set("current_segment_index", segment.index);
  config().blackboard->set("segment_type", segment.segment_type);
  config().blackboard->set("segment_debug_name", segment.debug_name);
  config().blackboard->set("execution_state", "SEGMENT_LOADED");

  return BT::NodeStatus::SUCCESS;  // 弹出成功，BT 继续执行子节点
}

PopNextMeilinSegment 返回 SUCCESS 后，BT 树的 Sequence 继续执行下一个子节点：SwitchSegmentType，它根据 segment_type 的值分派到 Move 或 Fetch。

三、Segment 数据结构

文件: src/TreeAction/r2_bt/include/r2_bt/segment.hpp

struct Segment
{
  int index = 0;
  std::string segment_type;       // "move" / "fetch" / "PLAN_DONE" / 旧 JSON 类型
  std::string debug_name;

  // === 梅林区 move 参数（planner 原始值）===
  int move_row = 0;
  int move_col = 0;
  double target_height_mm = 0.0;

  // === 梅林区 fetch 参数（planner 原始值）===
  int kfs_row = 0;
  int kfs_col = 0;
  double height_diff = 0.0;       // 正=升, 负=降, 0=无需调整

  // === 共用 ===
  double target_yaw = 0.0;

  // === 以下字段仅用于旧 JSON segment 路径（准备区/竞技区）===
  double target_x = 0.0;
  double target_y = 0.0;
  double max_speed = 0.5;
  double timeout_sec = 30.0;
  // ... 其他旧路径字段
};

梅林区 move/fetch 只使用 row/col/height/yaw 原始值，所有坐标计算在 Move/Fetch 内部完成，Tree XML 不暴露计算结果。

四、MeilinConfig — 共享配置

文件: src/TreeAction/r2_bt/include/r2_bt/segment.hpp

struct MeilinConfig
{
  std::string side = "blue";                // 红蓝方（影响 height_map row=2）
  double grid_size = 1.2;                   // 格子尺寸 (m)
  double grid_origin_x = 1.2;               // (0,0) 格子中心 X
  double grid_origin_y = 1.2;               // (0,0) 格子中心 Y
  double grasp_distance = 0.4;              // 抓取时车身距边线的距离 (m)
  double yaw_tolerance = 0.12;              // yaw 校验容差 (rad)
  double height_tolerance = 1.0;            // 高度差判断阈值 (mm)
  double align_timeout_sec = 30.0;          // Align 超时
  double suspension_timeout_sec = 10.0;     // SuspensionControl 超时
  double arm_timeout_sec = 30.0;            // ArmAction 超时
  double suspension_normal_height = 30.0;   // 正常行驶悬挂高度 H_INIT (mm)
  double pose_timeout_sec = 1.0;            // 定位数据超时
  double cell_center_tolerance = 0.15;      // 格子中心判定容差 (m)
  int rows = 6;
  int cols = 3;
};

bt_engine_node 启动时从 ROS 参数加载，写入 blackboard 的 meilin_config key。

五、工具函数（segment.hpp）

高度地图

inline double meilin_height_at(int row, int col, const std::string& side)

蓝方和红方的 height_map 仅在 row=2 不同（蓝 600/400/200，红 200/400/600）
超出范围返回 0.0

grid ↔ world 坐标转换

// grid → world: 格子中心的世界坐标
inline std::pair<double, double> meilin_grid_to_world(int row, int col, const MeilinConfig& cfg)
{
  return {
    cfg.grid_origin_x + row * cfg.grid_size,    // x
    cfg.grid_origin_y + col * cfg.grid_size     // y
  };
}

// world → grid: 世界坐标属于哪个格子（四舍五入）
inline bool meilin_world_to_grid(double x, double y, const MeilinConfig& cfg, int& row, int& col)

方向计算

inline bool meilin_direction_yaw(int from_row, int from_col, int to_row, int to_col,
                                  double& yaw, int& direction, std::string& direction_name)

位移	yaw (rad)	direction 枚举	名称
(+1, 0) 向前	0	0	FORWARD
(0, +1) 向左	π/2	1	LEFT
(0, -1) 向右	−π/2	2	RIGHT
(-1, 0) 向后	π	-1	BACKWARD（禁止）

抓取位姿计算

inline void meilin_calculate_grasp_position(int kfs_row, int kfs_col, double yaw,
                                             const MeilinConfig& cfg,
                                             int from_row, int from_col,
                                             double& out_x, double& out_y, double& out_yaw)

计算 KFS 中心的世界坐标
根据接近方向计算车应该停在 KFS 对面 grid_size/2 + grasp_distance 处

六、meilin_move.cpp 函数详解

文件: src/TreeAction/r2_bt/src/nodes/meilin_move.cpp

头文件: src/TreeAction/r2_bt/include/r2_bt/nodes/actions/meilin_move.hpp

类结构总览

MeilinMove : BT::StatefulActionNode
│
├── 构造函数 & 端口声明
│   ├── MeilinMove()           — 构造，无特殊逻辑
│   └── providedPorts()        — 声明 4 输入 + 1 输出端口
│
├── BT 生命周期 (BehaviorTree 框架调用)
│   ├── onStart()   — tick 首次进入，完成计算，发第一批 action
│   ├── onRunning() — 每 tick 调用，检查 action 完成并推进状态机
│   └── onHalted()  — 被中断时取消所有 goal，防止悬挂一直跑
│
├── 核心状态机
│   └── drive()     — 真正的状态机: START → WAIT_PHASE1 → WAIT_PHASE2 → DONE
│
└── Action Client 封装
    ├── sendAlignGoal()            — 向 /move_to_pose 发 Align goal
    ├── sendSuspensionGoal()       — 向 /suspension_control 发悬挂 goal
    ├── isActionDone/Active/Failed — 状态查询
    ├── failActiveAlignIfTimedOut  — 超时取消 + 标记失败
    ├── allActionsDone()           — 全部 action 都完成?
    └── resetActions()             — 复位所有状态

成员变量

// === 输入端口值（planner 原始值）===
int move_row_ = 0;
int move_col_ = 0;
double target_yaw_ = 0.0;
double target_height_mm_ = 0.0;

// === 内部计算结果 ===
bool pre_align_needed_ = false;       // 是否需要先回格子中心
double pre_align_x_, pre_align_y_, pre_align_yaw_;  // pre_align 目标
bool climb_needed_ = false;           // 是否需要爬台
int climb_mode_ = 0;                  // 1=UP, 2=DOWN
int climb_direction_ = 0;            // 0=FWD, 1=LEFT, 2=RIGHT
double climb_height_mm_ = 0.0;
double target_x_, target_y_;         // 目标格子中心世界坐标
int from_row_, from_col_;           // 起始格子

// === 阶段推进 ===
enum class Phase { START, WAIT_PHASE1, WAIT_PHASE2, DONE };
Phase phase_ = Phase::START;

// === Align Action Client ===
enum class ActionState { IDLE, ACTIVE, DONE, FAILED };
ActionState pre_align_state_ = ActionState::IDLE;
ActionState target_align_state_ = ActionState::IDLE;
rclcpp_action::Client<AlignAction>::SharedPtr align_client_;
std::shared_ptr<AlignGoalHandle> align_goal_handle_;
std::mutex align_mutex_;

// === Suspension Action Client ===
rclcpp_action::Client<SuspensionAction>::SharedPtr suspension_client_;
bool suspension_goal_done_ = false;
BT::NodeStatus suspension_result_ = BT::NodeStatus::FAILURE;
std::mutex suspension_mutex_;

逐函数解析

1. `providedPorts()` (第 272 行)

BT::PortsList MeilinMove::providedPorts()
{
  return {
    BT::InputPort<int>("move_row"),
    BT::InputPort<int>("move_col"),
    BT::InputPort<double>("target_height_mm"),
    BT::InputPort<double>("target_yaw"),
    BT::OutputPort<std::string>("message"),
  };
}

BT XML 中的 {move_row} 等是 blackboard key。PopNextMeilinSegment 先 setOutput 写入 blackboard，Move 的 InputPort 再 getInput 读出。这就是 数据在 BT 节点间传递的方式。

2. `onStart()` (第 283 行) — 最关键的初始化函数

执行 7 个步骤：

Step 1 — 读取 planner 原始输入（第 295-308 行）

const auto move_row = getInput<int>("move_row");
const auto move_col = getInput<int>("move_col");
const auto target_yaw = getInput<double>("target_yaw");
if (!move_row || !move_col || !target_yaw)
{
  // 缺参数即失败
  setOutput("message", "Move missing move_row/move_col/target_yaw");
  return BT::NodeStatus::FAILURE;
}
move_row_ = move_row.value();
move_col_ = move_col.value();
target_yaw_ = target_yaw.value();
target_height_mm_ = getInput<double>("target_height_mm").value_or(0.0);

Step 2 — 读取配置（第 313-318 行）

const auto cfg = config().blackboard->get<MeilinConfigPtr>("meilin_config");
if (!cfg)
{
  setOutput("message", "Missing meilin_config on blackboard");
  return BT::NodeStatus::FAILURE;
}

meilin_config 是 bt_engine_node 启动时从 ROS 参数构建的共享配置。

Step 3 — 读取当前状态，优先使用定位位姿（第 325-395 行）

// 默认从 blackboard 读（上一个 Move/Fetch 留下的模拟状态）
from_row_ = config().blackboard->get<int>("meilin_current_row");
from_col_ = config().blackboard->get<int>("meilin_current_col");
double current_height = config().blackboard->get<double>("meilin_current_height");
double current_yaw = config().blackboard->get<double>("meilin_current_yaw");
bool pose_is_center = config().blackboard->get<bool>("meilin_pose_is_cell_center");

// 如果有新鲜的 /transformed/pose 定位数据，优先使用
const bool pose_received = config().blackboard->get<bool>("meilin_pose_received");
if (pose_received && pose_fresh)
{
  // world → grid 转换得到真实的 row/col
  meilin_world_to_grid(pose_x, pose_y, *cfg, pose_row, pose_col);
  // 计算是否在格子中心
  pose_is_center = center_distance <= cfg->cell_center_tolerance;

  // 用真实位姿覆盖 blackboard 的模拟状态
  config().blackboard->set("meilin_current_row", from_row_);
  config().blackboard->set("meilin_current_col", from_col_);
  config().blackboard->set("meilin_current_height", current_height);
  config().blackboard->set("meilin_current_yaw", current_yaw);
  config().blackboard->set("meilin_pose_is_cell_center", pose_is_center);
}

这是 定位降级策略：优先用 /transformed/pose 的真实位姿，如果定位超时（pose_age_sec > pose_timeout_sec）或未接入，降级使用 blackboard 中的模拟状态。

Step 4 — grid → world 坐标转换（第 406-410 行）

const auto [wx, wy] = meilin_grid_to_world(move_row_, move_col_, *cfg);
target_x_ = wx;   // = origin_x + row × grid_size
target_y_ = wy;   // = origin_y + col × grid_size

Step 5 — 爬台判断（第 419-456 行）

const double height_diff = target_height_mm_ - current_height;
climb_needed_ = std::abs(height_diff) > cfg->height_tolerance;
if (climb_needed_)
{
  climb_mode_      = (height_diff > 0.0) ? 1 : 2;  // 1=CLIMB_UP, 2=CLIMB_DOWN
  climb_height_mm_ = std::abs(height_diff);

  // 计算爬台方向（基于 from→to 的相对位置）
  meilin_direction_yaw(from_row_, from_col_, move_row_, move_col_,
                       unused_yaw, climb_direction_, dir_name);
}

判断目标格子高度与当前高度差是否超过阈值（1mm）。如果超过，确定爬台方向（FWD/LEFT/RIGHT，禁止 BACKWARD）。特殊处理：如果上一个 Fetch 留下了 suspension_offset（悬挂已提前升高），计入计算中避免”降回 H_INIT → 再升高”的绕路。

Step 6 — pre_align 判断（第 462-471 行）

pre_align_needed_ = !pose_is_center;
if (pre_align_needed_)
{
  // 目标: 回到当前格子的中心
  const auto [cx, cy] = meilin_grid_to_world(from_row_, from_col_, *cfg);
  pre_align_x_ = cx;
  pre_align_y_ = cy;
  pre_align_yaw_ = current_yaw;
}

如果车不在格子中心（可能是上一个 Fetch 结束后停在格子边线位置），需要先回到格子中心，然后才能导航到目标格子中心。

Step 7 — 发第一批 action（第 476-493 行）

resetActions();
// 初始化悬挂状态
{
  std::lock_guard<std::mutex> lock(suspension_mutex_);
  suspension_goal_done_ = false;
  suspension_result_    = BT::NodeStatus::FAILURE;
  suspension_message_.clear();
  suspension_goal_handle_.reset();
}
phase_ = Phase::START;
phase1_fired_ = false;
phase2_fired_ = false;

setOutput("message", std::string{});
config().blackboard->set("active_action", "Move");
config().blackboard->set("execution_state", "ACTION_RUNNING");

return drive();  // 立即进入状态机

3. `onRunning()` (第 495 行) & `onHalted()` (第 500 行)

onRunning(): 每 tick（10ms @ 100Hz）被 BehaviorTree 调用一次，直接调用 drive()，检查 action 完成情况并推进状态机。
onHalted(): BT 树中断时调用，取消所有进行中的 goal（Align + Suspension），防止悬挂一直转。

void MeilinMove::onHalted()
{
  // 取消悬挂 goal
  {
    std::lock_guard<std::mutex> lock(suspension_mutex_);
    if (suspension_client_ && suspension_goal_handle_)
    {
      suspension_client_->async_cancel_goal(suspension_goal_handle_);
    }
    suspension_goal_done_ = true;
  }
  // 取消 Align goal
  {
    std::lock_guard<std::mutex> lock(align_mutex_);
    if (align_client_ && align_goal_handle_)
    {
      align_client_->async_cancel_goal(align_goal_handle_);
    }
    pre_align_state_ = ActionState::FAILED;
    target_align_state_ = ActionState::FAILED;
  }
}

4. `drive()` — 核心状态机（第 531 行）

这是 Move 最核心的逻辑，被 onStart 和 onRunning 调用，推进 4 个阶段：

Phase::START          ← onStart 进入
  │  并行发出 pre_align + climb (悬挂)
  │  → Phase::WAIT_PHASE1
  ▼
Phase::WAIT_PHASE1     ← 等 pre_align 完成（climb 继续并行）
  │  pre_align FAILED → 整个 Move FAIL
  │  悬挂提前失败 → FAIL（第 597 行: suspension_result_ == FAILURE）
  │  悬挂还在跑 → 不管它，先发 target_align
  │  → 发 target_align goal
  │  → Phase::WAIT_PHASE2
  ▼
Phase::WAIT_PHASE2     ← 等 target_align 和 climb 都完成
  │  任一 FAILED → FAIL
  │  全部 DONE → Phase::DONE
  ▼
Phase::DONE
  │  更新 blackboard:
  │    meilin_current_row/col/height/yaw = 目标值
  │    meilin_pose_is_cell_center = true
  │    meilin_suspension_offset = 0.0 (爬台后悬挂已恢复)
  │  → return SUCCESS

执行时序示意（有 pre_align + climb 的完整路径）：

tick0:  START → 发出 ┬ Align(pre_align)     ┐
                     └ Suspension(climb)     │ 并行
tick1:  pre_align 还在跑 → RUNNING           │
tick2:  pre_align ✓ → 发出 Align(target)     │
        Suspension 还在跑                     │  
tick3:  Align(target) ✓                      │
        Suspension ✓ → DONE                  ┘

Phase::START（第 540 行）：

case Phase::START:
{
  phase_ = Phase::WAIT_PHASE1;

  if (pre_align_needed_)
    sendAlignGoal("pre_align", pre_align_x_, pre_align_y_, pre_align_yaw_);

  if (climb_needed_)
    sendSuspensionGoal();

  if (!pre_align_needed_ && !climb_needed_)
    phase_ = Phase::WAIT_PHASE2;  // 跳过 phase1
  else
    return BT::NodeStatus::RUNNING;
}

Phase::WAIT_PHASE1（第 574 行）：

case Phase::WAIT_PHASE1:
{
  // 检查 pre_align 超时
  if (pre_align_needed_)
  {
    failActiveAlignIfTimedOut("pre_align");
    if (isActionActive("pre_align")) return BT::NodeStatus::RUNNING;
    if (isActionFailed("pre_align")) return BT::NodeStatus::FAILURE;
  }

  // 检查悬挂是否提前失败（不阻塞，只是提前退出）
  if (climb_needed_ && suspension_goal_done_ &&
      suspension_result_ == BT::NodeStatus::FAILURE)
    return BT::NodeStatus::FAILURE;

  // pre_align 完成，发 target_align
  phase_ = Phase::WAIT_PHASE2;
  sendAlignGoal("target_align", target_x_, target_y_, target_yaw_);
  return BT::NodeStatus::RUNNING;
}

Phase::WAIT_PHASE2（第 628 行）：

case Phase::WAIT_PHASE2:
{
  failActiveAlignIfTimedOut("target_align");
  if (!allActionsDone()) return BT::NodeStatus::RUNNING;  // 等全部完成

  if (isActionFailed("target_align")) return BT::NodeStatus::FAILURE;
  if (climb_needed_ && suspension_result_ != BT::NodeStatus::SUCCESS)
    return BT::NodeStatus::FAILURE;

  phase_ = Phase::DONE;
  // fall through
}

Phase::DONE（第 667 行）：

case Phase::DONE:
{
  // 更新 blackboard 给后续节点
  config().blackboard->set("meilin_current_row", move_row_);
  config().blackboard->set("meilin_current_col", move_col_);
  config().blackboard->set("meilin_current_height", target_height_mm_);
  config().blackboard->set("meilin_current_yaw", target_yaw_);
  config().blackboard->set("meilin_pose_is_cell_center", true);
  config().blackboard->set("meilin_suspension_offset", 0.0);  // 爬台后悬挂已恢复
  config().blackboard->set("execution_state", "ACTION_SUCCESS");
  return BT::NodeStatus::SUCCESS;
}

5. `sendAlignGoal()` (第 15 行) — 向 `/move_to_pose` 发 Align 请求

void MeilinMove::sendAlignGoal(const std::string& name,
                               double x, double y, double yaw_rad)

关键设计点：

懒初始化：首次调用时才 create_client（第 50 行）
check-then-act：发出前校验 action_server_is_ready()（第 55 行）
name 参数区分："pre_align" 和 "target_align" 用同一个 client 但不同的状态变量
双层回调：
- goal_response_callback（第 68 行）：goal 被 accept/reject 时触发
- result_callback（第 83 行）：goal 执行完毕时触发，设置 DONE 或 FAILED
线程安全：所有状态修改在 align_mutex_ 保护下进行
yaw 单位转换：goal.yaw_deg = yaw_rad * 180.0 / M_PI

6. `sendSuspensionGoal()` (第 186 行) — 向 `/suspension_control` 发悬挂请求

auto goal = SuspensionAction::Goal();
goal.mode       = static_cast<uint8_t>(climb_mode_);      // 1=UP, 2=DOWN
goal.direction  = static_cast<uint8_t>(climb_direction_); // 0=FWD,1=LEFT,2=RIGHT
goal.height     = static_cast<float>(climb_height_mm_);
goal.timeout_sec = static_cast<float>(timeout_sec);

Move 使用 CLIMB_UP/CLIMB_DOWN 模式，走传感器引导的状态机（与 Fetch 的 MODE_DIRECT 不同）。

7. 辅助函数

isActionDone/Active/Failed(name)（第 108-127 行）：根据 name 查询对应状态变量的当前值
failActiveAlignIfTimedOut(name)（第 129 行）：每 tick 检查 Align 是否超时，如果超时主动 async_cancel_goal 并标记 FAILED
allActionsDone()（第 162 行）：判断条件 — pre_align 和 target_align 都不是 ACTIVE，且如有 climb 则 climb 必须完成
resetActions()（第 172 行）：复位所有 Align 状态为 IDLE

七、meilin_fetch.cpp 函数详解

文件: src/TreeAction/r2_bt/src/nodes/meilin_fetch.cpp

头文件: src/TreeAction/r2_bt/include/r2_bt/nodes/actions/meilin_fetch.hpp

类结构总览

MeilinFetch : BT::StatefulActionNode
│
├── 构造函数 & 端口声明
│   ├── MeilinFetch()          — 构造
│   └── providedPorts()        — 声明 4 输入 + 1 输出
│
├── BT 生命周期
│   ├── onStart()              — 完成计算，发第一批 action
│   ├── onRunning()            — 每 tick 调用
│   └── onHalted()             — 取消所有 goal
│
├── 核心状态机（5 阶段 vs Move 的 4 阶段）
│   └── drive()                — START → WAIT_PHASE1 → START_ARM → WAIT_ARM → DONE
│
└── Action Client 封装
    ├── sendAlignGoal()            — Align 到抓取位姿
    ├── sendArmGoal()              — 发 CMD_GRASP 给机械臂
    ├── sendSuspensionGoal()       — 发 MODE_DIRECT 悬挂
    ├── isActionDone/Active/Failed — 状态查询
    ├── failActiveActionIfTimedOut — 统一超时处理 (Align + Arm)
    ├── allActionsDone()           — 全部 action 都完成?
    └── resetActions()             — 复位所有状态

成员变量

// === 输入端口值 ===
int kfs_row_ = 0;
int kfs_col_ = 0;
double height_diff_ = 0.0;       // 正=升，负=降
double target_yaw_ = 0.0;

// === 内部计算结果 ===
double grasp_x_ = 0.0;
double grasp_y_ = 0.0;
double abs_height_diff_ = 0.0;
int suspension_mode_ = 0;        // 0=无需, 3=MODE_DIRECT
int suspension_direction_ = 0;
double suspension_target_height_ = 30.0;  // 绝对目标高度 (mm)

// === 阶段 ===
enum class Phase { START, WAIT_PHASE1, START_ARM, WAIT_ARM, DONE };
Phase phase_ = Phase::START;

// === Align + Arm Action Client ===
ActionState align_state_ = ActionState::IDLE;
ActionState arm_state_ = ActionState::IDLE;
rclcpp_action::Client<AlignAction>::SharedPtr align_client_;
rclcpp_action::Client<ArmActionT>::SharedPtr arm_client_;
std::mutex align_mutex_, arm_mutex_;

// === Suspension Action Client ===
rclcpp_action::Client<SuspensionAction>::SharedPtr suspension_client_;
bool suspension_goal_done_ = false;
BT::NodeStatus suspension_result_ = BT::NodeStatus::FAILURE;

逐函数解析

1. `providedPorts()` (第 361 行)

BT::PortsList MeilinFetch::providedPorts()
{
  return {
    BT::InputPort<int>("kfs_row"),         // KFS 所在格子行
    BT::InputPort<int>("kfs_col"),         // KFS 所在格子列
    BT::InputPort<double>("height_diff"),  // 正=升，负=降
    BT::InputPort<double>("target_yaw"),
    BT::OutputPort<std::string>("message"),
  };
}

2. `onStart()` (第 372 行) — 初始化

Step 1 — 读取输入（第 384-397 行）

kfs_row_   = getInput<int>("kfs_row").value();
kfs_col_   = getInput<int>("kfs_col").value();
target_yaw_ = getInput<double>("target_yaw").value();
height_diff_ = getInput<double>("height_diff").value_or(0.0);

三个必填端口缺一即 FAIL。

Step 2 — 读取配置和当前状态（第 402-414 行）

const auto cfg = config().blackboard->get<MeilinConfigPtr>("meilin_config");
const int current_row = config().blackboard->get<int>("meilin_current_row");
const int current_col = config().blackboard->get<int>("meilin_current_col");

Step 3 — 计算抓取位姿（第 419-428 行）

double actual_yaw = 0.0;
meilin_calculate_grasp_position(kfs_row_, kfs_col_, target_yaw_, *cfg,
                                current_row, current_col,
                                grasp_x_, grasp_y_, actual_yaw);
target_yaw_ = actual_yaw;

// grasp_x = KFS_x - unit_x × (grid_size/2 + grasp_distance)
// grasp_y = KFS_y - unit_y × (grid_size/2 + grasp_distance)
// 车停在 KFS 对面的边线外 0.4m 处

meilin_calculate_grasp_position 根据从 (current_row, current_col) 到 (kfs_row, kfs_col) 的方向，计算车应该停在 KFS 对面 grid_size/2 + grasp_distance 的位置。

Step 4 — 悬挂高度调整判断（第 438-463 行）

abs_height_diff_ = std::abs(height_diff_);
if (abs_height_diff_ > cfg->height_tolerance)
{
  suspension_mode_ = 3;  // MODE_DIRECT ← 关键区别！
  suspension_direction_ = 0;  // MODE_DIRECT 不使用 direction

  // 绝对目标高度 = 正常行驶高度 + 高度差
  suspension_target_height_ = cfg->suspension_normal_height + height_diff_;
}

Fetch 悬挂与 Move 的关键区别：Fetch 用 MODE_DIRECT (3) 而不是 CLIMB_UP/CLIMB_DOWN (1/2)。原因是 Fetch 的高度调整不是真正的上下台阶，只是升高/降低车身以对准 KFS 高度，不需要走传感器的台阶检测状态机。MODE_DIRECT 直接设置四轮统一目标高度 = 正常行驶高度 + 高度差。

Step 5 — 发第一批 action（第 469-483 行）

resetActions();
phase_ = Phase::START;

setOutput("message", std::string{});
config().blackboard->set("active_action", "Fetch");
config().blackboard->set("execution_state", "ACTION_RUNNING");

return drive();

3. `drive()` — Fetch 状态机（第 530 行）

Fetch 比 Move 多一个 START_ARM 阶段（因为多一个机械臂 Action）：

Phase::START           ← onStart 进入
  │  并行发出 Align(grasp) + Suspension (MODE_DIRECT)
  │  → Phase::WAIT_PHASE1
  ▼
Phase::WAIT_PHASE1     ← 等 Align 和 Suspension 都完成
  │  Align FAILED → FAIL
  │  Suspension FAILED → FAIL
  │  全部 DONE → Phase::START_ARM   ← Align 完成后才能抓！
  ▼
Phase::START_ARM       ← 发 ArmAction(CMD_GRASP)
  │  → Phase::WAIT_ARM
  ▼
Phase::WAIT_ARM        ← 等 ArmAction 完成
  │  FAILED → FAIL
  │  DONE → Phase::DONE
  ▼
Phase::DONE
  │  更新 blackboard:
  │    meilin_current_yaw = target_yaw_
  │    meilin_pose_is_cell_center = false   ← 车停在 KFS 边线旁！
  │    meilin_suspension_offset = height_diff_  ← 留给下个 Move 用
  │  → return SUCCESS

执行时序示意（有悬挂的完整路径）：

tick0:  START → 发出 ┬ Align(grasp)   ┐
                     └ Suspension     │ 并行
tick1:  还在跑 → RUNNING              │
tick2:  Align ✓                        │
        Suspension ✓ → START_ARM       ┘
        发出 ArmAction(GRASP)          ← 串行：到位后才能抓
tick3:  ArmAction ✓ → DONE

为什么 Align 和 ArmAction 串行？ 注释说得很清楚：“不能边动边抓”（不能边动边抓）。机械臂抓取时必须车身静止，否则会撞到 KFS。

4. `sendAlignGoal()` (第 15 行) — 向 `/move_to_pose` 发 Align goal

与 Move 的 sendAlignGoal 类似，但：

没有 name 参数（Fetch 只有一个 Align goal，不需要区分 pre/target）
目标是 grasp_x_, grasp_y_（KFS 对面 0.4m 处）

auto goal = AlignAction::Goal();
goal.x = grasp_x_;
goal.y = grasp_y_;
goal.yaw_deg = target_yaw_ * 180.0 / M_PI;

5. `sendArmGoal()` (第 94 行) — 向 `/arm_action` 发 CMD_GRASP

auto goal = ArmActionT::Goal();
goal.command = ArmActionT::Goal::CMD_GRASP;   // 抓取命令
goal.wait_result = true;                       // 等抓取完成再回结果
goal.timeout_sec = static_cast<float>(timeout_sec);

与 Align 相同的异步 Action Client 模式（懒初始化、check-then-act、双层回调、mutex 保护）。

6. `failActiveActionIfTimedOut(name)` (第 197 行)

统一处理 Align 和 Arm 两种超时（根据 name 区分 timeout 配置和状态变量），与 Move 的 failActiveAlignIfTimedOut 相同模式。

7. `allActionsDone()` (第 249 行)

bool MeilinFetch::allActionsDone() const
{
  if (align_state_ == ActionState::ACTIVE) return false;
  if (arm_state_   == ActionState::ACTIVE) return false;
  if (suspension_mode_ != 0 && !suspension_goal_done_) return false;
  return true;
}

3 个条件必须全部满足（Align 不在 ACTIVE、Arm 不在 ACTIVE、如有悬挂则悬挂已完成）。

八、Move 与 Fetch 的关键差异总结

	MeilinMove	MeilinFetch
输入端口	`move_row`, `move_col`, `target_height_mm`, `target_yaw`	`kfs_row`, `kfs_col`, `height_diff`, `target_yaw`
pre_align	✅ 有（先回格中心再导航）	❌ 无（假设已在格中心，Fetch 要求 `pose_is_cell_center=true`）
悬挂模式	`CLIMB_UP(1) / CLIMB_DOWN(2)` — 传感器引导的台阶爬升	`MODE_DIRECT(3)` — 直接设四轮统一目标高度
机械臂	❌ 无	✅ `CMD_GRASP`（Align 完成后串行发出）
执行结束后 `pose_is_cell_center`	`true`（停在目标格中心）	`false`（停在 KFS 对面边线旁！）
`suspension_offset`	复位为 `0.0`（爬台后悬挂已恢复到正常高度）	留存 `height_diff` 给下个 Move，避免”降回→再升”绕路
状态机阶段数	4 (START → WAIT_P1 → WAIT_P2 → DONE)	5 (START → WAIT_P1 → START_ARM → WAIT_ARM → DONE)
并行/串行关系	两个 Align 串行（pre_align → target_align），Suspension 始终与 Align 并行	Align + Suspension 并行，全部完成后串行发 ArmAction
取消逻辑 (onHalted)	取消 Align + Suspension	取消 Align + Arm + Suspension

九、完整数据流速览（实例）

外部发一条 /mf_action_seq:
  {data: [0, 1, 1, 400, 0, 0,0,0,   1, 2, 1, 200, 0, 0,0,0]}
          └─ move(1,1) ─────────┘   └─ fetch KFS(2,1) ────┘

1. mf_action_callback 收到 → parse_mf_action_seq:
   Segment[0] = { type:"move",  move_row:1, move_col:1, target_height_mm:400, target_yaw:0 }
   Segment[1] = { type:"fetch", kfs_row:2,  kfs_col:1,  height_diff:200,       target_yaw:0 }
   Segment[2] = { type:"PLAN_DONE"}  ← 哨兵，ForEachSegment 检测到后退出循环

2. 写入 blackboard: segment_queue → execution_state = "MF_PLAN_READY"

3. BT 树 tick (100Hz):
   PopNextMeilinSegment → 弹出 Segment[0]
     setOutput → blackboard: move_row=1, move_col=1, target_height_mm=400, target_yaw=0
     setOutput → blackboard: segment_type="move"

4. SwitchSegmentType 匹配 "move" → MeilinMove::onStart():
   读: move_row=1, move_col=1, target_height_mm=400, target_yaw=0
   定位: /transformed/pose → grid=(0,0), height=0, center=true
   grid→world(1,1) → target_x=2.4, target_y=2.4
   height_diff=400 → climb_needed=true, climb_mode=1(UP), climb_height=400mm
   pose_is_center=true → pre_align_needed=false
   发 Suspension(UP, FWD, 400mm)  ← 仅有爬台，无 pre_align
   进入 WAIT_PHASE1

5. 每 tick 推进:
   WAIT_PHASE1 → Suspension 还在跑? RUNNING
   WAIT_PHASE1 → pre_align 本来就 false, 直接跳过
               发 Align(target: 2.4, 2.4, yaw=0°)  ← 导航到(1,1)格子中心
   WAIT_PHASE2 → Align 还在跑? RUNNING
   WAIT_PHASE2 → Align ✓ + Suspension ✓ → DONE
   更新 blackboard: current_row=1, col=1, height=400, yaw=0, center=true

6. Move 返回 SUCCESS → BT 继续:
   PopNextMeilinSegment → 弹出 Segment[1]
     setOutput → blackboard: kfs_row=2, kfs_col=1, height_diff=200, target_yaw=0
     setOutput → blackboard: segment_type="fetch"

7. SwitchSegmentType 匹配 "fetch" → MeilinFetch::onStart():
   读: kfs_row=2, kfs_col=1, height_diff=200, target_yaw=0
   当前状态: row=1, col=1, height=400
   计算抓取位姿: KFS(2,1)中心 → 从(1,1)方向是 forward
     grasp_x = KFS_x - 1*(grid_size/2+grasp_distance) = KFS_x - (0.6+0.4)
     车停在 KFS 对面 1.0m 处
   悬挂: MODE_DIRECT, target_height = 30 + 200 = 230mm
   发 Align(grasp) + Suspension(MODE_DIRECT) ← 并行

8. 每 tick 推进:
   WAIT_PHASE1 → 等 Align + Suspension 都完成
   WAIT_PHASE1 → 全部 DONE → START_ARM
   发 ArmAction(CMD_GRASP) ← 串行
   WAIT_ARM → ArmAction ✓ → DONE
   更新 blackboard: yaw=0, center=false, suspension_offset=200

9. Fetch 返回 SUCCESS → BT 继续:
   PopNextMeilinSegment → 弹出 Segment[2] = "PLAN_DONE"
   SwitchSegmentType → ForceSuccess → ForEachSegment 检测到 PLAN_DONE → 退出梅林区

10. 梅林子树返回 SUCCESS → BT 进入 FinalArea（竞技区）

十、关键文件索引

文件	路径
BT 引擎主节点	src/TreeAction/r2_bt/src/bt_engine_node.cpp
MeilinMove 实现	src/TreeAction/r2_bt/src/nodes/meilin_move.cpp
MeilinMove 头文件	src/TreeAction/r2_bt/include/r2_bt/nodes/actions/meilin_move.hpp
MeilinFetch 实现	src/TreeAction/r2_bt/src/nodes/meilin_fetch.cpp
MeilinFetch 头文件	src/TreeAction/r2_bt/include/r2_bt/nodes/actions/meilin_fetch.hpp
PopNextMeilinSegment	src/TreeAction/r2_bt/src/nodes/pop_next_meilin_segment.cpp
Segment 数据结构 & 工具函数	src/TreeAction/r2_bt/include/r2_bt/segment.hpp
全场行为树 XML	src/TreeAction/r2_bt/trees/full_match.xml
梅林段行为树 XML	src/TreeAction/r2_bt/trees/meilin_stage.xml
全场实车启动	src/TreeAction/r2_bringup/launch/r2_full.launch.py

R2 梅林区 Move / Fetch 代码详解

一、数据流全景：/mf_action_seq → Move/Fetch

二、中间节点：PopNextMeilinSegment

作用

tryPop() 函数

三、Segment 数据结构

四、MeilinConfig — 共享配置

五、工具函数（segment.hpp）

高度地图

grid ↔ world 坐标转换

方向计算

抓取位姿计算

六、meilin_move.cpp 函数详解

类结构总览

成员变量

逐函数解析

1. providedPorts() (第 272 行)

2. onStart() (第 283 行) — 最关键的初始化函数

3. onRunning() (第 495 行) & onHalted() (第 500 行)

4. drive() — 核心状态机（第 531 行）

5. sendAlignGoal() (第 15 行) — 向 /move_to_pose 发 Align 请求

6. sendSuspensionGoal() (第 186 行) — 向 /suspension_control 发悬挂请求

7. 辅助函数

七、meilin_fetch.cpp 函数详解

类结构总览

成员变量

逐函数解析

1. providedPorts() (第 361 行)

2. onStart() (第 372 行) — 初始化

3. drive() — Fetch 状态机（第 530 行）

4. sendAlignGoal() (第 15 行) — 向 /move_to_pose 发 Align goal

5. sendArmGoal() (第 94 行) — 向 /arm_action 发 CMD_GRASP

6. failActiveActionIfTimedOut(name) (第 197 行)

7. allActionsDone() (第 249 行)

八、Move 与 Fetch 的关键差异总结

九、完整数据流速览（实例）

十、关键文件索引

一、数据流全景：`/mf_action_seq` → Move/Fetch

`tryPop()` 函数

1. `providedPorts()` (第 272 行)

2. `onStart()` (第 283 行) — 最关键的初始化函数

3. `onRunning()` (第 495 行) & `onHalted()` (第 500 行)

4. `drive()` — 核心状态机（第 531 行）

5. `sendAlignGoal()` (第 15 行) — 向 `/move_to_pose` 发 Align 请求

6. `sendSuspensionGoal()` (第 186 行) — 向 `/suspension_control` 发悬挂请求

1. `providedPorts()` (第 361 行)

2. `onStart()` (第 372 行) — 初始化

3. `drive()` — Fetch 状态机（第 530 行）

4. `sendAlignGoal()` (第 15 行) — 向 `/move_to_pose` 发 Align goal

5. `sendArmGoal()` (第 94 行) — 向 `/arm_action` 发 CMD_GRASP

6. `failActiveActionIfTimedOut(name)` (第 197 行)

7. `allActionsDone()` (第 249 行)