add motion action, optimize code

2025-10-20 19:35:34 +08:00
parent e342eebb1c
commit 6eb7407100
26 changed files with 3199 additions and 1915 deletions
--- a/src/brain/config/bt_carry_boxes.params.yaml
+++ b/src/brain/config/bt_carry_boxes.params.yaml
@@ -0,0 +1,136 @@
+- name: root
+  params: '{}
+
+    '
+- name: retry_all_action
+  params: '{}
+
+    '
+- name: s1_waist_bend_down
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 0.0
+
+    '
+- name: s2_arm_stretch_out
+  params: 'body_id: 0
+
+    data_type: 0
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s3_hand_pickup
+  params: 'mode: 0
+
+    effort: 0.0
+
+    '
+- name: s4_arm_lift
+  params: 'body_id: 0
+
+    data_type: 0
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s5_waist_bend_up
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 0.0
+
+    '
+- name: s6_waist_turn_around
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 0.0
+
+    '
+- name: s7_leg_move_back
+  params: 'move_up_distance: 0.0
+
+    '
+- name: s8_wheel_move_back
+  params: 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s9_waist_bend_down
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 0.0
+
+    '
+- name: s10_arm_stretch_out
+  params: 'body_id: 0
+
+    data_type: 0
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s11_hand_release
+  params: 'mode: 0
+
+    effort: 0.0
+
+    '
+- name: s12_arm_move_to_snapshot_pose
+  params: 'body_id: 0
+
+    data_type: 0
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s13_camera_take_photo
+  params: 'mode: 0
+
+    effort: 0.0
+
+    '
+- name: s14_waist_bend_up
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 0.0
+
+    '
+- name: s15_arm_retract
+  params: 'body_id: 0
+
+    data_type: 0
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
--- a/src/brain/config/bt_carry_boxes.xml
+++ b/src/brain/config/bt_carry_boxes.xml
@@ -0,0 +1,27 @@
+<root BTCPP_format="4">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <!-- Retry waist + arm + hand until HandControl_H succeeds -->
+      <RetryUntilSuccessful name="retry_all_action" num_attempts="5">
+        <Sequence>
+          <!-- <MoveHome_H name="s0_move_home" /> -->
+          <MoveWaist_H name="s1_waist_bend_down" /> <!--pitch w rad -->
+          <Arm_H name="s2_arm_stretch_out" />
+          <HandControl_H name="s3_hand_pickup" />
+          <Arm_H name="s4_arm_lift" />
+          <MoveWaist_H name="s5_waist_bend_up" />
+          <MoveWaist_H name="s6_waist_turn_around" />
+          <MoveLeg_H name="s7_leg_move_back" />
+          <MoveWheel_H name="s8_wheel_move_back" />
+          <MoveWaist_H name="s9_waist_bend_down" />
+          <Arm_H name="s10_arm_stretch_out" />
+          <HandControl_H name="s11_hand_release" />
+          <Arm_H name="s12_arm_move_to_snapshot_pose" />
+          <CameraTakePhoto_H name="s13_camera_take_photo" />
+          <MoveWaist_H name="s14_waist_bend_up" />
+          <Arm_H name="s15_arm_retract" />
+        </Sequence>
+      </RetryUntilSuccessful>      
+    </Sequence>
+  </BehaviorTree>
+</root>
--- a/src/brain/config/bt_vision_grasp.params.yaml
+++ b/src/brain/config/bt_vision_grasp.params.yaml
@@ -0,0 +1,32 @@
+- name: root
+  params: '{}
+
+    '
+- name: retry_vision_hand
+  params: '{}
+
+    '
+- name: VisionObjectRecognition_H
+  params: '{}
+
+    '
+- name: Arm_H
+  params: 'body_id: 0
+
+    data_type: 0
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: HandControl_H
+  params: 'mode: 0
+
+    effort: 0.0
+
+    '
--- a/src/brain/config/robot_config.yaml
+++ b/src/brain/config/robot_config.yaml
@@ -0,0 +1,11 @@
+- name: brain
+  version: 1.0.0
+  skill_file: "/config/robot_skills.yaml"
+
+- name: cerebrum_node
+  version: 1.0.0
+  bt_config_file: "/config/bt_carry_boxes.xml"
+  bt_params_file: "/config/bt_carry_boxes.params.yaml"
+
+- name: cerebellum_node
+  version: 1.0.0
--- a/src/brain/config/robot_skills.yaml
+++ b/src/brain/config/robot_skills.yaml
@@ -2,19 +2,32 @@
  version: 1.0.0
  description: "视觉识别指定物体"
  interfaces:
-    - VisionObjectRecognition.service
+    - VisionObjectRecognition.srv

- name: ArmSpaceControl
+- name: MapLoad
  version: 1.0.0
-  description: "手臂空间控制"
+  description: "加载地图"
  interfaces:
-    - ArmSpaceControl.action
+    - MapLoad.srv
+
+- name: MapSave
+  version: 1.0.0
+  description: "保存地图"
+  interfaces:
+    - MapSave.srv

 - name: Arm
  version: 1.0.0
  description: "手臂控制"
  interfaces:
-    - Arm.action
+    - name: Arm.action
+      default_topic: "ArmAction"
+
+- name: CameraTakePhoto
+  version: 1.0.0
+  description: "相机拍照"
+  interfaces:
+    - CameraTakePhoto.action

 - name: HandControl
  version: 1.0.0
@@ -22,42 +35,30 @@
  interfaces:
    - HandControl.action

- name: LegControl
+- name: MoveWaist
+  version: 1.0.0
+  description: "腰部控制"
+  interfaces:
+    - name: MoveWaist.action
+      default_topic: ""
+
+- name: MoveLeg
  version: 1.0.0
  description: "腿部控制"
  interfaces:
-    - LegControl.action
+    - name: MoveLeg.action
+      default_topic: ""

- name: SlamMode
+- name: MoveWheel
  version: 1.0.0
-  description: "切换 SLAM 建图模式"
+  description: "轮子控制"
  interfaces:
-    - SlamMode.action
+    - name: MoveWheel.action
+      default_topic: ""

- name: MapSave
+- name: MoveHome
  version: 1.0.0
-  description: "保存导航地图"
+  description: "回到原点位姿"
  interfaces:
-    - MapSave.service
-
- name: MapLoad
-  version: 1.0.0
-  description: "加载导航地图"
-  interfaces:
-    - MapLoad.service
-
- name: NavigateToPose
-  version: 1.0.0
-  description: "在地图中导航至目标位姿"
-  interfaces:
-    - NavigateToPose.action
-
- name: VisionGraspObject
-  version: 1.0.0
-  description: "视觉识别并抓取指定物体"
-  required_skills:
-    - VisionObjectRecognition
-    - ArmSpaceControl
-    - HandControl
-  interfaces:
-    - VisionGraspObject.action
+    - name: MoveHome.action
+      default_topic: ""
--- a/src/brain/include/brain/cerebellum_node.hpp
+++ b/src/brain/include/brain/cerebellum_node.hpp
@@ -10,11 +10,16 @@
 #include <std_msgs/msg/string.hpp>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include <yaml-cpp/yaml.h>
 #include "brain_interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/msg/skill_call.hpp"
 #include "brain/action_registry.hpp"
 #include "brain/skill_manager.hpp"
 #include <smacc2/smacc_signal_detector.hpp>
 #include "brain/sm_cerebellum.hpp"  // for CerebellumData::ExecResult
+// For from_yaml<T>(YAML::Node)
+#include "brain/payload_converters.hpp"
+#include "brain/robot_config.hpp"

 namespace brain
 {
@@ -42,6 +47,9 @@ private:
  std::unique_ptr<ActionClientRegistry> action_clients_;
  std::unique_ptr<SkillManager> skill_manager_;
  std::unique_ptr<BTRegistry> bt_registry_;
+  std::unique_ptr<brain::robot_config::RobotConfig> robot_config_;
+  std::string share_directory_;
+  std::string robot_skill_file_path_;

  // SMACC2 execution context
  std::unique_ptr<smacc2::SmExecution> sm_exec_;
@@ -81,14 +89,68 @@ private:

  // Current accepted sequence epoch (from client) used to filter/reject stale goals
  std::atomic<uint32_t> current_epoch_{0};
+  // Latest ExecuteBtAction calls (Plan A minimal support); guarded by implicit action server thread confinement
+  std::vector<brain_interfaces::msg::SkillCall> last_calls_;
+
+  // Lookup the SkillCall for a given skill name in the latest goal, if any
+  const brain_interfaces::msg::SkillCall * FindCallForSkill(const std::string & name) const
+  {
+    for (const auto & c : last_calls_) {
+      if (c.name == name) return &c;
+    }
+    return nullptr;
+  }
  std::atomic<double> nav2_last_distance_remaining_{std::numeric_limits<double>::quiet_NaN()};
  std::atomic<bool> nav2_last_goal_succeeded_{false};

  // ---- helpers ----
+  /**
+   * @brief Try to parse the current goal's per-call YAML payload into a typed object.
+   * 
+   * Looks up the latest ExecuteBtAction.goal.calls entry matching skill_name, and if
+   * payload_yaml is non-empty attempts to YAML::Load and convert via brain::from_yaml<T>.
+   * On success, writes into 'out' and returns true; on failure returns false and leaves 'out' unchanged.
+   */
+  template<typename T>
+  bool TryParseCallPayload(const std::string & skill_name, T & out) const
+  {
+    if (last_calls_.empty()) {return false;}
+    for (const auto & c : last_calls_) {
+      if (c.name == skill_name && !c.payload_yaml.empty()) {
+        try {
+          auto node = YAML::Load(c.payload_yaml);
+          out = brain::from_yaml<T>(node);
+          RCLCPP_INFO(rclcpp::Node::get_logger(), "[%s] payload_yaml parse success", skill_name.c_str());
+          return true;
+        } catch (const std::exception & e) {
+          RCLCPP_ERROR(rclcpp::Node::get_logger(), "[%s] payload_yaml parse failed: %s",
+            skill_name.c_str(), e.what());
+        }
+        break;
+      }
+    }
+    return false;
+  }
  /** @brief Install application-specific action hook overrides for skill manager. */
  void ConfigureActionHooks();
+  void ConfigureArmSpaceControlHooks();
+  void ConfigureArmHooks();
+  void ConfigureHandControlHooks();
+  void ConfigureCameraTakePhotoHooks();
+  void ConfigureLegControlHooks();
+  void ConfigureVisionGraspObjectHooks();
+  void ConfigureSlamModeHooks();
+  void ConfigureNavigateToPoseHooks();
+  void ConfigureMoveWaistHooks();
+  void ConfigureMoveLegHooks();
+  void ConfigureMoveWheelHooks();
+  void ConfigureMoveHomeHooks();
+
  /** @brief Install application-specific service hook overrides. */
  void ConfigureServiceHooks();
+  void ConfigureVisionObjectRecognitionHooks();
+  void ConfigureMapLoadHooks();
+  void ConfigureMapSaveHooks();

  /**
   * @brief Parse a raw comma / semicolon / whitespace separated list of skill names.
--- a/src/brain/include/brain/cerebrum_node.hpp
+++ b/src/brain/include/brain/cerebrum_node.hpp
@@ -18,6 +18,9 @@
 #include "brain/constants.hpp"
 #include "brain/epoch_filter.hpp"
 #include <std_srvs/srv/trigger.hpp>
+#include "brain_interfaces/msg/skill_call.hpp"
+#include <rclcpp/node_interfaces/node_parameters_interface.hpp>
+#include "brain/robot_config.hpp"


 namespace brain
@@ -123,14 +126,6 @@ public:
  static ExecResultCode ParseResultDetail(const std::string & detail);

 private:
-  /**
-   * Initialize core registry objects (action, BT, skill manager).
-   *
-   * Ownership: Creates unique_ptr instances and wires dependencies.
-   * Must be called first in the constructor before any method that relies on
-   * these registries. Safe to call only once (no internal guards).
-   */
-  void InitializeRegistries();

  /**
   * Declare ROS 2 parameters used by this node.
@@ -150,7 +145,7 @@ private:
   *
   * Returns: Absolute or relative path string (may be empty on failure).
   */
-  std::string ResolveSkillsFile();
+  std::string ResolveSkillsFile(std::string skills_file_path);

  /**
   * Load skill definitions from a YAML file via SkillManager.
@@ -174,24 +169,6 @@ private:
   */
  void LoadParameters();

-  /**
-   * Register per-skill BT node handlers and the unified ExecuteBtAction client.
-   *
-   * This combines two tightly related registration phases to ensure action
-   * client availability for the handlers. Safe to call exactly once during
-   * construction.
-   */
-  void RegisterBtAndActionClients();
-
-  /**
-   * Create periodic timers:
-   *  - bt_timer_: high-frequency BehaviorTree ticking.
-   *  - task_timer_: low-frequency sequence/model update trigger.
-   *
-   * Must be invoked after registries & parameters are ready. Not re-entrant.
-   */
-  void CreateTimers();
-
  /**
   * Create external service interfaces (currently a Trigger to force BT rebuild).
   *
@@ -201,13 +178,28 @@ private:
   */
  void CreateServices();

+  /** Declare per-skill instance parameter entries used to customize outgoing calls. */
+  void UpdateBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name, const std::string & instance_params);
+  /** Declare per-instance parameter entries used to customize a specific BT node occurrence. */
+  void DeclareBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name);
+  /** Build a SkillCall message for given skill by consulting SkillSpec and per-skill params. */
+  brain_interfaces::msg::SkillCall BuildSkillCallForSkill(const std::string & skill_name) const;
+  /** Build a SkillCall with optional per-instance override (cerebrum.bt.<Skill>.<Instance>.*). */
+  brain_interfaces::msg::SkillCall BuildSkillCallForSkill(const std::string & skill_name, const std::optional<std::string> & instance_name) const;
+  /** Install on_set_parameters callback to validate and accept dynamic updates. */
+  void SetupDynamicParameterHandling();
+
  BT::Tree tree_;
  BT::BehaviorTreeFactory factory_;
  std::unique_ptr<ActionClientRegistry> action_registry_;
  std::unique_ptr<BTRegistry> bt_registry_;
  std::unique_ptr<SkillManager> skill_manager_;
+  std::unique_ptr<brain::robot_config::RobotConfig> robot_config_;
  std::string active_sequence_;
+  std::string share_directory_;
+  std::string robot_skill_file_path_;
  std::string bt_config_file_path_;
+  std::string bt_params_file_path_;
  std::filesystem::file_time_type bt_xml_last_write_time_{}; // last observed mod time (for hot reload)
  rclcpp::TimerBase::SharedPtr task_timer_;
  rclcpp::TimerBase::SharedPtr bt_timer_;
@@ -331,9 +323,20 @@ private:
   */
  void RegisterSkillBtActions();

+  /**
+   * @brief Load robot configuration from file.
+   * @return True on success.
+   */
+  bool LoadRobotConfiguration();
+
  // Temporary override used so the generic ExecuteBtAction goal builder can
  // send a single-skill goal when a per-skill BT node starts.
  std::optional<std::string> single_skill_goal_override_;
+  // When a per-skill BT node starts, capture its BT node instance name for per-instance params.
+  std::optional<std::string> single_skill_instance_override_;
+
+  // Handle for dynamic parameter updates (cerebrum.bt.<skill>.*)
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_handle_;

 };

--- a/src/brain/include/brain/payload_converters.hpp
+++ b/src/brain/include/brain/payload_converters.hpp
@@ -0,0 +1,243 @@
+// GENERATED FILE - do not edit by hand
+// Generated by src/scripts/gen_payload_converters.py
+#pragma once
+
+#include <yaml-cpp/yaml.h>
+#include <string>
+#include <vector>
+#include <cstdint>
+#include "interfaces/action/arm.hpp"
+#include "interfaces/action/arm_space_control.hpp"
+#include "interfaces/action/camera_take_photo.hpp"
+#include "interfaces/action/hand_control.hpp"
+#include "interfaces/action/leg_control.hpp"
+#include "interfaces/action/move_home.hpp"
+#include "interfaces/action/move_leg.hpp"
+#include "interfaces/action/move_to_position.hpp"
+#include "interfaces/action/move_waist.hpp"
+#include "interfaces/action/move_wheel.hpp"
+#include "interfaces/action/slam_mode.hpp"
+#include "interfaces/action/vision_grasp_object.hpp"
+#include "interfaces/srv/map_load.hpp"
+#include "interfaces/srv/map_save.hpp"
+#include "interfaces/srv/vision_object_recognition.hpp"
+
+
+namespace brain {
+
+template<typename T>
+T from_yaml(const YAML::Node & n);
+
+// VisionObjectRecognition::Request: camera_position
+template<>
+inline interfaces::srv::VisionObjectRecognition::Request from_yaml<interfaces::srv::VisionObjectRecognition::Request>(const YAML::Node & n)
+{
+  interfaces::srv::VisionObjectRecognition::Request r;
+  if (n && n.IsMap()) {
+    if (n["camera_position"]) r.camera_position = n["camera_position"].as<std::string>();
+  }
+  return r;
+}
+
+// MapLoad::Request: map_url
+template<>
+inline interfaces::srv::MapLoad::Request from_yaml<interfaces::srv::MapLoad::Request>(const YAML::Node & n)
+{
+  interfaces::srv::MapLoad::Request r;
+  if (n && n.IsMap()) {
+    if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
+  }
+  return r;
+}
+
+// MapSave::Request: map_url
+template<>
+inline interfaces::srv::MapSave::Request from_yaml<interfaces::srv::MapSave::Request>(const YAML::Node & n)
+{
+  interfaces::srv::MapSave::Request r;
+  if (n && n.IsMap()) {
+    if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
+  }
+  return r;
+}
+
+// Arm::Goal: body_id, data_type, data_length, command_id, frame_time_stamp, data_array
+template<>
+inline interfaces::action::Arm::Goal from_yaml<interfaces::action::Arm::Goal>(const YAML::Node & n)
+{
+  interfaces::action::Arm::Goal g;
+  if (n && n.IsMap()) {
+    if (n["body_id"]) g.body_id = n["body_id"].as<int8_t>();
+    if (n["data_type"]) g.data_type = n["data_type"].as<int8_t>();
+    if (n["data_length"]) g.data_length = n["data_length"].as<int16_t>();
+    if (n["command_id"]) g.command_id = n["command_id"].as<int64_t>();
+    if (n["frame_time_stamp"]) g.frame_time_stamp = n["frame_time_stamp"].as<int64_t>();
+    if (n["data_array"]) {
+      auto vec = n["data_array"].as<std::vector<double>>();
+      g.data_array.assign(vec.begin(), vec.end());
+    }
+  if (!g.data_array.empty()) {
+    using DL = decltype(g.data_length);
+    g.data_length = static_cast<DL>(g.data_array.size());
+  }
+  }
+  return g;
+}
+
+// CameraTakePhoto::Goal: mode, effort
+template<>
+inline interfaces::action::CameraTakePhoto::Goal from_yaml<interfaces::action::CameraTakePhoto::Goal>(const YAML::Node & n)
+{
+  interfaces::action::CameraTakePhoto::Goal g;
+  if (n && n.IsMap()) {
+    if (n["mode"]) g.mode = n["mode"].as<int32_t>();
+    if (n["effort"]) g.effort = n["effort"].as<float>();
+  }
+  return g;
+}
+
+// HandControl::Goal: mode, effort
+template<>
+inline interfaces::action::HandControl::Goal from_yaml<interfaces::action::HandControl::Goal>(const YAML::Node & n)
+{
+  interfaces::action::HandControl::Goal g;
+  if (n && n.IsMap()) {
+    if (n["mode"]) g.mode = n["mode"].as<int32_t>();
+    if (n["effort"]) g.effort = n["effort"].as<float>();
+  }
+  return g;
+}
+
+// MoveWaist::Goal: move_pitch_degree, move_yaw_degree
+template<>
+inline interfaces::action::MoveWaist::Goal from_yaml<interfaces::action::MoveWaist::Goal>(const YAML::Node & n)
+{
+  interfaces::action::MoveWaist::Goal g;
+  if (n && n.IsMap()) {
+    if (n["move_pitch_degree"]) g.move_pitch_degree = n["move_pitch_degree"].as<float>();
+    if (n["move_yaw_degree"]) g.move_yaw_degree = n["move_yaw_degree"].as<float>();
+  }
+  return g;
+}
+
+// MoveLeg::Goal: move_up_distance
+template<>
+inline interfaces::action::MoveLeg::Goal from_yaml<interfaces::action::MoveLeg::Goal>(const YAML::Node & n)
+{
+  interfaces::action::MoveLeg::Goal g;
+  if (n && n.IsMap()) {
+    if (n["move_up_distance"]) g.move_up_distance = n["move_up_distance"].as<float>();
+  }
+  return g;
+}
+
+// MoveWheel::Goal: move_distance, move_angle
+template<>
+inline interfaces::action::MoveWheel::Goal from_yaml<interfaces::action::MoveWheel::Goal>(const YAML::Node & n)
+{
+  interfaces::action::MoveWheel::Goal g;
+  if (n && n.IsMap()) {
+    if (n["move_distance"]) g.move_distance = n["move_distance"].as<float>();
+    if (n["move_angle"]) g.move_angle = n["move_angle"].as<float>();
+  }
+  return g;
+}
+
+// MoveHome::Goal: 
+template<>
+inline interfaces::action::MoveHome::Goal from_yaml<interfaces::action::MoveHome::Goal>(const YAML::Node & n)
+{
+  interfaces::action::MoveHome::Goal g;
+  if (n && n.IsMap()) {
+  }
+  return g;
+}
+
+// MoveToPosition::Goal: target_x, target_y
+template<>
+inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
+{
+  interfaces::action::MoveToPosition::Goal g;
+  if (n && n.IsMap()) {
+    if (n["target_x"]) g.target_x = n["target_x"].as<float>();
+    if (n["target_y"]) g.target_y = n["target_y"].as<float>();
+  }
+  return g;
+}
+
+// SlamMode::Goal: enable_mapping
+template<>
+inline interfaces::action::SlamMode::Goal from_yaml<interfaces::action::SlamMode::Goal>(const YAML::Node & n)
+{
+  interfaces::action::SlamMode::Goal g;
+  if (n && n.IsMap()) {
+    if (n["enable_mapping"]) g.enable_mapping = n["enable_mapping"].as<bool>();
+  }
+  return g;
+}
+
+// ArmSpaceControl::Goal: target
+template<>
+inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::ArmSpaceControl::Goal>(const YAML::Node & n)
+{
+  interfaces::action::ArmSpaceControl::Goal g;
+  if (n && n.IsMap()) {
+    // geometry_msgs::msg::PoseStamped
+    if (n) {
+      if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
+      if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
+      if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
+      if (auto p = n["position"]) {
+        if (p["x"]) g.target.pose.position.x = p["x"].as<double>();
+        if (p["y"]) g.target.pose.position.y = p["y"].as<double>();
+        if (p["z"]) g.target.pose.position.z = p["z"].as<double>();
+      }
+      if (auto q = n["orientation"]) {
+        if (q["x"]) g.target.pose.orientation.x = q["x"].as<double>();
+        if (q["y"]) g.target.pose.orientation.y = q["y"].as<double>();
+        if (q["z"]) g.target.pose.orientation.z = q["z"].as<double>();
+        if (q["w"]) g.target.pose.orientation.w = q["w"].as<double>();
+      }
+    }
+  }
+  return g;
+}
+
+// LegControl::Goal: target
+template<>
+inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(const YAML::Node & n)
+{
+  interfaces::action::LegControl::Goal g;
+  if (n && n.IsMap()) {
+    // geometry_msgs::msg::TwistStamped
+    if (n) {
+      if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
+      if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
+      if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
+      if (auto lin = n["linear"]) {
+        if (lin["x"]) g.target.twist.linear.x = lin["x"].as<double>();
+        if (lin["y"]) g.target.twist.linear.y = lin["y"].as<double>();
+        if (lin["z"]) g.target.twist.linear.z = lin["z"].as<double>();
+      }
+      if (auto ang = n["angular"]) {
+        if (ang["x"]) g.target.twist.angular.x = ang["x"].as<double>();
+        if (ang["y"]) g.target.twist.angular.y = ang["y"].as<double>();
+        if (ang["z"]) g.target.twist.angular.z = ang["z"].as<double>();
+      }
+    }
+  }
+  return g;
+}
+
+// VisionGraspObject::Goal: object_id
+template<>
+inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action::VisionGraspObject::Goal>(const YAML::Node & n)
+{
+  interfaces::action::VisionGraspObject::Goal g;
+  if (n && n.IsMap()) {
+    if (n["object_id"]) g.object_id = n["object_id"].as<std::string>();
+  }
+  return g;
+}
+
+} // namespace brain
--- a/src/brain/include/brain/robot_config.hpp
+++ b/src/brain/include/brain/robot_config.hpp
@@ -0,0 +1,99 @@
+// GENERATED FILE - do not edit by hand
+// Generated by src/scripts/gen_robot_config.py
+#ifndef BRAIN_ROBOT_CONFIG_HPP_
+#define BRAIN_ROBOT_CONFIG_HPP_
+
+#include <yaml-cpp/yaml.h>
+#include <cstddef>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+namespace brain { namespace robot_config {
+
+// Represents a single entry parsed from robot_config.yaml.
+struct Entry {
+  std::string name;
+  std::unordered_map<std::string, std::string> fields;
+};
+
+// Loader and query interface for robot configuration.
+class RobotConfig {
+ public:
+  RobotConfig() {
+    LoadFromFile(ament_index_cpp::get_package_share_directory("brain") + std::string("/config/robot_config.yaml"));
+  }
+  // Parse YAML from a file path. Returns true on success.
+  bool LoadFromFile(const std::string& path) {
+    entries_.clear();
+    YAML::Node root = YAML::LoadFile(path);
+    if (!root || !root.IsSequence()) { return false; }
+    for (const auto& node : root) {
+      if (!node.IsMap()) { continue; }
+      Entry e;
+      auto name_it = node["name"];
+      if (name_it && name_it.IsScalar()) { e.name = name_it.as<std::string>(); }
+      for (auto it = node.begin(); it != node.end(); ++it) {
+        const auto key = it->first.as<std::string>();
+        if (key == "name") { continue; }
+        if (it->second) {
+          if (it->second.IsScalar()) {
+            e.fields.emplace(key, it->second.as<std::string>());
+          } else {
+            e.fields.emplace(key, YAML::Dump(it->second));
+          }
+        }
+      }
+      if (!e.name.empty()) { entries_.emplace_back(std::move(e)); }
+    }
+    return true;
+  }
+
+  // Return pointer to an Entry with matching name or nullptr if not found.
+  const Entry* Find(const std::string& name) const {
+    for (const auto& e : entries_) { if (e.name == name) return &e; }
+    return nullptr;
+  }
+
+  // List all entry names.
+  std::vector<std::string> Names() const {
+    std::vector<std::string> out; out.reserve(entries_.size());
+    for (const auto& e : entries_) out.push_back(e.name);
+    return out;
+  }
+
+  // Generic value getter by name and key.
+  std::optional<std::string> GetValue(const std::string& name, const std::string& key) const {
+    const Entry* e = Find(name); if (!e) return std::nullopt;
+    auto it = e->fields.find(key);
+    if (it == e->fields.end()) return std::nullopt;
+    return it->second;
+  }
+
+  std::optional<std::string> Version(const std::string& name) const {
+    return GetValue(name, "version");
+  }
+
+  std::optional<std::string> SkillFile(const std::string& name) const {
+    return GetValue(name, "skill_file");
+  }
+
+  std::optional<std::string> BtConfigFile(const std::string& name) const {
+    return GetValue(name, "bt_config_file");
+  }
+
+  std::optional<std::string> BtParamsFile(const std::string& name) const {
+    return GetValue(name, "bt_params_file");
+  }
+
+ private:
+  std::vector<Entry> entries_;
+};
+
+}} // namespace robot_config; } // namespace brain
+
+#endif  // BRAIN_ROBOT_CONFIG_HPP_
--- a/src/brain/include/brain/skill_config.hpp
+++ b/src/brain/include/brain/skill_config.hpp
@@ -0,0 +1,126 @@
+// Generated by scripts/gen_skill_config.py — do not edit by hand unless you know what you are doing.
+#pragma once
+
+#include <tuple>
+#include <string>
+
+#include "interfaces/action/arm.hpp"
+#include "interfaces/action/camera_take_photo.hpp"
+#include "interfaces/action/hand_control.hpp"
+#include "interfaces/action/move_home.hpp"
+#include "interfaces/action/move_leg.hpp"
+#include "interfaces/action/move_waist.hpp"
+#include "interfaces/action/move_wheel.hpp"
+#include "interfaces/srv/map_load.hpp"
+#include "interfaces/srv/map_save.hpp"
+#include "interfaces/srv/vision_object_recognition.hpp"
+
+namespace brain {
+
+template<typename ActionT>
+struct SkillActionTrait;
+
+template<typename ServiceT>
+struct SkillServiceTrait;
+
+using SkillActionTypes = std::tuple<
+  interfaces::action::Arm,
+  interfaces::action::CameraTakePhoto,
+  interfaces::action::HandControl,
+  interfaces::action::MoveWaist,
+  interfaces::action::MoveLeg,
+  interfaces::action::MoveWheel,
+  interfaces::action::MoveHome
+>;
+
+using SkillServiceTypes = std::tuple<
+  interfaces::srv::VisionObjectRecognition,
+  interfaces::srv::MapLoad,
+  interfaces::srv::MapSave
+>;
+
+template<>
+struct SkillActionTrait<interfaces::action::Arm>
+{
+  static constexpr const char * skill_name = "Arm";
+  static constexpr const char * default_topic = "ArmAction";
+  static bool success(const interfaces::action::Arm::Result & r) {return (r.result == 0);}
+  static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::CameraTakePhoto>
+{
+  static constexpr const char * skill_name = "CameraTakePhoto";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::CameraTakePhoto::Result & r) {return r.success;}
+  static std::string message(const interfaces::action::CameraTakePhoto::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::HandControl>
+{
+  static constexpr const char * skill_name = "HandControl";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::HandControl::Result & r) {return r.success;}
+  static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::MoveWaist>
+{
+  static constexpr const char * skill_name = "MoveWaist";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::MoveWaist::Result & r) {return r.success;}
+  static std::string message(const interfaces::action::MoveWaist::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::MoveLeg>
+{
+  static constexpr const char * skill_name = "MoveLeg";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::MoveLeg::Result & r) {return r.success;}
+  static std::string message(const interfaces::action::MoveLeg::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::MoveWheel>
+{
+  static constexpr const char * skill_name = "MoveWheel";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::MoveWheel::Result & r) {return r.success;}
+  static std::string message(const interfaces::action::MoveWheel::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::MoveHome>
+{
+  static constexpr const char * skill_name = "MoveHome";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::MoveHome::Result & r) {return r.success;}
+  static std::string message(const interfaces::action::MoveHome::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::VisionObjectRecognition>
+{
+  static constexpr const char * skill_name = "VisionObjectRecognition";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::MapLoad>
+{
+  static constexpr const char * skill_name = "MapLoad";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::MapSave>
+{
+  static constexpr const char * skill_name = "MapSave";
+  static constexpr const char * default_topic = "";
+};
+
+} // namespace brain
--- a/src/brain/include/brain/skill_manager.hpp
+++ b/src/brain/include/brain/skill_manager.hpp
@@ -14,21 +14,10 @@
 #include "brain/action_registry.hpp"
 #include "brain/bt_registry.hpp"

-#include "interfaces/action/arm_space_control.hpp"
-#include "interfaces/action/hand_control.hpp"
-#include "interfaces/action/leg_control.hpp"
-#include "interfaces/action/vision_grasp_object.hpp"
-#include "interfaces/srv/vision_object_recognition.hpp"
-#include "interfaces/action/arm.hpp"
-#include "interfaces/action/slam_mode.hpp"
-#include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "brain/skill_config.hpp"

 namespace brain
 {
-
-template<typename ActionT>
-struct SkillActionTrait;
-
 /**
 * @brief Specification of a skill as parsed from YAML.
 *
@@ -42,6 +31,8 @@ struct SkillSpec
  std::string description;
  std::vector<std::string> required_skills;
  std::vector<std::string> interfaces;  // e.g. "BaseName.action" / "BaseName.service"
+  // Optional per-interface default topic overrides keyed by full token (e.g. "Arm.action").
+  std::unordered_map<std::string, std::string> interface_default_topics;
 };

 /**
@@ -97,7 +88,7 @@ private:
  ActionClientRegistry * action_clients_ {nullptr};
  BTRegistry * bt_ {nullptr};
  std::unordered_map<std::string, SkillSpec> skills_;
-  std::vector<rclcpp::ClientBase::SharedPtr> service_clients_; // 保存 service client 生命周期
+  std::vector<rclcpp::ClientBase::SharedPtr> service_clients_;
  std::unordered_map<std::string, rclcpp::ClientBase::SharedPtr> service_client_map_; // skill name -> client
  std::unordered_map<std::string, std::function<void(const std::string &, const std::string &)>>
    action_override_registrars_;
@@ -109,6 +100,41 @@ private:

 public:

+  // Public helpers for registrars to avoid accessing private members directly from free functions
+  template<typename ActionT>
+  void register_action_for_base(
+    const std::string & base,
+    const std::string & topic,
+    const std::string & internal_skill)
+  {
+    register_action_client_default<ActionT>(base, topic, internal_skill);
+  }
+
+  template<typename ServiceT>
+  void register_service_client(
+    const std::string & topic,
+    const std::string & internal_skill)
+  {
+    // Try reuse existing client if already created under either key
+    std::shared_ptr<rclcpp::Client<ServiceT>> typed_existing;
+    auto it_topic = service_client_map_.find(topic);
+    if (it_topic != service_client_map_.end()) {
+      typed_existing = std::dynamic_pointer_cast<rclcpp::Client<ServiceT>>(it_topic->second);
+    }
+    if (!typed_existing) {
+      auto it_name = service_client_map_.find(internal_skill);
+      if (it_name != service_client_map_.end()) {
+        typed_existing = std::dynamic_pointer_cast<rclcpp::Client<ServiceT>>(it_name->second);
+      }
+    }
+    auto client = typed_existing ? typed_existing : node_->create_client<ServiceT>(topic);
+    if (!typed_existing) {
+      service_clients_.push_back(client);
+    }
+    service_client_map_[internal_skill] = client;
+    service_client_map_[topic] = client;
+  }
+
  template<typename ServiceT>
  struct ServiceHookOptions
  {
@@ -323,105 +349,6 @@ private:
  }
 };

-
-/**
- * @brief Primary template (unspecialized) for skill action trait extraction.
- * Specializations must define: skill_name (constexpr const char*), success(result), message(result).
- */
-template<typename ActionT>
-struct SkillActionTrait; // intentionally undefined primary template
-
-template<>
-struct SkillActionTrait<interfaces::action::ArmSpaceControl>
-{
-  static constexpr const char * skill_name = "ArmSpaceControl";
-  static bool success(const interfaces::action::ArmSpaceControl::Result & r) {return r.success;}
-  static std::string message(const interfaces::action::ArmSpaceControl::Result & r)
-  {
-    return r.message;
-  }
-};
-template<>
-struct SkillActionTrait<interfaces::action::Arm>
-{
-  static constexpr const char * skill_name = "Arm";
-  static bool success(const interfaces::action::Arm::Result & r) {return (r.result == 0) ? true : false;}
-  static std::string message(const interfaces::action::Arm::Result & r)
-  {
-    (void)r;
-    return "r.message";
-  }
-};
-template<>
-struct SkillActionTrait<interfaces::action::HandControl>
-{
-  static constexpr const char * skill_name = "HandControl";
-  static bool success(const interfaces::action::HandControl::Result & r) {return r.success;}
-  static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
-};
-template<>
-struct SkillActionTrait<interfaces::action::LegControl>
-{
-  static constexpr const char * skill_name = "LegControl";
-  static bool success(const interfaces::action::LegControl::Result & r) {return r.success;}
-  static std::string message(const interfaces::action::LegControl::Result & r) {return r.message;}
-};
-template<>
-struct SkillActionTrait<interfaces::action::VisionGraspObject>
-{
-  static constexpr const char * skill_name = "VisionGraspObject";
-  static bool success(const interfaces::action::VisionGraspObject::Result & r) {return r.success;}
-  static std::string message(const interfaces::action::VisionGraspObject::Result & r)
-  {
-    return r.message;
-  }
-};
-
-template<>
-struct SkillActionTrait<interfaces::action::SlamMode>
-{
-  static constexpr const char * skill_name = "SlamMode";
-  static bool success(const interfaces::action::SlamMode::Result & r)
-  {
-    return r.success;
-  }
-  static std::string message(const interfaces::action::SlamMode::Result & r)
-  {
-    return r.message;
-  }
-};
-
-template<>
-struct SkillActionTrait<nav2_msgs::action::NavigateToPose>
-{
-  static constexpr const char * skill_name = "NavigateToPose";
-  static bool success(const nav2_msgs::action::NavigateToPose::Result & r)
-  {
-    (void)r;
-    // NavigateToPose::Result carries an empty payload; success is inferred from the
-    // rclcpp_action::ResultCode before this helper is invoked.
-    return true;
-  }
-  static std::string message(const nav2_msgs::action::NavigateToPose::Result & r)
-  {
-    (void)r;
-    return "NavigateToPose completed";
-  }
-};
-
-/**
- * @brief Tuple listing of all supported skill action types for dispatcher iteration.
- */
-using SkillActionTypes = std::tuple<
-  interfaces::action::ArmSpaceControl,
-  interfaces::action::Arm,
-  interfaces::action::HandControl,
-  interfaces::action::LegControl,
-  interfaces::action::VisionGraspObject,
-  interfaces::action::SlamMode,
-  nav2_msgs::action::NavigateToPose
->;
-
 /**
 * @brief Recursive compile-time dispatcher that matches a skill name to an action type and invokes send_and_wait.
 * @tparam I Current tuple index (internal use, defaults to 0).
@@ -459,5 +386,4 @@ bool dispatch_skill_action(
  }
 }

-
 } // namespace brain
--- a/src/brain/src/cerebellum_node.cpp
+++ b/src/brain/src/cerebellum_node.cpp
--- a/src/brain/src/cerebrum_node.cpp
+++ b/src/brain/src/cerebrum_node.cpp
@@ -20,6 +20,7 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <behaviortree_cpp/action_node.h>
 #include <rclcpp_action/rclcpp_action.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>

 // Project headers
 #include "brain/constants.hpp"
@@ -82,18 +83,36 @@ struct UpdatingFlagGuard
 */
 CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("cerebrum_node", options),
-  bt_config_file_path_(ament_index_cpp::get_package_share_directory("brain") + std::string("/config/bt_vision_grasp.xml"))
+  share_directory_(ament_index_cpp::get_package_share_directory("brain"))
 {
-  InitializeRegistries();
+  RCLCPP_INFO(this->get_logger(), "Cerebrum node started");
+
+  if (!LoadRobotConfiguration()) {
+    RCLCPP_ERROR(this->get_logger(), "Failed to load robot configuration");
+    return;
+  }
+
+  // Initialize
+  action_registry_ = std::make_unique<ActionClientRegistry>(this);
+  bt_registry_ = std::make_unique<BTRegistry>(factory_, this);
+  skill_manager_ = std::make_unique<SkillManager>(this, action_registry_.get(), bt_registry_.get());
+  if (!skill_manager_->load_from_file(robot_skill_file_path_)) {
+    RCLCPP_WARN(this->get_logger(), "Failed to load skills from %s", robot_skill_file_path_.c_str());
+  }
+
  DeclareParameters();
-  auto skills_file = ResolveSkillsFile();
-  LoadSkills(skills_file);
  LoadParameters();
-  RegisterBtAndActionClients();
-  CreateTimers();
+  SetupDynamicParameterHandling();
+
+  RegisterSkillBtActions();
+  RegisterActionClient();
+
+  bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
+  task_timer_ = this->create_wall_timer(5000ms, [this]() {CerebrumTask();});
+
  CreateServices();
-  RCLCPP_INFO(this->get_logger(),
-    "CerebrumNode initialized (timeout=%.2f random_pick=%zu)",
+
+  RCLCPP_INFO(this->get_logger(), "CerebrumNode initialized (timeout=%.2f random_pick=%zu)", 
    node_timeout_sec_, random_skill_pick_count_);
 }

@@ -119,9 +138,7 @@ void CerebrumNode::RegisterBtAction(
 void CerebrumNode::SendExecuteBt(const std::string & bt_action_name)
 {
  if (!action_registry_) {
-    RCLCPP_WARN(
-      this->get_logger(), "Action registry not initialized; cannot send %s",
-      bt_action_name.c_str());
+    RCLCPP_WARN(this->get_logger(), "Action registry not initialized; cannot send %s", bt_action_name.c_str());
    return;
  }
  action_registry_->send(bt_action_name, this->get_logger());
@@ -159,8 +176,23 @@ void CerebrumNode::RegisterActionClient()
        }
      }
      g.action_name = oss.str();
-      RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction]Sending goal epoch=%u skills=%s",
-        g.epoch, g.action_name.c_str());
+      // Populate per-skill calls[] using dynamic parameters
+      g.calls.clear();
+      if (single_skill_goal_override_) {
+        g.calls.push_back(BuildSkillCallForSkill(*single_skill_goal_override_, single_skill_instance_override_));
+      } else {
+        for (const auto & s : current_sequence_skills_) {
+          g.calls.push_back(BuildSkillCallForSkill(s, std::nullopt));
+        }
+      }
+      // Debug: log outgoing calls being sent to Cerebellum
+      RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal with calls_count=%zu", g.calls.size());
+      for (size_t i = 0; i < g.calls.size(); ++i) {
+        const auto & c = g.calls[i];
+        RCLCPP_INFO(this->get_logger(), "  out_call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
+          i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
+      }
+      RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction]Sending goal epoch=%u skills=%s", g.epoch, g.action_name.c_str());
      return g;
    },
    // Feedback
@@ -221,16 +253,11 @@ void CerebrumNode::RegisterActionClient()
        res.result &&
        res.result->success);
      if (ok) {
-        RCLCPP_INFO(
-          this->get_logger(),
-          "[ExecuteBtAction][result] OK: %s",
+        RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] OK: %s",
          res.result ? res.result->message.c_str() : "<null>");
      } else {
-        RCLCPP_ERROR(
-          this->get_logger(),
-          "[ExecuteBtAction][result] FAIL code=%d msg=%s",
-          static_cast<int>(res.code),
-          res.result ? res.result->message.c_str() : "<null>");
+        RCLCPP_ERROR(this->get_logger(), "[ExecuteBtAction][result] FAIL code=%d msg=%s",
+          static_cast<int>(res.code), res.result ? res.result->message.c_str() : "<null>");
      }
      for (auto & kv : per_node_exec_) {
        auto & info = kv.second;
@@ -239,11 +266,8 @@ void CerebrumNode::RegisterActionClient()
          info.awaiting_result = false;
          info.result = ok;               // authoritative final
          info.result_code = ok ? ExecResultCode::SUCCESS : ExecResultCode::FAILURE;
-          RCLCPP_INFO(
-            this->get_logger(),
-            "[ExecuteBtAction][result] Finalizing node %s result=%s",
-            kv.first.c_str(),
-            info.result ? "SUCCESS" : "FAILURE");
+          RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] Finalizing node %s result=%s",
+            kv.first.c_str(), info.result ? "SUCCESS" : "FAILURE");
        }
      }
    });
@@ -323,15 +347,12 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
 {
  BT::Tree new_tree;
  if (!bt_registry_->build_tree_from_xml_file(xml_file, new_tree)) {
-    RCLCPP_ERROR(
-      this->get_logger(), "Failed building BT from file %s",
-      xml_file.c_str());
+    RCLCPP_ERROR(this->get_logger(), "Failed building BT from file %s", xml_file.c_str());
    return;
  }
  try {
    tree_.haltTree();
  } catch (...) {
-    // Swallow halt exceptions.
    RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
  }
  tree_ = std::move(new_tree);
@@ -344,10 +365,7 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
  epoch_filter_.reset_epoch(new_epoch, seq_skills);
  per_node_exec_.clear();
  bt_pending_run_ = true;
-  RCLCPP_INFO(
-    this->get_logger(),
-    "BT built from file epoch=%llu",
-    static_cast<unsigned long long>(new_epoch));
+  RCLCPP_INFO(this->get_logger(), "BT built from file epoch=%llu", static_cast<unsigned long long>(new_epoch));
 }

 /**
@@ -366,9 +384,7 @@ void CerebrumNode::UpdateBehaviorTree()
  UpdatingFlagGuard guard(bt_updating_);
  BT::Tree new_tree;
  if (!bt_registry_->build_tree_from_sequence(active_sequence_, new_tree)) {
-    RCLCPP_ERROR(
-      this->get_logger(), "Failed building BT for sequence %s",
-      active_sequence_.c_str());
+    RCLCPP_ERROR(this->get_logger(), "Failed building BT for sequence %s", active_sequence_.c_str());
    return;
  }
  try {
@@ -387,11 +403,8 @@ void CerebrumNode::UpdateBehaviorTree()
  epoch_filter_.reset_epoch(new_epoch, seq_skills);
  per_node_exec_.clear();
  bt_pending_run_ = true;
-  RCLCPP_INFO(
-    this->get_logger(),
-    "BT updated epoch=%llu sequence=%s",
-    static_cast<unsigned long long>(new_epoch),
-    active_sequence_.c_str());
+  RCLCPP_INFO(this->get_logger(), "BT updated epoch=%llu sequence=%s",
+    static_cast<unsigned long long>(new_epoch), active_sequence_.c_str());
 }

 /**
@@ -415,7 +428,7 @@ void CerebrumNode::RegisterSkillBtActions()
    const auto & skill = kv.second;
    const std::string bt_type = skill.name + std::string("_H");
    BTActionHandlers handlers;
-    handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode &) {
+  handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode & node) {
        auto & info = per_node_exec_[bt_type];
        if (info.in_flight || info.result) {
          return info.result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::RUNNING;
@@ -427,8 +440,12 @@ void CerebrumNode::RegisterSkillBtActions()
        info.skill_name = skill.name;
        info.last_progress = 0.0;
        info.cancel_state = PerNodeExecInfo::CancelState::NONE;
+
+        const std::string instance_name = node.name();
+        DeclareBtActionParamsForSkillInstance(skill.name, instance_name);
        // Override goal builder to send only this skill.
        single_skill_goal_override_ = skill.name;
+        single_skill_instance_override_ = instance_name;
        if (!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
          RCLCPP_ERROR(this->get_logger(), "ExecuteBtAction server unavailable");
          info.in_flight = false;
@@ -441,6 +458,7 @@ void CerebrumNode::RegisterSkillBtActions()
        RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_start", bt_type.c_str());
        // Clear override after send to avoid affecting other triggers.
        single_skill_goal_override_.reset();
+        single_skill_instance_override_.reset();
        return BT::NodeStatus::RUNNING;
      };
    handlers.on_running = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
@@ -480,6 +498,210 @@ void CerebrumNode::RegisterSkillBtActions()
  }
 }

+/**
+ * @brief Update BT action parameters for a skill instance
+ * 
+ * @param skill_name
+ * @param instance_name 
+ * @param instance_params 
+ */
+void CerebrumNode::UpdateBtActionParamsForSkillInstance(
+    const std::string & skill_name, 
+    const std::string & instance_name, 
+    const std::string & instance_params)
+{
+  if (skill_name.empty() || instance_name.empty() || instance_params.empty()) {
+    RCLCPP_WARN(this->get_logger(), "Update BtAction Params ForSkillInstance called with empty skill_name, instance_name, or instance_params");
+    return;
+  }
+    // Per-instance namespace: cerebrum.bt.<Skill>.<Instance>.*
+  const std::string ns = std::string("cerebrum.bt.") + skill_name + "." + instance_name + ".";
+  const std::string p_topic = ns + "topic";
+  const std::string p_payload = ns + "payload_yaml";
+  if (!this->has_parameter(p_topic)) {
+    this->declare_parameter<std::string>(p_topic, std::string(""));
+  }
+  if (!this->has_parameter(p_payload)) {
+    this->declare_parameter<std::string>(p_payload, std::string(""));
+  }
+  RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill instance %s", instance_name.c_str());
+  try {
+    this->set_parameter(rclcpp::Parameter(p_payload, instance_params));
+    RCLCPP_INFO(this->get_logger(), "Seeded sample payload for %s instance %s", skill_name.c_str(), instance_name.c_str());
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(this->get_logger(), "[%s] seeding sample payload failed: %s", skill_name.c_str(), e.what());
+  }
+}
+
+/**
+ * @brief Declare BT action parameters for a skill instance
+ * 
+ * @param skill_name 
+ * @param instance_name 
+ */
+void CerebrumNode::DeclareBtActionParamsForSkillInstance(
+    const std::string & skill_name, 
+    const std::string & instance_name)
+{
+  if (skill_name.empty() || instance_name.empty()) {
+    RCLCPP_WARN(this->get_logger(), "Declare BtAction Params ForSkillInstance called with empty skill_name or instance_name");
+    return;
+  }
+
+  std::string instance_params;
+  if (skill_name == "MoveWaist" && instance_name == "s1_waist_bend_down") {
+    instance_params = "move_pitch_degree: 10.0\nmove_yaw_degree: -15.0\n";
+  } else if (skill_name == "MoveLeg" && instance_name == "s7_leg_move_back") {
+    instance_params = "move_up_distance: 0.08\n";
+  } else if (skill_name == "MoveWheel" && instance_name == "s8_wheel_move_back") {
+    instance_params = "move_distance: 0.60\nmove_angle: 20.0\n";
+  } else if (skill_name == "MoveHome" && instance_name == "s0_move_home") {
+    // MoveHome goal is empty; seed explicit empty map for clarity
+    instance_params = "{}\n";
+  } else if (skill_name == "Arm" && instance_name == "s2_arm_stretch_out") {
+    instance_params = "body_id: 1\ndata_type: 3\ndata_length: 7\ncommand_id: 1\nframe_time_stamp: 123456789\ndata_array: [0.3, 0.4, 0.5, -0.6, 0.07, -0.18, 0.72]\n";
+  }
+
+  UpdateBtActionParamsForSkillInstance(skill_name, instance_name, instance_params);
+}
+
+/**
+ * @brief Install on_set_parameters callback to accept dynamic per-skill updates.
+ *
+ * Only parameters within the prefix "cerebrum.bt." are intercepted. Minimal validation:
+ *  - topic: any string accepted
+ *  - payload_yaml: any string accepted (executor will validate downstream)
+      
+      # Set MoveWheel topic
+      ros2 param set /cerebrum_node cerebrum.bt.MoveWheel.topic /robot/move_wheel
+
+      # Set Arm payload
+      ros2 param set /cerebrum_node cerebrum.bt.Arm.payload_yaml "body_id: 1
+      data_type: 3
+      data_length: 7
+      command_id: 1
+      frame_time_stamp: 123456789
+      data_array: [0.1, 0.2, 0.3, -0.4, 0.05, -0.19, 0.73]"
+ */
+void CerebrumNode::SetupDynamicParameterHandling()
+{
+  using rclcpp::Parameter;
+  using rcl_interfaces::msg::SetParametersResult;
+  auto cb = [this](const std::vector<Parameter> & params) -> SetParametersResult {
+    SetParametersResult result;
+    result.successful = true;
+    result.reason = "ok";
+    const std::string prefix = "cerebrum.bt.";
+    for (const auto & p : params) {
+      const std::string & name = p.get_name();
+      if (name.rfind(prefix, 0) != 0) {
+        continue; // other params handled by default mechanisms
+      }
+      // Expected pattern: cerebrum.bt.<Skill>.topic | payload_yaml
+      // We perform a light parse to ensure a field segment exists
+      auto last_dot = name.rfind('.');
+      if (last_dot == std::string::npos || last_dot + 1 >= name.size()) {
+        result.successful = false;
+        result.reason = "invalid per-skill param name";
+        break;
+      }
+      const std::string field = name.substr(last_dot + 1);
+      if (field != "topic" && field != "payload_yaml") {
+        // Reject unknown fields under this namespace to avoid typos
+        result.successful = false;
+        result.reason = "unsupported field for cerebrum.bt.*";
+        break;
+      }
+      // Accept string-only values
+      if (p.get_type() != rclcpp::ParameterType::PARAMETER_STRING) {
+        result.successful = false;
+        result.reason = "per-skill params must be strings";
+        break;
+      }
+      // No further validation here; downstream will interpret.
+      RCLCPP_INFO(this->get_logger(), "[params] updated %s", name.c_str());
+    }
+    return result;
+  };
+  param_cb_handle_ = this->add_on_set_parameters_callback(cb);
+}
+
+/**
+ * @brief Build a SkillCall message for a specific skill.
+ *
+ * @param skill_name The name of the skill.
+ * @return brain_interfaces::msg::SkillCall The constructed SkillCall message.
+ */
+brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::string & skill_name) const
+{
+  brain_interfaces::msg::SkillCall call;
+  call.name = skill_name;
+  // Interface type/kind inference from SkillSpec
+  if (!skill_manager_) {return call;}
+  auto it = skill_manager_->skills().find(skill_name);
+  if (it != skill_manager_->skills().end() && !it->second.interfaces.empty()) {
+    // Use first interface as primary
+    const std::string & iface = it->second.interfaces.front();
+    call.interface_type = iface;
+    auto pos = iface.rfind('.');
+    std::string kind = (pos == std::string::npos) ? std::string{} : iface.substr(pos + 1);
+    for (auto & c : kind) c = static_cast<char>(::tolower(static_cast<unsigned char>(c)));
+    if (kind == "srv") kind = "service";
+    call.call_kind = kind;
+  }
+  // Read dynamic params
+  const std::string ns = std::string("cerebrum.bt.") + skill_name + ".";
+  const std::string p_topic = ns + "topic";
+  const std::string p_payload = ns + "payload_yaml";
+  try {
+    if (this->has_parameter(p_topic)) {
+      call.topic = this->get_parameter(p_topic).as_string();
+    }
+    if (this->has_parameter(p_payload)) {
+      call.payload_yaml = this->get_parameter(p_payload).as_string();
+    }
+  } catch (const std::exception & e) {
+    RCLCPP_WARN(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
+  }
+  return call;
+}
+
+/**
+ * @brief Declare BT action parameters for a skill instance
+ * 
+ * @param skill_name 
+ * @param instance_name 
+ * @return brain_interfaces::msg::SkillCall 
+ */
+brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
+  const std::string & skill_name,
+  const std::optional<std::string> & instance_name) const
+{
+  auto base = BuildSkillCallForSkill(skill_name);
+  // If instance_name provided, try per-instance override first, falling back to per-skill
+  if (!instance_name) {
+    return base;
+  }
+  const std::string ns = std::string("cerebrum.bt.") + skill_name + "." + *instance_name + ".";
+  const std::string p_topic = ns + "topic";
+  const std::string p_payload = ns + "payload_yaml";
+  try {
+    // topic override
+    if (this->has_parameter(p_topic)) {
+      auto v = this->get_parameter(p_topic).as_string();
+      if (!v.empty()) base.topic = v;
+    }
+    // payload override
+    if (this->has_parameter(p_payload)) {
+      auto v = this->get_parameter(p_payload).as_string();
+      if (!v.empty()) base.payload_yaml = v;
+    }
+  } catch (const std::exception & e) {
+    RCLCPP_WARN(this->get_logger(), "[%s/%s] read instance params failed: %s", skill_name.c_str(), instance_name->c_str(), e.what());
+  }
+  return base;
+}
+
 /**
 * @brief Randomly select up to count distinct action-capable skills (respecting whitelist).
 * @param count Maximum number of skills.
@@ -683,21 +905,6 @@ CerebrumNode::ExecResultCode CerebrumNode::ParseResultDetail(const std::string &
  return ExecResultCode::UNKNOWN;
 }

-/**
- * @brief Initialize registry/manager singletons used by the node.
- *
- * Allocates ActionClientRegistry, BTRegistry and SkillManager with proper
- * dependency ordering so later initialization steps (skill loading, BT
- * registration) can safely use them. Must be called exactly once during
- * construction before any other helper depending on these objects.
- */
-void CerebrumNode::InitializeRegistries()
-{
-  action_registry_ = std::make_unique<ActionClientRegistry>(this);
-  bt_registry_ = std::make_unique<BTRegistry>(factory_, this);
-  skill_manager_ = std::make_unique<SkillManager>(this, action_registry_.get(), bt_registry_.get());
-}
-
 /**
 * @brief Declare all ROS 2 parameters consumed by this node.
 *
@@ -707,60 +914,11 @@ void CerebrumNode::InitializeRegistries()
 */
 void CerebrumNode::DeclareParameters()
 {
-  this->declare_parameter<std::string>("skills_file", "");
  this->declare_parameter<double>("node_timeout_sec", node_timeout_sec_);
-  this->declare_parameter<bool>(
-    "allow_bt_rebuild_during_execution",
-    allow_bt_rebuild_during_execution_);
+  this->declare_parameter<bool>("allow_bt_rebuild_during_execution", allow_bt_rebuild_during_execution_);
  this->declare_parameter<std::string>("allowed_action_skills", "");
  this->declare_parameter<std::string>("fixed_skill_sequence", "");
-  this->declare_parameter<int>(
-    "random_skill_pick_count",
-    static_cast<int>(random_skill_pick_count_));
-}
-
-/**
- * @brief Resolve the effective skills definition YAML file path.
- *
- * Reads the declared 'skills_file' parameter. If empty, attempts to locate a
- * packaged default at:
- *   <ament_package_share_directory(brain)>/config/robot_skills.yaml
- * Fallback discovery failures are logged as warnings; the function never
- * throws. An empty return value indicates no usable path could be determined.
- *
- * @return Resolved skills file path (may be empty if neither parameter nor default exists).
- * @thread_safety Read-only parameter access; intended for use during construction only.
- */
-std::string CerebrumNode::ResolveSkillsFile()
-{
-  std::string skills_file;
-  this->get_parameter("skills_file", skills_file);
-  if (skills_file.empty()) {
-    try {
-      auto share = ament_index_cpp::get_package_share_directory("brain");
-      skills_file = share + std::string("/config/robot_skills.yaml");
-      RCLCPP_INFO(this->get_logger(), "Using default skills file: %s", skills_file.c_str());
-    } catch (const std::exception & e) {
-      RCLCPP_WARN(this->get_logger(), "Could not locate default skills file: %s", e.what());
-    }
-  }
-  return skills_file;
-}
-
-/**
- * @brief Load skill specifications from a YAML file.
- *
- * Emits a warning if the file cannot be parsed or loaded, but does not throw.
- * An empty path results in a no-op so callers can unconditionally invoke it.
- *
- * @param skills_file Absolute or relative path to YAML file (may be empty).
- */
-void CerebrumNode::LoadSkills(const std::string & skills_file)
-{
-  if (skills_file.empty()) {return;}
-  if (!skill_manager_->load_from_file(skills_file)) {
-    RCLCPP_WARN(this->get_logger(), "Failed to load skills from %s", skills_file.c_str());
-  }
+  this->declare_parameter<int>("random_skill_pick_count", static_cast<int>(random_skill_pick_count_));
 }

 /**
@@ -800,31 +958,6 @@ void CerebrumNode::LoadParameters()
  }
 }

-/**
- * @brief Register per-skill BT action handlers plus the unified action client.
- *
- * Bundled to ensure the ExecuteBtAction client exists before any BT node can
- * attempt to send goals. Idempotent in practice because internal registration
- * functions guard against duplication.
- */
-void CerebrumNode::RegisterBtAndActionClients()
-{
-  RegisterSkillBtActions();
-  RegisterActionClient();
-}
-
-/**
- * @brief Create timers used for BT ticking and periodic sequence/model updates.
- *
- * bt_timer_: High-frequency (10ms) ticker executing the BT root when a run is pending.
- * task_timer_: Low-frequency (5s) trigger for model-driven sequence selection and rebuild.
- */
-void CerebrumNode::CreateTimers()
-{
-  bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
-  task_timer_ = this->create_wall_timer(5000ms, [this]() {CerebrumTask();});
-}
-
 /**
 * @brief Create externally callable services.
 *
@@ -858,4 +991,40 @@ void CerebrumNode::CreateServices()
    });
 }

+/**
+ * @brief Load the robot configuration file.
+ * 
+ * @return true 
+ * @return false 
+ */
+bool CerebrumNode::LoadRobotConfiguration()
+{ 
+  robot_config_ = std::make_unique<brain::robot_config::RobotConfig>();
+  auto skill_file = robot_config_->SkillFile("brain");
+  if (skill_file == std::nullopt) {
+    RCLCPP_ERROR(this->get_logger(), "No skill_file entry found for 'brain' in robot_config.yaml");
+    return false;
+  }
+  robot_skill_file_path_ = share_directory_ + *skill_file;
+  RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
+
+  auto bt_config_file = robot_config_->BtConfigFile("cerebrum_node");
+  if (bt_config_file == std::nullopt) {
+    RCLCPP_ERROR(this->get_logger(), "No bt_config_file entry found for 'cerebrum_node' in robot_config.yaml");
+    return false;
+  }
+  bt_config_file_path_ = share_directory_ + *bt_config_file;
+  RCLCPP_WARN(this->get_logger(), "bt config file %s", bt_config_file_path_.c_str());
+
+  auto bt_params_file = robot_config_->BtParamsFile("cerebrum_node");
+  if (bt_params_file == std::nullopt) {
+    RCLCPP_ERROR(this->get_logger(), "No bt_params_file entry found for 'cerebrum_node' in robot_config.yaml");
+    return false;
+  }
+  bt_params_file_path_ = share_directory_ + *bt_params_file;
+  RCLCPP_WARN(this->get_logger(), "bt params file %s", bt_params_file_path_.c_str());
+
+  return true;
+}
+
 } // namespace brain
--- a/src/brain/src/skill_manager.cpp
+++ b/src/brain/src/skill_manager.cpp
@@ -15,24 +15,6 @@
 #include <utility>
 #include <rclcpp_action/rclcpp_action.hpp>

-#include "interfaces/action/arm_space_control.hpp"
-#include "interfaces/action/hand_control.hpp"
-#include "interfaces/action/leg_control.hpp"
-#include "interfaces/action/vision_grasp_object.hpp"
-#include "interfaces/srv/vision_object_recognition.hpp"
-#include "interfaces/action/slam_mode.hpp"
-#include "interfaces/srv/map_save.hpp"
-#include "interfaces/srv/map_load.hpp"
-#include "interfaces/action/arm.hpp"
-#include "nav2_msgs/action/navigate_to_pose.hpp"
-
-using interfaces::action::ArmSpaceControl;
-using interfaces::action::Arm;
-using interfaces::action::HandControl;
-using interfaces::action::LegControl;
-using interfaces::action::VisionGraspObject;
-using interfaces::srv::VisionObjectRecognition;
-
 namespace brain
 {

@@ -122,7 +104,30 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
    }
    if (item["interfaces"]) {
      for (const auto & v : item["interfaces"]) {
-        s.interfaces.push_back(v.as<std::string>());
+        if (v.IsScalar()) {
+          // Simple form: "BaseName.kind"
+          const auto token = v.as<std::string>();
+          s.interfaces.push_back(token);
+        } else if (v.IsMap()) {
+          // Extended form: { name: BaseName.kind, default_topic: <str> }
+          const auto name_node = v["name"];
+          if (!name_node || !name_node.IsScalar()) {
+            RCLCPP_ERROR(node_->get_logger(), "Invalid interface entry (missing 'name'): %s", YAML::Dump(v).c_str());
+            continue;
+          }
+          const std::string token = name_node.as<std::string>("");
+          if (token.empty()) { continue; }
+          s.interfaces.push_back(token);
+          const auto def_node = v["default_topic"];
+          if (def_node && def_node.IsScalar()) {
+            const std::string def_topic = def_node.as<std::string>("");
+            if (!def_topic.empty()) {
+              s.interface_default_topics[token] = def_topic;
+            }
+          }
+        } else {
+          RCLCPP_ERROR(node_->get_logger(), "Unsupported interface entry type: %s", YAML::Dump(v).c_str());
+        }
      }
    }
    if (s.name.empty()) {continue;}
@@ -151,6 +156,137 @@ std::vector<std::pair<std::string, std::string>> SkillManager::enumerate_action_
  return out;
 }

+using ActionRegistrar = void (*)(SkillManager * self, const std::string & topic, const std::string & internal_skill);
+using ServiceRegistrar = void (*)(SkillManager * self, const std::string & topic, const std::string & internal_skill);
+
+// Registrar function templates (no captures, usable as function pointers)
+template<typename ActionT>
+static void action_register(SkillManager * self, const std::string & topic, const std::string & internal_skill)
+{
+  self->register_action_for_base<ActionT>(SkillActionTrait<ActionT>::skill_name, topic, internal_skill);
+}
+
+template<typename ActionT>
+static constexpr std::pair<const char *, ActionRegistrar> make_action_entry()
+{
+  return {SkillActionTrait<ActionT>::skill_name, &action_register<ActionT>};
+}
+
+template<typename ServiceT>
+static void service_register(SkillManager * self, const std::string & topic, const std::string & internal_skill)
+{
+  self->register_service_client<ServiceT>(topic, internal_skill);
+}
+
+template<typename ServiceT>
+static constexpr std::pair<const char *, ServiceRegistrar> make_service_entry(const char * name)
+{
+  return {name, &service_register<ServiceT>};
+}
+
+// Helper to build the service map by expanding tuple indices (C++17-compatible).
+template<typename TupleT, std::size_t... I>
+static std::unordered_map<std::string, ServiceRegistrar> build_service_map(std::index_sequence<I...>) {
+  std::unordered_map<std::string, ServiceRegistrar> m;
+  (void)std::array<int, sizeof...(I)>{{(m.emplace(
+    std::string(SkillServiceTrait<std::tuple_element_t<I, TupleT>>::skill_name),
+    &service_register<std::tuple_element_t<I, TupleT>>), 0)...}};
+  return m;
+}
+
+// Helper to build the map by expanding tuple indices. Implemented as a template
+// function rather than a templated lambda for C++17 compatibility.
+template<typename TupleT, std::size_t... I>
+static std::unordered_map<std::string, ActionRegistrar> build_action_map(std::index_sequence<I...>) {
+  std::unordered_map<std::string, ActionRegistrar> m;
+  // Expand the pack by using a dummy array and the comma operator to call emplace for each index.
+  // This avoids constructing nested initializer_list objects and is C++17 compatible.
+  (void)std::array<int, sizeof...(I)>{{(m.emplace(
+    std::string(SkillActionTrait<std::tuple_element_t<I, TupleT>>::skill_name),
+    &action_register<std::tuple_element_t<I, TupleT>>), 0)...}};
+  return m;
+}
+
+// Factory methods to access static registries
+static const std::unordered_map<std::string, ActionRegistrar> & get_action_registrars()
+{
+  // Build the registrar map by iterating the SkillActionTypes tuple at compile time.
+  using TupleT = SkillActionTypes;
+  constexpr std::size_t N = std::tuple_size<TupleT>::value;
+  static const std::unordered_map<std::string, ActionRegistrar> kMap = build_action_map<TupleT>(std::make_index_sequence<N>{});
+  return kMap;
+}
+
+static const std::unordered_map<std::string, ServiceRegistrar> & get_service_registrars()
+{
+  using TupleT = SkillServiceTypes;
+  constexpr std::size_t N = std::tuple_size<TupleT>::value;
+  static const std::unordered_map<std::string, ServiceRegistrar> kMap = build_service_map<TupleT>(std::make_index_sequence<N>{});
+  return kMap;
+}
+
+// Build maps of default topics for actions and services (name -> default topic string).
+template<typename TupleT, std::size_t... I>
+static std::unordered_map<std::string, std::string> build_action_default_topics(std::index_sequence<I...>) {
+  std::unordered_map<std::string, std::string> m;
+  (void)std::array<int, sizeof...(I)>{{(m.emplace(
+    std::string(SkillActionTrait<std::tuple_element_t<I, TupleT>>::skill_name),
+    std::string(SkillActionTrait<std::tuple_element_t<I, TupleT>>::default_topic)), 0)...}};
+  return m;
+}
+
+static const std::unordered_map<std::string, std::string> & get_action_default_topics()
+{
+  using TupleT = SkillActionTypes;
+  constexpr std::size_t N = std::tuple_size<TupleT>::value;
+  static const std::unordered_map<std::string, std::string> kMap = build_action_default_topics<TupleT>(std::make_index_sequence<N>{});
+  return kMap;
+}
+
+template<typename TupleT, std::size_t... I>
+static std::unordered_map<std::string, std::string> build_service_default_topics(std::index_sequence<I...>) {
+  std::unordered_map<std::string, std::string> m;
+  (void)std::array<int, sizeof...(I)>{{(m.emplace(
+    std::string(SkillServiceTrait<std::tuple_element_t<I, TupleT>>::skill_name),
+    std::string(SkillServiceTrait<std::tuple_element_t<I, TupleT>>::default_topic)), 0)...}};
+  return m;
+}
+
+static const std::unordered_map<std::string, std::string> & get_service_default_topics()
+{
+  using TupleT = SkillServiceTypes;
+  constexpr std::size_t N = std::tuple_size<TupleT>::value;
+  static const std::unordered_map<std::string, std::string> kMap = build_service_default_topics<TupleT>(std::make_index_sequence<N>{});
+  return kMap;
+}
+
+// Utility: join map keys for diagnostics
+template<typename T>
+static std::string join_keys(const std::unordered_map<std::string, T> & m)
+{
+  std::string out;
+  bool first = true;
+  for (const auto & kv : m) {
+    if (!first) {out += ", ";}
+    first = false;
+    out += kv.first;
+  }
+  return out;
+}
+
+template<size_t I = 0>
+static void validate_action_registrars_complete(rclcpp::Logger logger)
+{
+  if constexpr (I < std::tuple_size<SkillActionTypes>::value) {
+    using ActionT = std::tuple_element_t<I, SkillActionTypes>;
+    const char * name = SkillActionTrait<ActionT>::skill_name;
+    if (get_action_registrars().find(name) == get_action_registrars().end()) {
+      RCLCPP_ERROR(logger, "Missing action registrar for '%s'", name);
+    }
+    validate_action_registrars_complete<I + 1>(logger);
+  }
+}
+
 /**
 * @brief Register action and service interfaces declared by a SkillSpec.
 *
@@ -161,69 +297,24 @@ std::vector<std::pair<std::string, std::string>> SkillManager::enumerate_action_
 */
 void SkillManager::register_interfaces_(const SkillSpec & s)
 {
-  static const std::unordered_map<std::string, std::function<void(const std::string &,
-    const std::string &)>> action_registrars = {
-    {"ArmSpaceControl", [this](const std::string & topic, const std::string & internal_skill) {
-        register_action_client_default<ArmSpaceControl>("ArmSpaceControl", topic, internal_skill);
-      }},
-    {"Arm", [this](const std::string & topic, const std::string & internal_skill) {
-        (void)topic;
-        register_action_client_default<Arm>("Arm", "ArmAction", internal_skill);
-      }},
-    {"HandControl", [this](const std::string & topic, const std::string & internal_skill) {
-        register_action_client_default<HandControl>("HandControl", topic, internal_skill);
-      }},
-    {"LegControl", [this](const std::string & topic, const std::string & internal_skill) {
-        register_action_client_default<LegControl>("LegControl", topic, internal_skill);
-      }},
-    {"VisionGraspObject", [this](const std::string & topic, const std::string & internal_skill) {
-        register_action_client_default<VisionGraspObject>("VisionGraspObject", topic, internal_skill);
-      }},
-    {"SlamMode", [this](const std::string & topic, const std::string & internal_skill) {
-        register_action_client_default<interfaces::action::SlamMode>("SlamMode", topic, internal_skill);
-      }},
-    {"NavigateToPose", [this](const std::string & topic, const std::string & internal_skill) {
-        register_action_client_default<nav2_msgs::action::NavigateToPose>("NavigateToPose", topic, internal_skill);
-      }},
-
-  };
-  static const std::unordered_map<std::string, std::function<void(const std::string &,
-    const std::string &)>> service_registrars = {
-      {"VisionObjectRecognition",
-        [this](const std::string & topic, const std::string & internal_skill) {
-          auto client = node_->create_client<VisionObjectRecognition>(topic);
-          service_clients_.push_back(client);
-          service_client_map_[internal_skill] = client;
-          service_client_map_[topic] = client;
-        }},
-      {"MapSave",
-        [this](const std::string & topic, const std::string & internal_skill) {
-          auto client = node_->create_client<interfaces::srv::MapSave>(topic);
-          service_clients_.push_back(client);
-          service_client_map_[internal_skill] = client;
-          service_client_map_[topic] = client;
-        }},
-      {"MapLoad",
-        [this](const std::string & topic, const std::string & internal_skill) {
-          auto client = node_->create_client<interfaces::srv::MapLoad>(topic);
-          service_clients_.push_back(client);
-          service_client_map_[internal_skill] = client;
-          service_client_map_[topic] = client;
-        }},
-
-  };
+  // One-time runtime validation to ensure all actions in SkillActionTypes have a registrar.
+  static bool s_validated = false;
+  if (!s_validated) {
+    validate_action_registrars_complete(node_->get_logger());
+    s_validated = true;
+  }

  auto join_topic = [](const std::string & prefix, const std::string & base) {
-      if (prefix.empty()) {return base;}
-      if (prefix == "/") {return std::string{"/"} + base;}
-      if (prefix.back() == '/') {return prefix + base;}
-      return prefix + "/" + base;
-    };
+    if (prefix.empty()) {return base;}
+    if (prefix == "/") {return std::string{"/"} + base;}
+    if (prefix.back() == '/') {return prefix + base;}
+    return prefix + "/" + base;
+  };

  for (const auto & iface : s.interfaces) {
    auto parsed = parse_interface_entry(iface);
    if (!parsed) {
-      RCLCPP_WARN(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
+      RCLCPP_ERROR(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
      continue;
    }

@@ -232,38 +323,57 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
    const std::string base_snake = to_snake_case(parsed->base);

    auto resolve_topic_name = [&](const std::string & kind_suffix) {
-        const std::string default_topic = join_topic(prefix_value, base_snake);
-        const std::string param_name = base_snake + "." + kind_suffix + "_name";
-        std::string topic_override;
-        if (node_->get_parameter(param_name, topic_override)) {
-          if (topic_override.empty()) {
-            topic_override = default_topic;
-          }
-          return topic_override;
-        }
-        node_->declare_parameter<std::string>(param_name, default_topic);
-        return default_topic;
-      };
+      // Compute the prior default (prefix + base_snake), but allow per-trait default topic when provided.
+      const std::string prefix_default = join_topic(prefix_value, base_snake);
+      const auto & action_defaults = get_action_default_topics();
+      const auto & service_defaults = get_service_default_topics();
+      std::string trait_default;
+      if (kind_suffix == "action") {
+        auto it = action_defaults.find(parsed->base);
+        if (it != action_defaults.end()) { trait_default = it->second; }
+      } else {
+        auto it = service_defaults.find(parsed->base);
+        if (it != service_defaults.end()) { trait_default = it->second; }
+      }
+      // YAML override: exact token (e.g., "Arm.action")
+      std::string yaml_override;
+      auto it_yaml = s.interface_default_topics.find(iface);
+      if (it_yaml != s.interface_default_topics.end()) {
+        yaml_override = it_yaml->second;
+      }
+      const bool has_trait_default = !trait_default.empty();
+      const std::string computed_default = !yaml_override.empty() ? yaml_override : (has_trait_default ? trait_default : prefix_default);
+      const std::string param_name = base_snake + "." + kind_suffix + "_name";
+      std::string topic_override;
+      if (node_->get_parameter(param_name, topic_override)) {
+        // If user explicitly sets an empty parameter, fall back to computed_default.
+        if (topic_override.empty()) { topic_override = computed_default; }
+        return topic_override;
+      }
+      // Declare with computed default so it appears in parameter list; keep existing semantics.
+      node_->declare_parameter<std::string>(param_name, computed_default);
+      return computed_default;
+    };

    if (parsed->kind == "action") {
      const std::string topic_name = resolve_topic_name("action");
-      auto it = action_registrars.find(parsed->base);
-      if (it != action_registrars.end()) {
-        it->second(topic_name, s.name);
+      const auto & action_regs = get_action_registrars();
+      auto it = action_regs.find(parsed->base);
+      if (it != action_regs.end()) {
+        it->second(this, topic_name, s.name);
      } else {
-        RCLCPP_WARN(
-          node_->get_logger(), "No action registrar for interface base '%s'",
-          parsed->base.c_str());
+        RCLCPP_ERROR(node_->get_logger(), "No action registrar for interface base '%s'. Available: [%s]",
+          parsed->base.c_str(), join_keys(action_regs).c_str());
      }
-    } else if (parsed->kind == "service") {
-      const std::string topic_name = resolve_topic_name("service");
-      auto it = service_registrars.find(parsed->base);
-      if (it != service_registrars.end()) {
-        it->second(topic_name, s.name);
+    } else if (parsed->kind == "srv") {
+      const std::string topic_name = resolve_topic_name("srv");
+      const auto & service_regs = get_service_registrars();
+      auto it = service_regs.find(parsed->base);
+      if (it != service_regs.end()) {
+        it->second(this, topic_name, s.name);
      } else {
-        RCLCPP_WARN(
-          node_->get_logger(), "No service registrar for interface base '%s'",
-          parsed->base.c_str());
+        RCLCPP_ERROR(node_->get_logger(), "No service registrar for interface base '%s'. Available: [%s]",
+          parsed->base.c_str(), join_keys(service_regs).c_str());
      }
    }
  }
--- a/src/brain_interfaces/CMakeLists.txt
+++ b/src/brain_interfaces/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(interfaces REQUIRED)

 set(msg_files
  msg/Movej.msg
+  msg/SkillCall.msg
 )
 set(action_files
  action/ExecuteBtAction.action
--- a/src/brain_interfaces/action/ExecuteBtAction.action
+++ b/src/brain_interfaces/action/ExecuteBtAction.action
@@ -29,6 +29,7 @@
 #        server can reject stale goals and stop publishing feedback from older epochs.
 uint32 epoch
 string action_name
+brain_interfaces/SkillCall[] calls

 ---
 # Result
--- a/src/brain_interfaces/msg/SkillCall.msg
+++ b/src/brain_interfaces/msg/SkillCall.msg
@@ -0,0 +1,6 @@
+# Generic skill call descriptor for ExecuteBtAction (scheme A)
+string name           # Skill name, e.g., "MoveWaist" / "MoveLeg"
+string interface_type # e.g., "interfaces/action/MoveWaist" or "interfaces/srv/VisionObjectRecognition"
+string call_kind      # "action" | "srv"
+string topic          # optional topic/service override; empty => use defaults/parameters
+string payload_yaml   # YAML string of Goal/Request fields; may be empty for default goal
--- a/src/modules/motion/arm_control/src/arm_control_node.cpp
+++ b/src/modules/motion/arm_control/src/arm_control_node.cpp
@@ -103,7 +103,7 @@ private:
    // Simulate progress from 0 to 100 with small delay; react to cancel requests
    for (int i = 0; i <= 100; i += 10) {
      if (goal_handle->is_canceling()) {
-        result->result = false;
+        result->result = 1;
        // result->message = "Arm canceled";
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Arm goal canceled");
@@ -118,7 +118,8 @@ private:
    }

    // Complete successfully
-    result->result = true;
+    result->result = 0;
+    result->command_id = goal->command_id;
    // result->message = std::string("Arm moved to target in frame '") +
    //   goal->target.header.frame_id + "'";
    goal_handle->succeed(result);
--- a/src/modules/motion/move_waist/CMakeLists.txt
+++ b/src/modules/motion/move_waist/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.8)
+project(move_waist)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(move_waist_node src/move_waist_node.cpp)
+target_include_directories(move_waist_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(move_waist_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(move_waist_node rclcpp rclcpp_action interfaces geometry_msgs)
+
+install(TARGETS move_waist_node
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
--- a/src/modules/motion/move_waist/package.xml
+++ b/src/modules/motion/move_waist/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>move_waist</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>interfaces</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/src/modules/motion/move_waist/src/move_waist_node.cpp
+++ b/src/modules/motion/move_waist/src/move_waist_node.cpp
@@ -0,0 +1,136 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "interfaces/action/move_waist.hpp"
+
+using namespace std::chrono_literals;
+
+// MoveWaistNode 节点：
+// - 启动一个 MoveWaist 动作服务端（默认话题名称：/move_waist，可通过参数 action_name 修改）
+// - 接受目标后模拟进度反馈，支持取消，并在完成后返回结果
+class MoveWaistNode : public rclcpp::Node
+{
+public:
+  using MoveWaist = interfaces::action::MoveWaist;
+  using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
+
+  // 构造函数：
+  // - 声明参数 action_name（默认 "move_waist"，统一使用首字母大写形式避免重复）
+  // - 创建 MoveWaist 动作服务端，并绑定目标处理、取消处理和接收回调
+  MoveWaistNode()
+  : Node("move_waist_node")
+  {
+  // Declare a parameter for action name (default: "MoveWaist")
+  action_name_ = this->declare_parameter<std::string>("action_name", "move_waist");
+
+    using std::placeholders::_1;
+    using std::placeholders::_2;
+
+    server_ = rclcpp_action::create_server<MoveWaist>(
+      this->get_node_base_interface(),
+      this->get_node_clock_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      action_name_,
+      std::bind(&MoveWaistNode::handle_goal, this, _1, _2),
+      std::bind(&MoveWaistNode::handle_cancel, this, _1),
+      std::bind(&MoveWaistNode::handle_accepted, this, _1));
+
+    RCLCPP_INFO(
+      this->get_logger(), "MoveWaist action server started on '%s'",
+      action_name_.c_str());
+  }
+
+private:
+  // 目标处理回调：
+  // - 参数 uuid：目标的唯一标识
+  // - 参数 goal：ArmSpaceControl 的目标数据（target pose）
+  // - 返回值：是否接受目标，这里直接接受并执行
+  rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const MoveWaist::Goal> goal)
+  {
+    (void)uuid;
+    RCLCPP_INFO(
+      this->get_logger(), "Received MoveWaist goal: move_pitch_degree=%f, move_yaw_degree=%f",
+      goal->move_pitch_degree, goal->move_yaw_degree);
+    // Accept all goals for now. You can validate mode/effort here.
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  // 取消处理回调：
+  // - 当客户端请求取消时被调用，这里统一接受取消请求
+  rclcpp_action::CancelResponse handle_cancel(
+    const std::shared_ptr<GoalHandleMoveWaist>/*goal_handle*/)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel MoveWaist goal");
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  // 接受目标回调：
+  // - 收到新的目标后，开启独立线程执行，避免阻塞执行器线程
+  void handle_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+  {
+    // Execute goal in a new thread to avoid blocking executor thread
+    std::thread{std::bind(&MoveWaistNode::execute, this, std::placeholders::_1), goal_handle}.detach();
+  }
+
+  // 执行函数：
+  // - 模拟执行流程，按 10% 递增发布反馈(progress)
+  // - 如果检测到取消请求，则返回取消结果
+  // - 正常完成后返回 success=true，并附带简单描述信息
+  void execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+  {
+    const auto goal = goal_handle->get_goal();
+
+    auto feedback = std::make_shared<MoveWaist::Feedback>();
+    auto result = std::make_shared<MoveWaist::Result>();
+
+    // Simulate progress from 0 to 100 with small delay; react to cancel requests
+    for (int i = 0; i <= 100; i += 10) {
+      if (goal_handle->is_canceling()) {
+        result->success = false;
+        goal_handle->canceled(result);
+        RCLCPP_INFO(this->get_logger(), "MoveWaist goal canceled");
+        return;
+      }
+      // feedback->progress = static_cast<float>(i);
+      // feedback->command_id = goal->command_id;
+      // feedback->int_lenth = i;
+      goal_handle->publish_feedback(feedback);
+      RCLCPP_INFO(this->get_logger(), "MoveWaist progress: %d", i);
+      std::this_thread::sleep_for(100ms);
+    }
+
+    // Complete successfully
+    result->success = true;
+    // result->message = std::string("MoveWaist moved to target in frame '") +
+    //   goal->target.header.frame_id + "'";
+    goal_handle->succeed(result);
+    RCLCPP_INFO(this->get_logger(), "MoveWaist goal succeeded");
+  }
+
+private:
+  std::string action_name_;
+  rclcpp_action::Server<MoveWaist>::SharedPtr server_;
+};
+
+// 主函数：
+// - 初始化 ROS，启动 MotionNode，进入自旋，退出时关闭
+// 测试：
+// ros2 action send_goal --feedback /MoveWaist interfaces/action/MoveWaist "{target: {header: {frame_id: base_link}, pose: {position: {x: 0.5, y: 0.0, z: 0.3}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}}}"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<MoveWaistNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
--- a/src/scripts/gen_bt_params.py
+++ b/src/scripts/gen_bt_params.py
@@ -0,0 +1,256 @@
+#!/usr/bin/env python3
+"""
+Generate YAML params mapping for BehaviorTree action nodes.
+
+This script parses a BehaviorTree XML (e.g., src/brain/config/bt_carry_boxes.xml),
+extracts each leaf action node's instance name (the 'name' attribute) and node type
+tag (e.g., MoveWaist_H), resolves the corresponding .action definition from a
+neighboring hivecore_robot_interfaces package, and writes a YAML configuration file.
+
+Output YAML format (list):
+  - name: <bt_node_name>
+    params: |  # YAML snippet as a string
+      field_a: 0
+      field_b: []
+
+By default, interface types are resolved from ../hivecore_robot_interfaces relative to
+this repository root, matching other generator scripts.
+
+Usage:
+  python3 src/scripts/gen_bt_params.py \
+    --xml src/brain/config/bt_carry_boxes.xml \
+    --interfaces ../hivecore_robot_interfaces \
+    --out src/brain/config/bt_carry_boxes.params.yaml
+
+Notes:
+  - Only the Goal part of .action files is considered (fields before the first ---).
+  - For unknown/complex ROS types, placeholder structures are emitted (e.g., for
+    geometry_msgs/PoseStamped). Otherwise the field is set to null as a placeholder.
+"""
+
+from __future__ import annotations
+
+import argparse
+import os
+import re
+import sys
+import textwrap
+import xml.etree.ElementTree as ET
+from typing import Dict, List, Optional, Sequence, Tuple
+
+import yaml
+
+
+PRIMITIVES_DEFAULTS: Dict[str, object] = {
+    # integers
+    'int8': 0, 'int16': 0, 'int32': 0, 'int64': 0,
+    'uint8': 0, 'uint16': 0, 'uint32': 0, 'uint64': 0,
+    'int': 0,
+    # floats
+    'float32': 0.0, 'float64': 0.0, 'float': 0.0, 'double': 0.0,
+    # misc
+    'bool': False,
+    'string': '',
+    'time': 0,
+}
+
+
+def to_snake(s: str) -> str:
+    out = ''
+    for i, ch in enumerate(s):
+        if ch.isupper():
+            if i > 0:
+                out += '_'
+            out += ch.lower()
+        else:
+            out += ch
+    return out
+
+
+def snake_to_camel(s: str) -> str:
+    parts = re.split(r'[_\-]', s)
+    return ''.join(p.capitalize() for p in parts if p)
+
+
+def find_file_case_insensitive(dirname: str, filename: str) -> Optional[str]:
+    if not os.path.isdir(dirname):
+        return None
+    target = filename.lower()
+    for e in os.listdir(dirname):
+        if e.lower() == target:
+            return os.path.join(dirname, e)
+    return None
+
+
+def resolve_interface_subdir(root: str, sub: str) -> str:
+    candidates = [
+        os.path.join(root, sub),
+        os.path.join(root, 'src', sub),
+        os.path.join(root, 'interfaces', sub),
+    ]
+    for c in candidates:
+        if os.path.isdir(c):
+            return c
+    return candidates[0]
+
+
+def parse_action_file(path: str) -> List[Tuple[str, str]]:
+    """Parse a .action file, return list of (ros_type, field_name) only for Goal part."""
+    try:
+        with open(path, 'r', encoding='utf-8') as f:
+            content = f.read()
+    except FileNotFoundError:
+        return []
+    parts = content.split('\n---\n')
+    goal_part = parts[0] if parts else ''
+    fields: List[Tuple[str, str]] = []
+    for line in goal_part.splitlines():
+        line = line.strip()
+        if not line or line.startswith('#'):
+            continue
+        m = re.match(r"^([A-Za-z0-9_\[\]/]+)\s+([A-Za-z0-9_]+).*$", line)
+        if m:
+            ty = m.group(1)
+            name = m.group(2)
+            fields.append((ty, name))
+    return fields
+
+
+def locate_action_file(interfaces_root: str, base: str) -> Optional[str]:
+    action_dir = resolve_interface_subdir(interfaces_root, 'action')
+    # 1) Try as-is (CamelCase)
+    candidate = os.path.join(action_dir, f'{base}.action')
+    if os.path.exists(candidate):
+        return candidate
+    # 2) Try CamelCase of snake
+    camel = snake_to_camel(base)
+    if camel != base:
+        candidate2 = os.path.join(action_dir, f'{camel}.action')
+        if os.path.exists(candidate2):
+            return candidate2
+    # 3) Try snake_case
+    snake = to_snake(base)
+    candidate3 = os.path.join(action_dir, f'{snake}.action')
+    if os.path.exists(candidate3):
+        return candidate3
+    # 4) Case-insensitive lookup
+    found = find_file_case_insensitive(action_dir, f'{base}.action')
+    if found:
+        return found
+    # Not found
+    return None
+
+
+def make_placeholder_for_type(ros_type: str) -> object:
+    """Return a default placeholder Python value for a ROS field type.
+
+    - Primitive -> default numeric/bool/string
+    - Primitive[] -> empty list
+    - geometry_msgs/PoseStamped -> nested dict with common keys
+    - geometry_msgs/TwistStamped -> nested dict with common keys
+    - Otherwise -> None
+    """
+    is_array = ros_type.endswith('[]')
+    base = ros_type[:-2] if is_array else ros_type
+    # normalize namespace
+    base_norm = base.replace('::', '/')
+    base_norm = base_norm.replace('/msg/', '/').replace('/Msg/', '/')
+
+    if is_array:
+        return []
+    if '/' not in base_norm:
+        # primitive
+        return PRIMITIVES_DEFAULTS.get(base_norm, None)
+    if base_norm in ("geometry_msgs/PoseStamped",):
+        return {
+            'frame_id': '',
+            'stamp_sec': 0,
+            'stamp_nanosec': 0,
+            'position': {'x': 0.0, 'y': 0.0, 'z': 0.0},
+            'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0},
+        }
+    if base_norm in ("geometry_msgs/TwistStamped",):
+        return {
+            'frame_id': '',
+            'stamp_sec': 0,
+            'stamp_nanosec': 0,
+            'linear': {'x': 0.0, 'y': 0.0, 'z': 0.0},
+            'angular': {'x': 0.0, 'y': 0.0, 'z': 0.0},
+        }
+    # unknown complex -> None
+    return None
+
+
+def build_yaml_params_from_fields(fields: Sequence[Tuple[str, str]]) -> str:
+    """Create a YAML snippet string from .action Goal fields."""
+    mapping: Dict[str, object] = {}
+    for ros_type, name in fields:
+        mapping[name] = make_placeholder_for_type(ros_type)
+    # Dump as YAML block string
+    # Ensure multi-line by setting default_flow_style=False
+    snippet = yaml.safe_dump(mapping, allow_unicode=True, sort_keys=False)
+    # Guarantee trailing newline for nicer block scalars
+    if not snippet.endswith('\n'):
+        snippet += '\n'
+    return snippet
+
+
+def collect_bt_nodes(xml_path: str) -> List[Tuple[str, str]]:
+    """Return list of (instance_name, node_tag) in source order for elements with a 'name' attr.
+
+    Heuristic: We primarily care about leaf actions registered as <SkillName>_H. To be inclusive,
+    we collect any XML element with a 'name' attribute that is not a container like Sequence,
+    Fallback, RetryUntilSuccessful, etc. Containers typically don't have a 'name' in our trees,
+    and their tags are known; but we simply include all with 'name' and let type resolution
+    decide (missing interface files will yield empty params).
+    """
+    tree = ET.parse(xml_path)
+    root = tree.getroot()
+    out: List[Tuple[str, str]] = []
+    for elem in root.iter():
+        name = elem.attrib.get('name')
+        if not name:
+            continue
+        tag = elem.tag
+        out.append((name, tag))
+    return out
+
+
+def main():
+    ap = argparse.ArgumentParser(description='Generate YAML params for BT nodes.')
+    ap.add_argument('--xml', default='src/brain/config/bt_carry_boxes.xml', help='Path to BehaviorTree XML')
+    ap.add_argument('--interfaces', default='../hivecore_robot_interfaces', help='Path to hivecore_robot_interfaces root')
+    ap.add_argument('--out', default='src/brain/config/bt_carry_boxes.params.default.yaml', help='Output YAML path')
+    args = ap.parse_args()
+
+    # Resolve repo root as two levels up from this script (src/scripts/.. -> repo)
+    repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
+    xml_path = args.xml if os.path.isabs(args.xml) else os.path.join(repo_root, args.xml)
+    interfaces_root = args.interfaces if os.path.isabs(args.interfaces) else os.path.join(repo_root, args.interfaces)
+    out_path = args.out if os.path.isabs(args.out) else os.path.join(repo_root, args.out)
+
+    if not os.path.exists(xml_path):
+        print(f'ERROR: XML not found: {xml_path}', file=sys.stderr)
+        sys.exit(2)
+
+    nodes = collect_bt_nodes(xml_path)
+    results = []
+
+    for instance_name, node_tag in nodes:
+        # Map tag to interface base by stripping trailing '_H' if present
+        base = node_tag[:-2] if node_tag.endswith('_H') else node_tag
+        action_file = locate_action_file(interfaces_root, base)
+        fields: List[Tuple[str, str]] = parse_action_file(action_file) if action_file else []
+        yaml_params = build_yaml_params_from_fields(fields)
+        results.append({'name': instance_name, 'params': yaml_params})
+
+    # Write output as YAML list; ensure deterministic order as in XML
+    os.makedirs(os.path.dirname(out_path), exist_ok=True)
+    with open(out_path, 'w', encoding='utf-8') as f:
+        yaml.safe_dump(results, f, allow_unicode=True, sort_keys=False)
+
+    print(f'Wrote {len(results)} entries to {out_path}')
+
+
+if __name__ == '__main__':
+    main()
--- a/src/scripts/gen_payload_converters.py
+++ b/src/scripts/gen_payload_converters.py
@@ -0,0 +1,459 @@
+#!/usr/bin/env python3
+"""
+Generate a C++ header with YAML -> Goal/Request converters
+based on src/brain/config/robot_skills.yaml and a sibling
+'hivecore_robot_interfaces' package containing .action/.srv files.
+
+Output: src/scripts/payload_converters.hpp
+
+Usage:
+  python3 gen_payload_converters.py [--skills path/to/robot_skills.yaml] [--interfaces /path/to/hivecore_robot_interfaces] [--out path]
+
+If arguments omitted the script assumes:
+  skills: ../src/brain/config/robot_skills.yaml (relative to repo root)
+  interfaces: ../hivecore_robot_interfaces (sibling folder)
+  out: src/scripts/payload_converters.hpp
+
+This generator is conservative: it only auto-generates handling for primitive
+and array primitives. For complex ROS message types it will emit a TODO comment.
+"""
+
+import argparse
+import os
+import re
+import sys
+import textwrap
+import yaml
+
+PRIMITIVE_MAP = {
+    'int8': 'int8_t',
+    'int16': 'int16_t',
+    'float32': 'float',
+    'float64': 'double',
+    'float': 'double',
+    'double': 'double',
+    'uint8': 'uint8_t',
+    'uint16': 'uint16_t',
+    'int32': 'int32_t',
+    'int64': 'int64_t',
+    'uint32': 'uint32_t',
+    'uint64': 'uint64_t',
+    'int': 'int32_t',
+    'string': 'std::string',
+    'bool': 'bool',
+    'time': 'rclcpp::Time',
+}
+
+HEADER_PREAMBLE = '''// GENERATED FILE - do not edit by hand
+// Generated by src/scripts/gen_payload_converters.py
+#pragma once
+
+#include <yaml-cpp/yaml.h>
+#include <string>
+#include <vector>
+#include <cstdint>
+'''
+
+NAMESPACE_BEGIN = '''
+namespace brain {
+
+template<typename T>
+T from_yaml(const YAML::Node & n);
+
+'''
+
+HEADER_POSTAMBLE = '\n} // namespace brain\n'
+
+
+def to_snake(s: str) -> str:
+    out = ''
+    for i,ch in enumerate(s):
+        if ch.isupper():
+            if i>0:
+                out += '_'
+            out += ch.lower()
+        else:
+            out += ch
+    return out
+
+
+def parse_action_file(path: str):
+    """Parse an .action file, return dict with goal_fields: list of (type,name)
+    """
+    with open(path, 'r', encoding='utf-8') as f:
+        content = f.read()
+    parts = content.split('\n---\n')
+    goal_part = parts[0] if parts else ''
+    fields = []
+    for line in goal_part.splitlines():
+        line = line.strip()
+        if not line or line.startswith('#'):
+            continue
+        # lines like: float32 move_pitch_degree
+        m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
+        if m:
+            ty = m.group(1)
+            name = m.group(2)
+            fields.append((ty, name))
+    return fields
+
+
+def parse_srv_file(path: str):
+    # request before ---
+    with open(path, 'r', encoding='utf-8') as f:
+        content = f.read()
+    parts = content.split('\n---\n')
+    req_part = parts[0] if parts else ''
+    fields = []
+    for line in req_part.splitlines():
+        line = line.strip()
+        if not line or line.startswith('#'):
+            continue
+        m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
+        if m:
+            ty = m.group(1)
+            name = m.group(2)
+            fields.append((ty, name))
+    return fields
+
+
+def find_file_case_insensitive(dirname: str, filename: str):
+    """Return the path to a file in dirname matching filename case-insensitively, or None."""
+    if not os.path.isdir(dirname):
+        return None
+    target = filename.lower()
+    for e in os.listdir(dirname):
+        if e.lower() == target:
+            return os.path.join(dirname, e)
+    return None
+
+
+def make_cpp_type(ros_type: str):
+    # arrays: float32[]
+    is_array = ros_type.endswith('[]')
+    base = ros_type[:-2] if is_array else ros_type
+    if '/' in base:
+        # complex message type
+        cpp = None
+    else:
+        cpp = PRIMITIVE_MAP.get(base)
+    return is_array, cpp, base
+
+
+def snake_to_camel(s: str) -> str:
+    # move_waist -> MoveWaist
+    parts = re.split(r'[_\-]', s)
+    return ''.join(p.capitalize() for p in parts if p)
+
+
+def resolve_interface_subdir(root: str, sub: str):
+    """Return the first existing directory for common layouts: root/sub, root/src/sub, root/interfaces/sub."""
+    candidates = [
+        os.path.join(root, sub),
+        os.path.join(root, 'src', sub),
+        os.path.join(root, 'interfaces', sub),
+    ]
+    for c in candidates:
+        if os.path.isdir(c):
+            return c
+    # default to primary location even if missing
+    return candidates[0]
+
+
+def gen_converter_action(base: str, fields, include_path: str):
+    # base: MoveWaist
+    cpp = []
+    cpp.append(f'// {base}::Goal: ' + ', '.join([n for _,n in fields]))
+    cpp.append('template<>')
+    cpp.append(f'inline interfaces::action::{base}::Goal from_yaml<interfaces::action::{base}::Goal>(const YAML::Node & n)')
+    cpp.append('{')
+    cpp.append(f'  interfaces::action::{base}::Goal g;')
+    cpp.append('  if (n && n.IsMap()) {')
+    has_data_array = any(n == 'data_array' for _, n in fields)
+    has_data_length = any(n == 'data_length' for _, n in fields)
+    for ty,name in fields:
+        is_array, cpp_type, base_type = make_cpp_type(ty)
+        if is_array and cpp_type:
+            cpp.append(f'    if (n["{name}"]) {{')
+            cpp.append(f'      auto vec = n["{name}"].as<std::vector<{cpp_type}>>();')
+            cpp.append(f'      g.{name}.assign(vec.begin(), vec.end());')
+            cpp.append('    }')
+        elif not is_array and cpp_type:
+            # map to as<type>()
+            cpp.append(f'    if (n["{name}"]) g.{name} = n["{name}"].as<{cpp_type}>();')
+        else:
+            # complex types: handle some known messages
+            # normalize geometry type names like geometry_msgs/msg/PoseStamped -> geometry_msgs/PoseStamped
+            bt = base_type.replace('::', '/')
+            bt = bt.replace('/msg/', '/')
+            bt = bt.replace('/Msg/', '/')
+            if bt in ("geometry_msgs/PoseStamped", "geometry_msgs/PoseStamped") and not is_array:
+                cpp.append('    // geometry_msgs::msg::PoseStamped')
+                cpp.append('    if (n) {')
+                cpp.append('      if (n["frame_id"]) g.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
+                cpp.append('      if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
+                cpp.append('      if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
+                cpp.append('      if (auto p = n["position"]) {')
+                cpp.append('        if (p["x"]) g.{0}.pose.position.x = p["x"].as<double>();'.format(name))
+                cpp.append('        if (p["y"]) g.{0}.pose.position.y = p["y"].as<double>();'.format(name))
+                cpp.append('        if (p["z"]) g.{0}.pose.position.z = p["z"].as<double>();'.format(name))
+                cpp.append('      }')
+                cpp.append('      if (auto q = n["orientation"]) {')
+                cpp.append('        if (q["x"]) g.{0}.pose.orientation.x = q["x"].as<double>();'.format(name))
+                cpp.append('        if (q["y"]) g.{0}.pose.orientation.y = q["y"].as<double>();'.format(name))
+                cpp.append('        if (q["z"]) g.{0}.pose.orientation.z = q["z"].as<double>();'.format(name))
+                cpp.append('        if (q["w"]) g.{0}.pose.orientation.w = q["w"].as<double>();'.format(name))
+                cpp.append('      }')
+                cpp.append('    }')
+            elif bt in ("geometry_msgs/TwistStamped", "geometry_msgs/TwistStamped") and not is_array:
+                cpp.append('    // geometry_msgs::msg::TwistStamped')
+                cpp.append('    if (n) {')
+                cpp.append('      if (n["frame_id"]) g.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
+                cpp.append('      if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
+                cpp.append('      if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
+                cpp.append('      if (auto lin = n["linear"]) {')
+                cpp.append('        if (lin["x"]) g.{0}.twist.linear.x = lin["x"].as<double>();'.format(name))
+                cpp.append('        if (lin["y"]) g.{0}.twist.linear.y = lin["y"].as<double>();'.format(name))
+                cpp.append('        if (lin["z"]) g.{0}.twist.linear.z = lin["z"].as<double>();'.format(name))
+                cpp.append('      }')
+                cpp.append('      if (auto ang = n["angular"]) {')
+                cpp.append('        if (ang["x"]) g.{0}.twist.angular.x = ang["x"].as<double>();'.format(name))
+                cpp.append('        if (ang["y"]) g.{0}.twist.angular.y = ang["y"].as<double>();'.format(name))
+                cpp.append('        if (ang["z"]) g.{0}.twist.angular.z = ang["z"].as<double>();'.format(name))
+                cpp.append('      }')
+                cpp.append('    }')
+            else:
+                cpp.append(f'    // TODO: parse field `{name}` of ROS type `{ty}` into g.{name} (complex type)')
+    # post-processing example: if both data_array and data_length exist, set length from array size
+    if has_data_array and has_data_length:
+        cpp.append('  if (!g.data_array.empty()) {')
+        cpp.append('    using DL = decltype(g.data_length);')
+        cpp.append('    g.data_length = static_cast<DL>(g.data_array.size());')
+        cpp.append('  }')
+    cpp.append('  }')
+    cpp.append('  return g;')
+    cpp.append('}\n')
+    return '\n'.join(cpp)
+
+
+def gen_converter_srv(base: str, fields, include_path: str):
+    cpp = []
+    cpp.append(f'// {base}::Request: ' + ', '.join([n for _,n in fields]))
+    cpp.append('template<>')
+    cpp.append(f'inline interfaces::srv::{base}::Request from_yaml<interfaces::srv::{base}::Request>(const YAML::Node & n)')
+    cpp.append('{')
+    cpp.append(f'  interfaces::srv::{base}::Request r;')
+    cpp.append('  if (n && n.IsMap()) {')
+    for ty,name in fields:
+        is_array, cpp_type, base_type = make_cpp_type(ty)
+        if is_array and cpp_type:
+            cpp.append(f'    if (n["{name}"]) {{')
+            cpp.append(f'      auto vec = n["{name}"].as<std::vector<{cpp_type}>>();')
+            cpp.append(f'      r.{name}.assign(vec.begin(), vec.end());')
+            cpp.append('    }')
+        elif not is_array and cpp_type:
+            cpp.append(f'    if (n["{name}"]) r.{name} = n["{name}"].as<{cpp_type}>();')
+        else:
+            bt = base_type.replace('::', '/')
+            bt = bt.replace('/msg/', '/')
+            bt = bt.replace('/Msg/', '/')
+            if bt in ("geometry_msgs/PoseStamped", "geometry_msgs/PoseStamped") and not is_array:
+                cpp.append('    // geometry_msgs::msg::PoseStamped')
+                cpp.append('    if (n) {')
+                cpp.append('      if (n["frame_id"]) r.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
+                cpp.append('      if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
+                cpp.append('      if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
+                cpp.append('      if (auto p = n["position"]) {')
+                cpp.append('        if (p["x"]) r.{0}.pose.position.x = p["x"].as<double>();'.format(name))
+                cpp.append('        if (p["y"]) r.{0}.pose.position.y = p["y"].as<double>();'.format(name))
+                cpp.append('        if (p["z"]) r.{0}.pose.position.z = p["z"].as<double>();'.format(name))
+                cpp.append('      }')
+                cpp.append('      if (auto q = n["orientation"]) {')
+                cpp.append('        if (q["x"]) r.{0}.pose.orientation.x = q["x"].as<double>();'.format(name))
+                cpp.append('        if (q["y"]) r.{0}.pose.orientation.y = q["y"].as<double>();'.format(name))
+                cpp.append('        if (q["z"]) r.{0}.pose.orientation.z = q["z"].as<double>();'.format(name))
+                cpp.append('        if (q["w"]) r.{0}.pose.orientation.w = q["w"].as<double>();'.format(name))
+                cpp.append('      }')
+                cpp.append('    }')
+            elif bt in ("geometry_msgs/TwistStamped", "geometry_msgs/TwistStamped") and not is_array:
+                cpp.append('    // geometry_msgs::msg::TwistStamped')
+                cpp.append('    if (n) {')
+                cpp.append('      if (n["frame_id"]) r.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
+                cpp.append('      if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
+                cpp.append('      if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
+                cpp.append('      if (auto lin = n["linear"]) {')
+                cpp.append('        if (lin["x"]) r.{0}.twist.linear.x = lin["x"].as<double>();'.format(name))
+                cpp.append('        if (lin["y"]) r.{0}.twist.linear.y = lin["y"].as<double>();'.format(name))
+                cpp.append('        if (lin["z"]) r.{0}.twist.linear.z = lin["z"].as<double>();'.format(name))
+                cpp.append('      }')
+                cpp.append('      if (auto ang = n["angular"]) {')
+                cpp.append('        if (ang["x"]) r.{0}.twist.angular.x = ang["x"].as<double>();'.format(name))
+                cpp.append('        if (ang["y"]) r.{0}.twist.angular.y = ang["y"].as<double>();'.format(name))
+                cpp.append('        if (ang["z"]) r.{0}.twist.angular.z = ang["z"].as<double>();'.format(name))
+                cpp.append('      }')
+                cpp.append('    }')
+            else:
+                cpp.append(f'    // TODO: parse field `{name}` of ROS type `{ty}` into r.{name} (complex type)')
+    cpp.append('  }')
+    cpp.append('  return r;')
+    cpp.append('}\n')
+    return '\n'.join(cpp)
+
+
+def main():
+    p = argparse.ArgumentParser()
+    p.add_argument('--skills', default='src/brain/config/robot_skills.yaml')
+    p.add_argument('--interfaces', default='../hivecore_robot_interfaces')
+    p.add_argument('--out', default='src/brain/include/brain/payload_converters.hpp')
+    args = p.parse_args()
+
+    repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
+    skills_path = args.skills if os.path.isabs(args.skills) else os.path.join(repo_root, args.skills)
+    interfaces_root = args.interfaces if os.path.isabs(args.interfaces) else os.path.join(repo_root, args.interfaces)
+    out_path = args.out if os.path.isabs(args.out) else os.path.join(repo_root, args.out)
+
+    if not os.path.exists(skills_path):
+        print('ERROR: skills file not found:', skills_path, file=sys.stderr)
+        sys.exit(2)
+
+    print('Using skills:', skills_path)
+    print('Using interfaces root:', interfaces_root)
+    print('Writing output to:', out_path)
+
+    with open(skills_path, 'r', encoding='utf-8') as f:
+        skills = yaml.safe_load(f)
+
+    includes = set()
+    converters = []
+    added_actions = set()
+    added_srvs = set()
+
+    for item in skills:
+        if not isinstance(item, dict):
+            continue
+        name = item.get('name')
+        interfaces = item.get('interfaces', [])
+        for iface in interfaces:
+            token = None
+            if isinstance(iface, str):
+                token = iface
+            elif isinstance(iface, dict):
+                token = iface.get('name')
+            if not token:
+                continue
+            # token like BaseName.action
+            if '.' not in token:
+                continue
+            base, kind = token.split('.', 1)
+            kind = kind.lower()
+            header_snake = to_snake(base)
+            if kind.startswith('action'):
+                includes.add(f'"interfaces/action/{header_snake}.hpp"')
+                # locate .action file in common subdirs
+                action_dir = resolve_interface_subdir(interfaces_root, 'action')
+                tried_paths = []
+                action_file = os.path.join(action_dir, base + '.action')
+                tried_paths.append(action_file)
+                if not os.path.exists(action_file):
+                    camel = snake_to_camel(base)
+                    if camel != base:
+                        action_file_camel = os.path.join(action_dir, camel + '.action')
+                        tried_paths.append(action_file_camel)
+                        if os.path.exists(action_file_camel):
+                            action_file = action_file_camel
+                if not os.path.exists(action_file):
+                    found = find_file_case_insensitive(action_dir, base + '.action')
+                    if found:
+                        action_file = found
+                if not os.path.exists(action_file):
+                    # final fallback to snake name in case-insensitive form
+                    snake_path = os.path.join(action_dir, header_snake + '.action')
+                    tried_paths.append(snake_path)
+                    if os.path.exists(snake_path):
+                        action_file = snake_path
+                if not os.path.exists(action_file):
+                    print('WARN: action file not found for', base, 'checked', tried_paths[-1])
+                    fields = []
+                else:
+                    fields = parse_action_file(action_file)
+                converters.append(gen_converter_action(base, fields, header_snake))
+                added_actions.add(base)
+            elif kind.startswith('srv') or kind.startswith('service'):
+                includes.add(f'"interfaces/srv/{header_snake}.hpp"')
+                srv_dir = resolve_interface_subdir(interfaces_root, 'srv')
+                tried_paths = []
+                srv_file = os.path.join(srv_dir, base + '.srv')
+                tried_paths.append(srv_file)
+                if not os.path.exists(srv_file):
+                    # try snake_case -> CamelCase fallback
+                    camel = snake_to_camel(base)
+                    if camel != base:
+                        srv_file_camel = os.path.join(srv_dir, camel + '.srv')
+                        tried_paths.append(srv_file_camel)
+                        if os.path.exists(srv_file_camel):
+                            srv_file = srv_file_camel
+                    if srv_file == os.path.join(srv_dir, base + '.srv'):
+                        found = find_file_case_insensitive(srv_dir, base + '.srv')
+                        if found:
+                            srv_file = found
+                        else:
+                            srv_file = os.path.join(srv_dir, header_snake + '.srv')
+                            tried_paths.append(srv_file)
+                if not os.path.exists(srv_file):
+                    print('WARN: srv file not found for', base, 'checked', tried_paths[-1])
+                    fields = []
+                else:
+                    fields = parse_srv_file(srv_file)
+                converters.append(gen_converter_srv(base, fields, header_snake))
+                added_srvs.add(base)
+            # end of iface loop
+            pass
+
+    # Second pass: also generate for all action/srv files in interfaces even if not referenced in skills
+    action_dir = resolve_interface_subdir(interfaces_root, 'action')
+    if os.path.isdir(action_dir):
+        for fname in os.listdir(action_dir):
+            if not fname.endswith('.action'):
+                continue
+            base_name = os.path.splitext(fname)[0]
+            # preserve filename's CamelCase as type, only use to_snake for header path
+            base_type = base_name
+            if base_type in added_actions:
+                continue
+            header_snake = to_snake(base_type)
+            includes.add(f'"interfaces/action/{header_snake}.hpp"')
+            fields = parse_action_file(os.path.join(action_dir, fname))
+            converters.append(gen_converter_action(base_type, fields, header_snake))
+            added_actions.add(base_type)
+
+    srv_dir = resolve_interface_subdir(interfaces_root, 'srv')
+    if os.path.isdir(srv_dir):
+        for fname in os.listdir(srv_dir):
+            if not fname.endswith('.srv'):
+                continue
+            base_name = os.path.splitext(fname)[0]
+            base_type = base_name
+            if base_type in added_srvs:
+                continue
+            header_snake = to_snake(base_type)
+            includes.add(f'"interfaces/srv/{header_snake}.hpp"')
+            fields = parse_srv_file(os.path.join(srv_dir, fname))
+            converters.append(gen_converter_srv(base_type, fields, header_snake))
+            added_srvs.add(base_type)
+
+    # assemble header
+    os.makedirs(os.path.dirname(out_path), exist_ok=True)
+    with open(out_path, 'w', encoding='utf-8') as outf:
+        outf.write(HEADER_PREAMBLE)
+        if includes:
+            for inc in sorted(includes):
+                outf.write(f'#include {inc}\n')
+            outf.write('\n')
+        outf.write(NAMESPACE_BEGIN)
+        outf.write('\n'.join(converters))
+        outf.write(HEADER_POSTAMBLE)
+
+    print('Done.')
+
+if __name__ == '__main__':
+    main()
--- a/src/scripts/gen_robot_config.py
+++ b/src/scripts/gen_robot_config.py
@@ -0,0 +1,182 @@
+#!/usr/bin/env python3
+"""
+Generate a C++ header `robot_config.hpp` from YAML `robot_config.yaml`.
+
+Requirements:
+- The generated header must parse the YAML file at runtime (do not embed YAML values).
+- Provide a clear C++ interface to query configuration by name and key.
+- Follow Google C++ style (CamelCase types/functions, namespace lower_case, member_ with trailing underscore, include guards).
+
+YAML example:
+    - name: brain
+        version: 1.0.0
+        skill_file: robot_skills.yaml
+    - name: cerebrum_node
+        version: 1.0.0
+        bt_config_file: bt_carry_boxes.xml
+        bt_params_file: bt_carry_boxes.params.yaml
+
+The header exposes:
+    - brain::robot_config::Entry: holds a name and a map of string keys->string values.
+    - brain::robot_config::RobotConfig: loader and query API.
+        * bool LoadFromFile(const std::string& path)
+        * const Entry* Find(const std::string& name) const
+        * std::vector<std::string> Names() const
+        * std::optional<std::string> GetValue(const std::string& name, const std::string& key) const
+        * Typed helpers for all keys present in YAML (CamelCase)
+
+Usage:
+    python3 src/scripts/gen_robot_config.py \
+        --in src/brain/config/robot_config.yaml \
+        --out src/brain/include/robot_config.hpp
+"""
+from __future__ import annotations
+
+import argparse
+import os
+from pathlib import Path
+from typing import List
+
+import yaml
+
+
+def to_camel(s: str) -> str:
+    import re
+    parts = re.split(r"[^A-Za-z0-9]+", s)
+    parts = [p for p in parts if p]
+    camel = "".join(p[:1].upper() + p[1:] for p in parts)
+    if not camel or not camel[0].isalpha():
+        camel = "X" + camel
+    return camel
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="Generate robot_config.hpp from YAML")
+    parser.add_argument("--in", dest="inp", default="src/brain/config/robot_config.yaml")
+    parser.add_argument("--out", dest="out", default="src/brain/include/brain/robot_config.hpp")
+    args = parser.parse_args()
+
+    repo_root = Path(__file__).resolve().parents[2]
+    in_path = Path(args.inp) if os.path.isabs(args.inp) else repo_root / args.inp
+    out_path = Path(args.out) if os.path.isabs(args.out) else repo_root / args.out
+
+    if not in_path.exists():
+        raise SystemExit(f"Config YAML not found: {in_path}")
+
+    data = yaml.safe_load(in_path.read_text(encoding="utf-8")) or []
+    if not isinstance(data, list):
+        raise SystemExit("robot_config.yaml must be a YAML sequence (list)")
+
+    # Collect all keys across items (ensure 'name' exists first)
+    key_set = {"name"}
+    preferred_order = ["version", "skill_file", "bt_config_file", "bt_params_file"]
+    for item in data:
+        if isinstance(item, dict):
+            for k in item.keys():
+                key_set.add(k)
+
+    # Build ordered keys: name, preferred if present, then remaining alphabetically
+    keys: List[str] = ["name"]
+    for k in preferred_order:
+        if k in key_set and k not in keys:
+            keys.append(k)
+    keys.extend(sorted(k for k in key_set if k not in keys))
+
+    out_path.parent.mkdir(parents=True, exist_ok=True)
+    with out_path.open("w", encoding="utf-8") as out:
+        out.write("// GENERATED FILE - do not edit by hand\n")
+        out.write("// Generated by src/scripts/gen_robot_config.py\n")
+        out.write("#ifndef BRAIN_ROBOT_CONFIG_HPP_\n")
+        out.write("#define BRAIN_ROBOT_CONFIG_HPP_\n\n")
+        out.write("#include <yaml-cpp/yaml.h>\n")
+        out.write("#include <cstddef>\n")
+        out.write("#include <optional>\n")
+        out.write("#include <string>\n")
+        out.write("#include <string_view>\n")
+        out.write("#include <unordered_map>\n")
+        out.write("#include <utility>\n")
+        out.write("#include <vector>\n")
+        out.write("#include <ament_index_cpp/get_package_share_directory.hpp>\n\n")
+        out.write("namespace brain { namespace robot_config {\n\n")
+        out.write("// Represents a single entry parsed from robot_config.yaml.\n")
+        out.write("struct Entry {\n")
+        out.write("  std::string name;\n")
+        out.write("  std::unordered_map<std::string, std::string> fields;\n")
+        out.write("};\n\n")
+        out.write("// Loader and query interface for robot configuration.\n")
+        out.write("class RobotConfig {\n")
+        out.write(" public:\n")
+        out.write("  RobotConfig() {\n")
+        out.write(
+            "    LoadFromFile(ament_index_cpp::get_package_share_directory(\"brain\") + std::string(\"/config/robot_config.yaml\"));\n"
+        )
+        out.write("  }\n")
+        out.write("  // Parse YAML from a file path. Returns true on success.\n")
+        out.write("  bool LoadFromFile(const std::string& path) {\n")
+        out.write("    entries_.clear();\n")
+        out.write("    YAML::Node root = YAML::LoadFile(path);\n")
+        out.write("    if (!root || !root.IsSequence()) { return false; }\n")
+        out.write("    for (const auto& node : root) {\n")
+        out.write("      if (!node.IsMap()) { continue; }\n")
+        out.write("      Entry e;\n")
+        out.write("      auto name_it = node[\"name\"];\n")
+        out.write("      if (name_it && name_it.IsScalar()) { e.name = name_it.as<std::string>(); }\n")
+        out.write("      for (auto it = node.begin(); it != node.end(); ++it) {\n")
+        out.write("        const auto key = it->first.as<std::string>();\n")
+        out.write("        if (key == \"name\") { continue; }\n")
+        out.write("        if (it->second) {\n")
+        out.write("          if (it->second.IsScalar()) {\n")
+        out.write("            e.fields.emplace(key, it->second.as<std::string>());\n")
+        out.write("          } else {\n")
+        out.write("            e.fields.emplace(key, YAML::Dump(it->second));\n")
+        out.write("          }\n")
+        out.write("        }\n")
+        out.write("      }\n")
+        out.write("      if (!e.name.empty()) { entries_.emplace_back(std::move(e)); }\n")
+        out.write("    }\n")
+        out.write("    return true;\n")
+        out.write("  }\n\n")
+        out.write("  // Return pointer to an Entry with matching name or nullptr if not found.\n")
+        out.write("  const Entry* Find(const std::string& name) const {\n")
+        out.write("    for (const auto& e : entries_) { if (e.name == name) return &e; }\n")
+        out.write("    return nullptr;\n")
+        out.write("  }\n\n")
+        out.write("  // List all entry names.\n")
+        out.write("  std::vector<std::string> Names() const {\n")
+        out.write("    std::vector<std::string> out; out.reserve(entries_.size());\n")
+        out.write("    for (const auto& e : entries_) out.push_back(e.name);\n")
+        out.write("    return out;\n")
+        out.write("  }\n\n")
+        out.write("  // Generic value getter by name and key.\n")
+        out.write(
+            "  std::optional<std::string> GetValue(const std::string& name, const std::string& key) const {\n"
+        )
+        out.write("    const Entry* e = Find(name); if (!e) return std::nullopt;\n")
+        out.write("    auto it = e->fields.find(key);\n")
+        out.write("    if (it == e->fields.end()) return std::nullopt;\n")
+        out.write("    return it->second;\n")
+        out.write("  }\n\n")
+
+        # Typed getters for all keys except 'name'
+        for k in keys:
+            if k == "name":
+                continue
+            func = to_camel(k)
+            out.write(
+                f"  std::optional<std::string> {func}(const std::string& name) const " + "{\n"
+            )
+            out.write(f"    return GetValue(name, \"{k}\");\n")
+            out.write("  }\n\n")
+
+        out.write(" private:\n")
+        out.write("  std::vector<Entry> entries_;\n")
+        out.write("};\n\n")
+        out.write("}} // namespace robot_config; } // namespace brain\n\n")
+        out.write("#endif  // BRAIN_ROBOT_CONFIG_HPP_\n")
+
+    print(f"Wrote {out_path}")
+
+
+if __name__ == "__main__":
+    main()
+
--- a/src/scripts/gen_skill_config.py
+++ b/src/scripts/gen_skill_config.py
@@ -0,0 +1,278 @@
+#!/usr/bin/env python3
+"""
+Generate C++ header `skill_config.hpp` from YAML skill descriptions and interface definitions.
+
+Usage: ./gen_skill_config.py
+
+- Reads `src/brain/config/robot_skills.yaml` for skill entries.
+- Scans sibling package `../hivecore_robot_interfaces/src/{action,srv}` for available interfaces.
+- Writes `src/brain/include/brain/skill_config.hpp` containing:
+  - SkillActionTypes / SkillServiceTypes tuples
+  - SkillActionTrait and SkillServiceTrait specializations with `skill_name` and `default_topic` fields
+
+The script is intentionally conservative: it emits empty default_topic values; you can edit generated header or extend the YAML to include explicit default_topic overrides.
+"""
+
+import os
+import sys
+import yaml
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[2]
+SKILL_YAML = ROOT / 'src' / 'brain' / 'config' / 'robot_skills.yaml'
+INTERFACES_DIR = ROOT.parent / 'hivecore_robot_interfaces' / 'src'
+OUT_HEADER = ROOT / 'src' / 'brain' / 'include' / 'brain' / 'skill_config.hpp'
+
+if not SKILL_YAML.exists():
+    print(f"Skill YAML not found: {SKILL_YAML}")
+    sys.exit(1)
+
+if not INTERFACES_DIR.exists():
+    print(f"Interfaces dir not found: {INTERFACES_DIR}")
+    sys.exit(1)
+
+# Gather available action and srv base names from the interfaces package
+action_dir = INTERFACES_DIR / 'action'
+srv_dir = INTERFACES_DIR / 'srv'
+
+actions = []
+services = []
+
+if action_dir.exists():
+    for p in sorted(action_dir.iterdir()):
+        if p.suffix == '.action':
+            actions.append(p.stem)
+if srv_dir.exists():
+    for p in sorted(srv_dir.iterdir()):
+        if p.suffix == '.srv':
+            services.append(p.stem)
+
+# Read skills YAML
+with SKILL_YAML.open() as f:
+    skills = yaml.safe_load(f)
+
+# Extract unique action/service types referenced by skills in the YAML
+skill_action_types = []
+skill_service_types = []
+# allow optional default_topic per interface: map base -> default_topic
+action_default_map = {}
+service_default_map = {}
+
+def parse_iface_item(item):
+    """Return (base, kind, default_topic) for an interface YAML item.
+
+    Supported forms:
+      - "Arm.action"  (string)
+      - { name: "Arm.action", default_topic: "ArmAction" }
+      - "Arm.action": { default_topic: "ArmAction" }  (less common)
+    """
+    if isinstance(item, str):
+        s = item
+        if s.endswith('.action'):
+            return s[:-len('.action')], 'action', ''
+        if s.endswith('.srv'):
+            return s[:-len('.srv')], 'srv', ''
+        return None
+    if isinstance(item, dict):
+        # dict can be either { name: ... , default_topic: ... } or { "Arm.action": { default_topic: ... } }
+        if 'name' in item:
+            name = item['name']
+            d = item.get('default_topic', '')
+            if name.endswith('.action'):
+                return name[:-len('.action')], 'action', d
+            if name.endswith('.srv'):
+                return name[:-len('.srv')], 'srv', d
+            return None
+        # fallback: single kv pair mapping
+        if len(item) == 1:
+            k, v = next(iter(item.items()))
+            if isinstance(v, dict):
+                d = v.get('default_topic', '')
+                if k.endswith('.action'):
+                    return k[:-len('.action')], 'action', d
+                if k.endswith('.srv'):
+                    return k[:-len('.srv')], 'srv', d
+        return None
+
+for entry in skills:
+    if 'interfaces' not in entry:
+        continue
+    for iface in entry['interfaces']:
+        parsed = parse_iface_item(iface)
+        if not parsed:
+            continue
+        base, kind, dtopic = parsed
+        if kind == 'action':
+            if base not in skill_action_types:
+                skill_action_types.append(base)
+            if dtopic:
+                action_default_map[base] = dtopic
+        elif kind == 'srv':
+            if base not in skill_service_types:
+                skill_service_types.append(base)
+            if dtopic:
+                service_default_map[base] = dtopic
+
+# Helper to map base name to include path / C++ type name.
+# Assumption: interfaces live in package `interfaces` and types are under `interfaces::action::Name` or `interfaces::srv::Name`.
+
+def action_cpp_type(base):
+    return f"interfaces::action::{base}"
+
+def service_cpp_type(base):
+    return f"interfaces::srv::{base}"
+
+def to_snake_case(s: str) -> str:
+    out = []
+    for i, ch in enumerate(s):
+        if ch.isupper():
+            if i != 0:
+                out.append('_')
+            out.append(ch.lower())
+        else:
+            out.append(ch)
+    return ''.join(out)
+
+
+def parse_action_result_fields(action_file: Path):
+    """Parse the Result section of an .action file and return a dict name->type.
+
+    If the file doesn't exist or parsing fails, return empty dict.
+    """
+    if not action_file.exists():
+        return {}
+    text = action_file.read_text()
+    parts = [p.strip() for p in text.split('---')]
+    # Expect parts: [goal, result, feedback] usually. We want parts[1] if exists.
+    if len(parts) < 2:
+        return {}
+    result_section = parts[1]
+    fields = {}
+    for line in result_section.splitlines():
+        line = line.strip()
+        if not line or line.startswith('#'):
+            continue
+        # remove inline comments
+        if '#' in line:
+            line = line.split('#', 1)[0].strip()
+        toks = line.split()
+        if len(toks) < 2:
+            continue
+        typ = toks[0]
+        name = toks[1]
+        # strip any array or filler
+        name = name.split('[')[0]
+        fields[name] = typ
+    return fields
+
+
+def pick_success_and_message_fields(result_fields: dict):
+    """Given a mapping name->type, choose success_field and message_field heuristically.
+
+    Returns (success_field_name_or_None, message_field_name_or_None, success_kind)
+    success_kind: 'bool', 'int_zero', or None
+    """
+    # normalize types
+    bool_names = []
+    int_names = []
+    string_names = []
+    for name, typ in result_fields.items():
+        t = typ.lower()
+        if 'bool' in t:
+            bool_names.append(name)
+        elif any(x in t for x in ('int8', 'int16', 'int32', 'int64', 'uint8', 'uint16', 'uint32', 'uint64', 'byte', 'char', 'long')):
+            int_names.append(name)
+        elif 'string' in t or 'char' in t:
+            string_names.append(name)
+        else:
+            # fallback: treat unknown as string if name suggests so
+            if 'msg' in name or 'message' in name or 'detail' in name:
+                string_names.append(name)
+    # choose success
+    if 'success' in result_fields and 'success' in bool_names:
+        return 'success', ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), 'bool'
+    if bool_names:
+        return bool_names[0], ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), 'bool'
+    if 'result' in result_fields and 'result' in int_names:
+        return 'result', ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), 'int_zero'
+    if int_names:
+        return int_names[0], ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), 'int_zero'
+    # no good numeric/bool -> fallback
+    return None, ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), None
+
+# Generate header
+OUT_HEADER.parent.mkdir(parents=True, exist_ok=True)
+with OUT_HEADER.open('w') as out:
+    out.write('// Generated by scripts/gen_skill_config.py — do not edit by hand unless you know what you are doing.\n')
+    out.write('#pragma once\n\n')
+    out.write('#include <tuple>\n')
+    out.write('#include <string>\n')
+    out.write('\n')
+    # includes
+    for a in sorted(skill_action_types):
+        out.write(f'#include "interfaces/action/{to_snake_case(a)}.hpp"\n')
+    for s in sorted(skill_service_types):
+        out.write(f'#include "interfaces/srv/{to_snake_case(s)}.hpp"\n')
+    out.write('\nnamespace brain {\n\n')
+    # forward templates
+    out.write('template<typename ActionT>\nstruct SkillActionTrait;\n\n')
+    out.write('template<typename ServiceT>\nstruct SkillServiceTrait;\n\n')
+    # tuples
+    out.write('using SkillActionTypes = std::tuple<\n')
+    for i, a in enumerate(skill_action_types):
+        sep = ',' if i + 1 < len(skill_action_types) else ''
+        out.write(f'  {action_cpp_type(a)}{sep}\n')
+    out.write('>;\n\n')
+    out.write('using SkillServiceTypes = std::tuple<\n')
+    for i, s in enumerate(skill_service_types):
+        sep = ',' if i + 1 < len(skill_service_types) else ''
+        out.write(f'  {service_cpp_type(s)}{sep}\n')
+    out.write('>;\n\n')
+    # specializations
+    for a in skill_action_types:
+            d = action_default_map.get(a, '')
+            d_esc = d.replace('"', '\\"')
+            out.write(f'template<>\nstruct SkillActionTrait<{action_cpp_type(a)}>\n{{\n')
+            out.write(f'  static constexpr const char * skill_name = "{a}";\n')
+            out.write(f'  static constexpr const char * default_topic = "{d_esc}";\n')
+            # attempt to parse the corresponding .action file to synthesize success/message helpers
+            # Try multiple filename candidates: CamelCase, snake_case, lowercase
+            action_dir_root = INTERFACES_DIR / 'action'
+            candidates = [
+                action_dir_root / f"{a}.action",
+                action_dir_root / f"{to_snake_case(a)}.action",
+                action_dir_root / f"{a.lower()}.action",
+            ]
+            action_file_found = None
+            for c in candidates:
+                if c.exists():
+                    action_file_found = c
+                    break
+            result_fields = parse_action_result_fields(action_file_found) if action_file_found is not None else {}
+            success_field, message_field, success_kind = pick_success_and_message_fields(result_fields)
+            if success_kind == 'bool' and success_field is not None:
+                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r) {{return r.{success_field};}}\n')
+            elif success_kind == 'int_zero' and success_field is not None:
+                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r) {{return (r.{success_field} == 0);}}\n')
+            else:
+                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r) {{(void)r; return true;}}\n')
+            if message_field is not None:
+                out.write(f'  static std::string message(const {action_cpp_type(a)}::Result & r) {{return r.{message_field};}}\n')
+            else:
+                out.write(f'  static std::string message(const {action_cpp_type(a)}::Result & r) {{(void)r; return "completed";}}\n')
+            out.write('};\n\n')
+    for s in skill_service_types:
+        d = service_default_map.get(s, '')
+        d_esc = d.replace('"', '\\"')
+        out.write(f'template<>\nstruct SkillServiceTrait<{service_cpp_type(s)}>\n{{\n')
+        out.write(f'  static constexpr const char * skill_name = "{s}";\n')
+        out.write(f'  static constexpr const char * default_topic = "{d_esc}";\n')
+        out.write('};\n\n')
+
+    out.write('} // namespace brain\n')
+
+print(f'Wrote {OUT_HEADER}')
+
+# Make script executable
+os.chmod(Path(__file__), 0o755)
+print('Done.')
--- a/src/scripts/gen_skill_manager.py
+++ b/src/scripts/gen_skill_manager.py