optimize code, add nav and arm interfaces

2025-10-10 09:45:56 +08:00
parent dc632d008f
commit 75ba0f9429
14 changed files with 2185 additions and 453 deletions
--- a/src/brain/CMakeLists.txt
+++ b/src/brain/CMakeLists.txt
@@ -26,6 +26,7 @@ find_package(yaml-cpp REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(Doxygen QUIET)
 find_package(std_srvs REQUIRED)
+find_package(nav2_msgs REQUIRED)

 # 创建一个可执行文件，将两个节点编译到一个进程中
 add_executable(brain_node
@@ -51,6 +52,7 @@ ament_target_dependencies(brain_node
  std_msgs
  smacc2
  smacc2_msgs
+  nav2_msgs
  rclcpp_lifecycle
  ament_index_cpp
  yaml-cpp
@@ -96,6 +98,7 @@ if(BUILD_TESTING)
      rclcpp_action
      interfaces
      brain_interfaces
+      nav2_msgs
      std_srvs
    )
    target_include_directories(test_action_registry PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
@@ -113,6 +116,18 @@ if(BUILD_TESTING)
    target_include_directories(test_bt_registry PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
  endif()

+  ament_add_gtest(test_bt_registry_xml
+    test/test_bt_registry_xml.cpp
+  )
+  if(TARGET test_bt_registry_xml)
+    ament_target_dependencies(test_bt_registry_xml
+      rclcpp
+      behaviortree_cpp
+      std_srvs
+    )
+    target_include_directories(test_bt_registry_xml PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+  endif()
+
  ament_add_gtest(test_cerebellum_node
    test/test_cerebellum_node.cpp
    src/cerebellum_node.cpp
@@ -129,6 +144,7 @@ if(BUILD_TESTING)
      ament_index_cpp
      interfaces
      brain_interfaces
+      nav2_msgs
      std_srvs
    )
    target_include_directories(test_cerebellum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
@@ -153,6 +169,7 @@ if(BUILD_TESTING)
      smacc2
      smacc2_msgs
      ament_index_cpp
+      nav2_msgs
      std_srvs
    )
    target_include_directories(test_cerebrum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
@@ -171,6 +188,7 @@ if(BUILD_TESTING)
      smacc2_msgs
      interfaces
      brain_interfaces
+      nav2_msgs
      std_srvs
    )
    target_include_directories(test_sm_cerebellum PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
@@ -193,6 +211,7 @@ if(BUILD_TESTING)
      smacc2
      smacc2_msgs
      ament_index_cpp
+      nav2_msgs
      std_srvs
    )
    target_include_directories(test_execute_bt_action PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
@@ -216,6 +235,7 @@ if(BUILD_TESTING)
      smacc2
      smacc2_msgs
      ament_index_cpp
+      nav2_msgs
      std_srvs
    )
    target_include_directories(test_execute_bt_action_extended PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
@@ -233,7 +253,7 @@ if(BUILD_TESTING)
    if(TARGET ${test_name})
      target_compile_definitions(${test_name} PRIVATE BRAIN_DISABLE_SM=1)
      ament_target_dependencies(${test_name}
-        rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp std_srvs)
+        rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs)
      target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
      target_link_libraries(${test_name} Boost::thread yaml-cpp)
    endif()
@@ -248,7 +268,7 @@ if(BUILD_TESTING)
  if(TARGET test_cerebrum_utils)
    target_compile_definitions(test_cerebrum_utils PRIVATE BRAIN_DISABLE_SM=1)
    ament_target_dependencies(test_cerebrum_utils
-      rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp std_srvs)
+      rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs)
    target_include_directories(test_cerebrum_utils PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
    target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp)
  endif()
--- a/src/brain/config/bt_vision_grasp.xml
+++ b/src/brain/config/bt_vision_grasp.xml
@@ -0,0 +1,9 @@
+<root BTCPP_format="4">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <!-- <VisionObjectRecognition_H name="VisionObjectRecognition_H" />
+      <HandControl_H name="HandControl_H" /> -->
+      <Arm_H name="Arm_H" />
+    </Sequence>
+  </BehaviorTree>
+</root>
--- a/src/brain/config/robot_skills.yaml
+++ b/src/brain/config/robot_skills.yaml
@@ -10,6 +10,12 @@
  interfaces:
    - ArmSpaceControl.action

+- name: Arm
+  version: 1.0.0
+  description: "手臂控制"
+  interfaces:
+    - Arm.action
+
 - name: HandControl
  version: 1.0.0
  description: "手部控制"
@@ -22,6 +28,30 @@
  interfaces:
    - LegControl.action

+- name: SlamMode
+  version: 1.0.0
+  description: "切换 SLAM 建图模式"
+  interfaces:
+    - SlamMode.action
+
+- name: MapSave
+  version: 1.0.0
+  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: "视觉识别并抓取指定物体"
--- a/src/brain/include/brain/action_registry.hpp
+++ b/src/brain/include/brain/action_registry.hpp
@@ -1,5 +1,6 @@
 #pragma once

+#include <chrono>
 #include <functional>
 #include <memory>
 #include <string>
@@ -180,7 +181,7 @@ public:
    void send(rclcpp::Logger logger) override
    {
      if (!client) {return;}
-      RCLCPP_DEBUG(logger, "[ActionClientRegistry] waiting for server '%s'", name.c_str());
+      RCLCPP_INFO(logger, "[ActionClientRegistry] waiting for server '%s'", name.c_str());
      if (!client->wait_for_action_server(std::chrono::seconds(10))) {
        RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
        return;
@@ -202,7 +203,7 @@ public:
            auto gh = future_handle.get();
            if (gh) {
              last_goal_handle = gh;
-              RCLCPP_DEBUG(logger, "Stored goal handle for '%s'", name.c_str());
+              RCLCPP_INFO(logger, "Stored goal handle for '%s'", name.c_str());
            }
          } catch (const std::exception & e) {
            RCLCPP_WARN(
@@ -266,7 +267,7 @@ public:
      RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
      return;
    }
-    RCLCPP_DEBUG(logger, "Sending action '%s'", name.c_str());
+    RCLCPP_INFO(logger, "Sending action '%s'", name.c_str());
    it->second->send(logger);
  }

@@ -348,36 +349,61 @@ public:
      return std::nullopt;
    }
    auto goal = e->make_goal ? e->make_goal() : typename ActionT::Goal{};
-    auto prom =
-      std::make_shared<std::promise<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult>>();
-    auto fut = prom->get_future();
    auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
-    if (e->on_goal_response) {opts.goal_response_callback = e->on_goal_response;}
    if (e->on_feedback) {opts.feedback_callback = e->on_feedback;}
-    opts.result_callback = [p = prom, cb = e->on_result](const auto & res) mutable {
-        if (cb) {cb(res);}
-        p->set_value(res);
-      };
-    auto future_handle = e->client->async_send_goal(goal, opts);
-    std::shared_ptr<GoalHandle<ActionT>> goal_handle_ptr;
+    auto goal_future = e->client->async_send_goal(goal, opts);
+    auto goal_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
+    if (goal_future.wait_until(goal_deadline) != std::future_status::ready) {
+      RCLCPP_ERROR(logger, "Timed out waiting for goal response on '%s'", name.c_str());
+      return std::nullopt;
+    }
+
+    typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle;
    try {
-      if (future_handle.wait_for(std::chrono::seconds(5)) == std::future_status::ready) {
-        goal_handle_ptr = future_handle.get();
-        if (goal_handle_ptr) {e->last_goal_handle = goal_handle_ptr;}
-      }
+      goal_handle = goal_future.get();
    } catch (const std::exception & ex) {
-      RCLCPP_WARN(logger, "Exception getting goal handle for '%s': %s", name.c_str(), ex.what());
+      RCLCPP_ERROR(logger, "Failed to get goal handle for '%s': %s", name.c_str(), ex.what());
+      return std::nullopt;
    }
-    if (fut.wait_for(timeout) == std::future_status::ready) {
-      return fut.get();
+    if (!goal_handle) {
+      RCLCPP_WARN(logger, "Goal rejected for '%s'", name.c_str());
+      return std::nullopt;
    }
-    RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
-    if (goal_handle_ptr) {
-      try {e->client->async_cancel_goal(goal_handle_ptr);} catch (...) {}
-    } else {
-      e->cancel(logger);
+    e->last_goal_handle = goal_handle;
+    if (e->on_goal_response) {
+      try {
+        e->on_goal_response(goal_handle);
+      } catch (const std::exception & ex) {
+        RCLCPP_WARN(logger, "Goal response callback threw for '%s': %s", name.c_str(), ex.what());
+      }
    }
-    return std::nullopt;
+
+    auto result_future = e->client->async_get_result(goal_handle);
+    auto deadline = std::chrono::steady_clock::now() + timeout;
+    if (result_future.wait_until(deadline) != std::future_status::ready) {
+      RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
+      try {
+        e->client->async_cancel_goal(goal_handle);
+      } catch (const std::exception & ex) {
+        RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
+      }
+      return std::nullopt;
+    }
+    typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult wrapped_result;
+    try {
+      wrapped_result = result_future.get();
+    } catch (const std::exception & ex) {
+      RCLCPP_ERROR(logger, "Failed to get result for '%s': %s", name.c_str(), ex.what());
+      return std::nullopt;
+    }
+    if (e->on_result) {
+      try {
+        e->on_result(wrapped_result);
+      } catch (const std::exception & ex) {
+        RCLCPP_WARN(logger, "Result callback threw for '%s': %s", name.c_str(), ex.what());
+      }
+    }
+    return wrapped_result;
  }

 private:
--- a/src/brain/include/brain/bt_registry.hpp
+++ b/src/brain/include/brain/bt_registry.hpp
@@ -31,6 +31,7 @@
 #include <vector>
 #include <sstream>
 #include <unordered_map>
+#include <fstream>

 /**
 * @brief Specification for a single Behavior Tree action node used when programmatically composing XML.
@@ -209,6 +210,59 @@ public:
    return true;
  }

+  /**
+   * @brief Build a tree directly from raw XML text (already in BTCPP v4 format).
+   * @param xml Raw XML document string.
+   * @param out_tree Output tree.
+   * @return true on success, false if creation throws.
+   */
+  bool build_tree_from_xml_text(const std::string & xml, BT::Tree & out_tree)
+  {
+    try {
+      out_tree = factory_.createTreeFromText(xml);
+      return true;
+    } catch (const std::exception & e) {
+      if (node_) {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to build BT from XML text: %s", e.what());
+      }
+      return false;
+    }
+  }
+
+  /**
+   * @brief Build a tree from an XML file on disk.
+   *
+   * Reads the file contents into memory (so that subsequent modifications to the file do not
+   * affect the already built tree) and invokes build_tree_from_xml_text(). Any I/O or parsing
+   * errors are logged.
+   *
+   * @param file_path Absolute or relative path to XML file.
+   * @param out_tree Output tree.
+   * @return true if the file was read and the tree successfully constructed; false otherwise.
+   */
+  bool build_tree_from_xml_file(const std::string & file_path, BT::Tree & out_tree)
+  {
+    std::ifstream ifs(file_path);
+    if (!ifs.is_open()) {
+      if (node_) {
+        RCLCPP_ERROR(node_->get_logger(), "Could not open BT XML file: %s", file_path.c_str());
+      }
+      return false;
+    }
+    std::ostringstream buffer;
+    buffer << ifs.rdbuf();
+    if (buffer.str().empty()) {
+      if (node_) {
+        RCLCPP_ERROR(node_->get_logger(), "BT XML file empty: %s", file_path.c_str());
+      }
+      return false;
+    }
+    if (node_) {
+      RCLCPP_INFO(node_->get_logger(), "Building BT from XML file: %s \n%s", file_path.c_str(), buffer.str().c_str());
+    }
+    return build_tree_from_xml_text(buffer.str(), out_tree);
+  }
+
 private:
  /**
   * @brief Convert a linear sequence spec into BTCPP v4 XML text.
--- a/src/brain/include/brain/cerebellum_node.hpp
+++ b/src/brain/include/brain/cerebellum_node.hpp
@@ -1,6 +1,8 @@
 #ifndef CEREBELLUM_CEREBELLUM_NODE_HPP_
 #define CEREBELLUM_CEREBELLUM_NODE_HPP_

+#include <atomic>
+#include <limits>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "brain_interfaces/action/execute_bt_action.hpp"
@@ -44,6 +46,14 @@ private:
  bool abort_on_failure_ {true};
  double default_action_timeout_sec_ {60.0};
  double vision_grasp_object_timeout_sec_ {120.0};
+  std::string map_save_url_ {"/tmp/humanoid_map"};
+  std::string map_save_image_format_ {"pgm"};
+  double map_save_free_thresh_ {0.25};
+  double map_save_occupied_thresh_ {0.65};
+  std::string map_load_url_ {};
+  double nav_goal_yaw_offset_ {0.0};
+  double nav_goal_distance_tolerance_ {0.5};
+  bool slam_enable_mapping_default_ {true};

  // Optional per-skill override timeout (skill_name -> seconds)
  std::unordered_map<std::string, double> skill_timeouts_;
@@ -60,8 +70,15 @@ private:

  // Current accepted sequence epoch (from client) used to filter/reject stale goals
  std::atomic<uint32_t> current_epoch_{0};
+  std::atomic<double> nav2_last_distance_remaining_{std::numeric_limits<double>::quiet_NaN()};
+  std::atomic<bool> nav2_last_goal_succeeded_{false};

  // ---- helpers ----
+  /** @brief Install application-specific action hook overrides for skill manager. */
+  void ConfigureActionHooks();
+  /** @brief Install application-specific service hook overrides. */
+  void ConfigureServiceHooks();
+
  /**
   * @brief Parse a raw comma / semicolon / whitespace separated list of skill names.
   *
@@ -186,15 +203,19 @@ private:
    std::string & detail);

  /**
-   * @brief Execute a service-based skill (currently VisionObjectRecognition only).
+   * @brief Execute a service-based skill using SkillManager-configured hooks.
   * @param skill Skill name.
-   * @param detail [out] Detail / diagnostics.
-   * @param goal_handle Goal handle.
+  * @param spec Skill specification (interfaces used to resolve service base).
+  * @param detail [out] Detail / diagnostics.
+  * @param start_progress Sequence progress prior to executing the service.
+  * @param goal_handle Goal handle (used for feedback publishing).
   * @return true if successful.
   */
  bool ExecuteServiceSkill(
    const std::string & skill,
+    const SkillSpec & spec,
    std::string & detail,
+    float start_progress,
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
  /**
   * @brief Record per-skill success/failure counts (thread-safe via mutex).
--- a/src/brain/include/brain/cerebrum_node.hpp
+++ b/src/brain/include/brain/cerebrum_node.hpp
@@ -10,6 +10,7 @@
 #include <unordered_set>
 #include <optional>
 #include <mutex>
+#include <filesystem>
 #include "brain/action_registry.hpp"
 #include <behaviortree_cpp/bt_factory.h>
 #include "brain/bt_registry.hpp"
@@ -206,6 +207,8 @@ private:
  std::unique_ptr<BTRegistry> bt_registry_;
  std::unique_ptr<SkillManager> skill_manager_;
  std::string active_sequence_;
+  std::string bt_config_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_;
  // Service to trigger immediate rebuild of sequence + BT
@@ -267,6 +270,12 @@ private:
   * @thread_safety Guarded by atomic flag bt_updating_; not re-entrant.
   */
  void UpdateBehaviorTree();
+  /**
+   * @brief Build the behavior tree from an XML file.
+   * @param xml_file Path to XML file.
+   * @thread_safety Not thread-safe with concurrent ticking; intended for controlled rebuild phases.
+   */
+  void BuildBehaviorTreeFromFile(const std::string & xml_file);
  /**
   * @brief Tick the behavior tree root if a run is pending.
   * @thread_safety Called from a timer callback; not thread-safe against concurrent UpdateBehaviorTree (atomic gating only).
--- a/src/brain/include/brain/skill_manager.hpp
+++ b/src/brain/include/brain/skill_manager.hpp
@@ -2,10 +2,14 @@

 #include <rclcpp/rclcpp.hpp>
 #include <yaml-cpp/yaml.h>
+#include <chrono>
+#include <functional>
+#include <future>
+#include <memory>
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
-#include <memory>

 #include "brain/action_registry.hpp"
 #include "brain/bt_registry.hpp"
@@ -15,12 +19,16 @@
 #include "interfaces/action/leg_control.hpp"
 #include "interfaces/action/vision_grasp_object.hpp"
 #include "interfaces/srv/vision_object_recognition.hpp"
-
-#include "brain_interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
+#include "interfaces/action/arm.hpp"
+#include "interfaces/action/slam_mode.hpp"
+#include "nav2_msgs/action/navigate_to_pose.hpp"

 namespace brain
 {

+template<typename ActionT>
+struct SkillActionTrait;
+
 /**
 * @brief Specification of a skill as parsed from YAML.
 *
@@ -50,13 +58,20 @@ class SkillManager
 {
 public:
  /**
-   * @brief Construct with external registry references.
   * @param node ROS 2 node pointer (not owned).
   * @param action_clients Action client registry used to register skill actions.
   * @param bt Behavior tree registry reference (not used for node creation directly here).
   */
  SkillManager(rclcpp::Node * node, ActionClientRegistry * action_clients, BTRegistry * bt)
-  : node_(node), action_clients_(action_clients), bt_(bt) {}
+  : node_(node), action_clients_(action_clients), bt_(bt)
+  {
+    if (node_) {
+      constexpr auto prefix_param = "skill_topic_prefix";
+      if (!node_->has_parameter(prefix_param)) {
+        node_->declare_parameter<std::string>(prefix_param, "");
+      }
+    }
+  }

  /**
   * @brief Load skill specifications from a YAML file.
@@ -84,10 +99,147 @@ private:
  std::unordered_map<std::string, SkillSpec> skills_;
  std::vector<rclcpp::ClientBase::SharedPtr> service_clients_; // 保存 service client 生命周期
  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_;
+  std::unordered_map<std::string, std::function<bool(
+    const std::string & skill_name,
+    std::string & detail)>> service_hook_registrars_;

  void register_interfaces_(const SkillSpec & s);

 public:
+
+  template<typename ServiceT>
+  struct ServiceHookOptions
+  {
+    using ClientPtr = std::shared_ptr<rclcpp::Client<ServiceT>>;
+    using RequestPtr = std::shared_ptr<typename ServiceT::Request>;
+    using ResponsePtr = std::shared_ptr<typename ServiceT::Response>;
+
+    std::function<RequestPtr(const std::string & skill_name)> make_request;
+    std::function<std::chrono::nanoseconds(const std::string & skill_name)> wait_timeout;
+    std::function<std::chrono::nanoseconds(const std::string & skill_name)> call_timeout;
+    std::function<void(const std::string & skill_name, const ClientPtr & client)> on_before_call;
+    std::function<bool(
+      const std::string & skill_name,
+      const ClientPtr & client,
+      const ResponsePtr & response,
+      std::string & detail)> on_response;
+  };
+  template<typename ActionT>
+  struct ActionHookOptions
+  {
+    using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
+    std::function<typename ActionT::Goal(const std::string & skill_name)> make_goal;
+    std::function<void(
+      const std::string & skill_name,
+      typename GoalHandle::SharedPtr,
+      const std::shared_ptr<const typename ActionT::Feedback>)> on_feedback;
+    std::function<void(
+      const std::string & skill_name,
+      const typename GoalHandle::WrappedResult &)> on_result;
+    std::function<void(
+      const std::string & skill_name,
+      const std::shared_ptr<GoalHandle> &)> on_goal_response;
+  };
+
+  template<typename ActionT>
+  void configure_action_hooks(const std::string & base, ActionHookOptions<ActionT> options)
+  {
+    action_override_registrars_[base] =
+      [this,
+      make_goal = std::move(options.make_goal),
+      on_feedback = std::move(options.on_feedback),
+      on_result = std::move(options.on_result),
+      on_goal_response = std::move(options.on_goal_response)](
+      const std::string & topic, const std::string & internal_skill) {
+        std::function<typename ActionT::Goal()> make_goal_cb;
+        if (make_goal) {
+          make_goal_cb = [make_goal, internal_skill]() {return make_goal(internal_skill);};
+        }
+        using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
+        std::function<void(
+          typename GoalHandle::SharedPtr,
+          const std::shared_ptr<const typename ActionT::Feedback>)> feedback_cb;
+        if (on_feedback) {
+          feedback_cb = [on_feedback, internal_skill](
+              typename GoalHandle::SharedPtr handle,
+              const std::shared_ptr<const typename ActionT::Feedback> feedback) {
+              on_feedback(internal_skill, handle, feedback);
+            };
+        }
+        std::function<void(const typename GoalHandle::WrappedResult &)> result_cb;
+        if (on_result) {
+          result_cb = [on_result, internal_skill](const typename GoalHandle::WrappedResult & res) {
+              on_result(internal_skill, res);
+            };
+        }
+        std::function<void(const std::shared_ptr<GoalHandle> &)> goal_response_cb;
+        if (on_goal_response) {
+          goal_response_cb = [on_goal_response, internal_skill](
+              const std::shared_ptr<GoalHandle> & handle) {
+              on_goal_response(internal_skill, handle);
+            };
+        }
+        register_action_client_with_overrides<ActionT>(
+          topic, internal_skill, make_goal_cb, feedback_cb, result_cb, goal_response_cb);
+      };
+  }
+
+  template<typename ServiceT>
+  void configure_service_hooks(const std::string & base, ServiceHookOptions<ServiceT> options)
+  {
+    service_hook_registrars_[base] = [this, options = std::move(options)](
+        const std::string & skill_name,
+        std::string & detail) -> bool {
+        auto client = this->get_typed_service_client<ServiceT>(skill_name);
+        if (!client) {
+          detail = "Service client missing";
+          return false;
+        }
+        const auto wait_timeout = options.wait_timeout ?
+          options.wait_timeout(skill_name) : std::chrono::seconds(2);
+        if (!client->wait_for_service(wait_timeout)) {
+          detail = "Service unavailable";
+          return false;
+        }
+        if (options.on_before_call) {
+          options.on_before_call(skill_name, client);
+        }
+        auto request = options.make_request ?
+          options.make_request(skill_name) : std::make_shared<typename ServiceT::Request>();
+        const auto call_timeout = options.call_timeout ?
+          options.call_timeout(skill_name) : std::chrono::seconds(5);
+        auto future = client->async_send_request(request);
+        if (future.wait_for(call_timeout) != std::future_status::ready) {
+          detail = "Service call timeout";
+          return false;
+        }
+        auto response = future.get();
+        if (!response) {
+          detail = "Null response";
+          return false;
+        }
+        if (options.on_response) {
+          return options.on_response(skill_name, client, response, detail);
+        }
+        detail = "success";
+        return true;
+      };
+  }
+
+  bool invoke_service_hook(
+    const std::string & base,
+    const std::string & skill_name,
+    std::string & detail) const
+  {
+    auto it = service_hook_registrars_.find(base);
+    if (it == service_hook_registrars_.end()) {
+      return false;
+    }
+    return it->second(skill_name, detail);
+  }
+
  /**
   * @brief Retrieve a typed service client for a named skill if available.
   * @tparam T Service type.
@@ -101,6 +253,73 @@ public:
    if (it == service_client_map_.end()) {return nullptr;}
    return std::dynamic_pointer_cast<rclcpp::Client<T>>(it->second);
  }
+
+private:
+  template<typename ActionT>
+  void register_action_client_default(
+    const std::string & base,
+    const std::string & topic,
+    const std::string & internal_skill)
+  {
+    auto it = action_override_registrars_.find(base);
+    if (it != action_override_registrars_.end()) {
+      it->second(topic, internal_skill);
+      return;
+    }
+    using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
+    register_action_client_with_overrides<ActionT>(
+      topic,
+      internal_skill,
+      std::function<typename ActionT::Goal()>{},
+      std::function<void(typename GoalHandle::SharedPtr,
+        const std::shared_ptr<const typename ActionT::Feedback>)>{},
+      std::function<void(const typename GoalHandle::WrappedResult &)> {},
+      std::function<void(const std::shared_ptr<GoalHandle> &)> {});
+  }
+
+  template<typename ActionT>
+  void register_action_client_with_overrides(
+    const std::string & topic,
+    const std::string & internal_skill,
+    const std::function<typename ActionT::Goal()> & make_goal_override,
+    const std::function<void(typename ActionClientRegistry::GoalHandle<ActionT>::SharedPtr,
+      const std::shared_ptr<const typename ActionT::Feedback>)> & on_feedback_override,
+    const std::function<void(const typename ActionClientRegistry::GoalHandle<ActionT>::WrappedResult &)> & on_result_override,
+    const std::function<void(const std::shared_ptr<typename ActionClientRegistry::GoalHandle<ActionT>> &)> & on_goal_response_override)
+  {
+    if (!action_clients_) {return;}
+    using Goal = typename ActionT::Goal;
+    using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
+    std::function<Goal()> make_goal = make_goal_override ? make_goal_override : []() {return Goal{};};
+    auto feedback_cb = on_feedback_override;
+    auto goal_response_cb = on_goal_response_override;
+    std::function<void(const typename GoalHandle::WrappedResult &)> result_cb = on_result_override;
+    if (!result_cb) {
+      result_cb = [this, internal_skill](const typename GoalHandle::WrappedResult & res) {
+          if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
+            SkillActionTrait<ActionT>::success(*res.result)) {
+            RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
+          } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
+            RCLCPP_WARN(node_->get_logger(), "action cancelled: %s", internal_skill.c_str());
+          } else {
+            std::string detail;
+            if (res.result) {
+              detail = SkillActionTrait<ActionT>::message(*res.result);
+            } else {
+              detail = "result unavailable";
+            }
+            RCLCPP_ERROR(
+              node_->get_logger(), "action failed: %s (%s)", internal_skill.c_str(), detail.c_str());
+          }
+        };
+    }
+    action_clients_->register_client<ActionT>(
+      topic,
+      make_goal,
+      feedback_cb,
+      result_cb,
+      goal_response_cb);
+  }
 };


@@ -122,6 +341,16 @@ struct SkillActionTrait<interfaces::action::ArmSpaceControl>
  }
 };
 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;}
+  static std::string message(const interfaces::action::Arm::Result & r)
+  {
+    return "r.message";
+  }
+};
+template<>
 struct SkillActionTrait<interfaces::action::HandControl>
 {
  static constexpr const char * skill_name = "HandControl";
@@ -146,14 +375,49 @@ struct SkillActionTrait<interfaces::action::VisionGraspObject>
  }
 };

+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::VisionGraspObject,
+  interfaces::action::SlamMode,
+  nav2_msgs::action::NavigateToPose
 >;

 /**
--- a/src/brain/src/cerebellum_node.cpp
+++ b/src/brain/src/cerebellum_node.cpp
@@ -7,12 +7,17 @@

 // C / C++ standard library.
 #include <chrono>
+#include <algorithm>
 #include <cctype>
+#include <cmath>
+#include <future>
 #include <iostream>
+#include <limits>
 #include <memory>
 #include <sstream>
 #include <string>
 #include <thread>
+#include <utility>
 #include <vector>

 // Third-party / external libraries (ROS2, ament).
@@ -32,7 +37,10 @@
 #include "interfaces/action/leg_control.hpp"
 #include "interfaces/action/vision_grasp_object.hpp"
 #include "brain_interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
+#include "interfaces/action/slam_mode.hpp"
 #include "interfaces/srv/vision_object_recognition.hpp"
+#include "interfaces/srv/map_save.hpp"
+#include "interfaces/srv/map_load.hpp"
 #include <std_msgs/msg/string.hpp>

 namespace brain
@@ -56,6 +64,8 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
  skill_manager_ = std::make_unique<SkillManager>(this, action_clients_.get(), nullptr);

  DeclareAndLoadParameters();
+  ConfigureActionHooks();
+  ConfigureServiceHooks();
  LoadSkillsFile();
  SetupStatsTimerAndPublisher();
  SetupExecuteBtServer();
@@ -66,11 +76,525 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
    RCLCPP_INFO(this->get_logger(), "SMACC2 SmCerebellum started");
  }
 #else
-  RCLCPP_DEBUG(
+  RCLCPP_INFO(
    this->get_logger(), "SMACC2 disabled via BRAIN_DISABLE_SM for lightweight test context");
 #endif
 }

+void CerebellumNode::ConfigureActionHooks()
+{
+  if (!skill_manager_) {
+    RCLCPP_WARN(this->get_logger(), "SkillManager 不可用，跳过动作钩子配置");
+    return;
+  }
+
+  auto log_goal_response = [this](const std::string & skill_name, bool accepted, const std::string & action_name) {
+      if (accepted) {
+        RCLCPP_INFO(this->get_logger(), "[%s] %s goal 已被服务器接收", skill_name.c_str(), action_name.c_str());
+      } else {
+        RCLCPP_WARN(this->get_logger(), "[%s] %s goal 被服务器拒绝", skill_name.c_str(), action_name.c_str());
+      }
+    };
+
+  // ArmSpaceControl
+  SkillManager::ActionHookOptions<interfaces::action::ArmSpaceControl> arm_sc_hooks;
+  arm_sc_hooks.make_goal = [this](const std::string & skill_name) {
+      interfaces::action::ArmSpaceControl::Goal goal{};
+      const std::string param = "arm_space_control.reference_frame";
+      if (!this->has_parameter(param)) {
+        this->declare_parameter<std::string>(param, std::string("base_link"));
+      }
+      const auto frame = this->get_parameter(param).as_string();
+      RCLCPP_INFO(this->get_logger(), "[%s] ArmSpaceControl 使用参考坐标系 %s",
+        skill_name.c_str(), frame.c_str());
+      (void)frame;
+      //TODO : 
+      goal.target.pose.position.x = 0.5;
+      goal.target.pose.position.y = 0.6;
+      goal.target.pose.position.z = 0.7;
+      return goal;
+    };
+  arm_sc_hooks.on_feedback = [this](
+      const std::string & skill_name,
+      ActionClientRegistry::GoalHandle<interfaces::action::ArmSpaceControl>::SharedPtr,
+      const std::shared_ptr<const interfaces::action::ArmSpaceControl::Feedback> & feedback) {
+      if (!feedback) {
+        RCLCPP_WARN(this->get_logger(), "[%s] ArmSpaceControl feedback 为空", skill_name.c_str());
+        return;
+      }
+      (void)feedback;
+      //TODO :
+      RCLCPP_INFO(this->get_logger(), "[%s] Current progress: %.2f", skill_name.c_str(), feedback->progress);
+
+    };
+  arm_sc_hooks.on_result = [this](
+      const std::string & skill_name,
+      const ActionClientRegistry::GoalHandle<interfaces::action::ArmSpaceControl>::WrappedResult & res) {
+      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
+      const std::string message = res.result ? res.result->message : std::string("无返回信息");
+      if (success) {
+        RCLCPP_INFO(this->get_logger(), "[%s] ArmSpaceControl 完成: %s", skill_name.c_str(), message.c_str());
+      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
+        RCLCPP_WARN(this->get_logger(), "[%s] ArmSpaceControl 被取消", skill_name.c_str());
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[%s] ArmSpaceControl 失败 (code=%d): %s",
+          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
+      }
+    };
+  arm_sc_hooks.on_goal_response = [log_goal_response](
+      const std::string & skill_name,
+      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::ArmSpaceControl>> & handle) {
+      log_goal_response(skill_name, static_cast<bool>(handle), "ArmSpaceControl");
+    };
+  skill_manager_->configure_action_hooks<interfaces::action::ArmSpaceControl>("ArmSpaceControl", std::move(arm_sc_hooks));
+
+  // Arm
+  SkillManager::ActionHookOptions<interfaces::action::Arm> arm_hooks;
+  arm_hooks.make_goal = [this](const std::string & skill_name) {
+      interfaces::action::Arm::Goal goal{};
+      const std::string param = "arm.reference_frame";
+      if (!this->has_parameter(param)) {
+        this->declare_parameter<std::string>(param, std::string("base_link"));
+      }
+      const auto frame = this->get_parameter(param).as_string();
+      RCLCPP_INFO(this->get_logger(), "[%s] Arm 使用参考坐标系 %s",
+        skill_name.c_str(), frame.c_str());
+      (void)frame;
+
+      static int64_t command_id = 0;
+      //TODO : 
+      goal.body_id = 1;
+      goal.data_type = 1;
+      goal.data_length = 3;
+      goal.command_id = command_id++;
+
+      rclcpp::Time now = this->get_clock()->now();
+      goal.frame_time_stamp = now.nanoseconds();
+
+      goal.data_array = {0.1, 0.2, 0.3};
+
+      RCLCPP_INFO(this->get_logger(), "[%s] Arm 目标参数 body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.1f, %.1f, %.1f]",
+        skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
+        goal.data_array[0], goal.data_array[1], goal.data_array[2]);
+
+      return goal;
+    };
+  arm_hooks.on_feedback = [this](
+      const std::string & skill_name,
+      ActionClientRegistry::GoalHandle<interfaces::action::Arm>::SharedPtr,
+      const std::shared_ptr<const interfaces::action::Arm::Feedback> & feedback) {
+      if (!feedback) {
+        RCLCPP_WARN(this->get_logger(), "[%s] Arm feedback 为空", skill_name.c_str());
+        return;
+      }
+      (void)feedback;
+      //TODO :
+      RCLCPP_INFO(this->get_logger(), "[%s] command_id: %d, current progress: %d", 
+        skill_name.c_str(), feedback->command_id, feedback->int_lenth);
+
+    };
+  arm_hooks.on_result = [this](
+      const std::string & skill_name,
+      const ActionClientRegistry::GoalHandle<interfaces::action::Arm>::WrappedResult & res) {
+      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result;
+      const std::string message = res.result ? std::string("action end") : std::string("无返回信息");
+      if (success) {
+        RCLCPP_INFO(this->get_logger(), "[%s] Arm 完成: %s", skill_name.c_str(), message.c_str());
+      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
+        RCLCPP_WARN(this->get_logger(), "[%s] Arm 被取消", skill_name.c_str());
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[%s] Arm 失败 (code=%d): %s",
+          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
+      }
+    };
+  arm_hooks.on_goal_response = [log_goal_response](
+      const std::string & skill_name,
+      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::Arm>> & handle) {
+      log_goal_response(skill_name, static_cast<bool>(handle), "Arm");
+    };
+  skill_manager_->configure_action_hooks<interfaces::action::Arm>("Arm", std::move(arm_hooks));
+
+  // HandControl
+  SkillManager::ActionHookOptions<interfaces::action::HandControl> hand_hooks;
+  hand_hooks.make_goal = [this](const std::string & skill_name) {
+      interfaces::action::HandControl::Goal goal{};
+      const std::string param = "hand_control.target_pose";
+      if (!this->has_parameter(param)) {
+        this->declare_parameter<std::vector<double>>(param, {0.0, 0.0, 0.0});
+      }
+      const auto values = this->get_parameter(param).as_double_array();
+      RCLCPP_INFO(this->get_logger(), "[%s] HandControl 目标参数 size=%zu", skill_name.c_str(), values.size());
+      (void)values;
+      goal.mode = 1;
+      return goal;
+    };
+  hand_hooks.on_feedback = [this](
+      const std::string & skill_name,
+      ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::SharedPtr,
+      const std::shared_ptr<const interfaces::action::HandControl::Feedback> & feedback) {
+      if (!feedback) {return;}
+      (void)feedback;
+      //TODO :
+      RCLCPP_INFO(this->get_logger(), "[%s] Current progress: %.2f", skill_name.c_str(), feedback->progress);
+    };
+  hand_hooks.on_result = [this](
+      const std::string & skill_name,
+      const ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::WrappedResult & res) {
+      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
+      const std::string message = res.result ? res.result->message : std::string("无返回信息");
+      if (success) {
+        RCLCPP_INFO(this->get_logger(), "[%s] HandControl 成功: %s", skill_name.c_str(), message.c_str());
+      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
+        RCLCPP_WARN(this->get_logger(), "[%s] HandControl 被取消", skill_name.c_str());
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[%s] HandControl 失败 (code=%d): %s",
+          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
+      }
+    };
+  hand_hooks.on_goal_response = [log_goal_response](
+      const std::string & skill_name,
+      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::HandControl>> & handle) {
+      log_goal_response(skill_name, static_cast<bool>(handle), "HandControl");
+    };
+  skill_manager_->configure_action_hooks<interfaces::action::HandControl>("HandControl", std::move(hand_hooks));
+
+  // LegControl
+  SkillManager::ActionHookOptions<interfaces::action::LegControl> leg_hooks;
+  leg_hooks.make_goal = [this](const std::string & skill_name) {
+      interfaces::action::LegControl::Goal goal{};
+      const std::string param = "leg_control.speed_scale";
+      if (!this->has_parameter(param)) {
+        this->declare_parameter<double>(param, 1.0);
+      }
+      const auto scale = this->get_parameter(param).as_double();
+      RCLCPP_INFO(this->get_logger(), "[%s] LegControl speed_scale=%.2f", skill_name.c_str(), scale);
+      (void)scale;
+      return goal;
+    };
+  leg_hooks.on_feedback = [this](
+      const std::string & skill_name,
+      ActionClientRegistry::GoalHandle<interfaces::action::LegControl>::SharedPtr,
+      const std::shared_ptr<const interfaces::action::LegControl::Feedback> & feedback) {
+      if (!feedback) {return;}
+      RCLCPP_INFO(this->get_logger(), "[%s] LegControl feedback 接收", skill_name.c_str());
+      (void)feedback;
+    };
+  leg_hooks.on_result = [this](
+      const std::string & skill_name,
+      const ActionClientRegistry::GoalHandle<interfaces::action::LegControl>::WrappedResult & res) {
+      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
+      const std::string message = res.result ? res.result->message : std::string("无返回信息");
+      if (success) {
+        RCLCPP_INFO(this->get_logger(), "[%s] LegControl 成功: %s", skill_name.c_str(), message.c_str());
+      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
+        RCLCPP_WARN(this->get_logger(), "[%s] LegControl 被取消", skill_name.c_str());
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[%s] LegControl 失败 (code=%d): %s",
+          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
+      }
+    };
+  leg_hooks.on_goal_response = [log_goal_response](
+      const std::string & skill_name,
+      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::LegControl>> & handle) {
+      log_goal_response(skill_name, static_cast<bool>(handle), "LegControl");
+    };
+  skill_manager_->configure_action_hooks<interfaces::action::LegControl>("LegControl", std::move(leg_hooks));
+
+  // VisionGraspObject
+  SkillManager::ActionHookOptions<interfaces::action::VisionGraspObject> vision_hooks;
+  vision_hooks.make_goal = [this](const std::string & skill_name) {
+      interfaces::action::VisionGraspObject::Goal goal{};
+      const std::string target_param = "vision_grasp_object.target";
+      if (!this->has_parameter(target_param)) {
+        this->declare_parameter<std::string>(target_param, std::string("cup"));
+      }
+      const auto target = this->get_parameter(target_param).as_string();
+      RCLCPP_INFO(this->get_logger(), "[%s] VisionGraspObject target=%s",
+        skill_name.c_str(), target.c_str());
+      (void)target;
+      return goal;
+    };
+  vision_hooks.on_feedback = [this](
+      const std::string & skill_name,
+      ActionClientRegistry::GoalHandle<interfaces::action::VisionGraspObject>::SharedPtr,
+      const std::shared_ptr<const interfaces::action::VisionGraspObject::Feedback> & feedback) {
+      if (!feedback) {return;}
+      RCLCPP_INFO(this->get_logger(), "[%s] VisionGraspObject feedback 接收", skill_name.c_str());
+      (void)feedback;
+    };
+  vision_hooks.on_result = [this](
+      const std::string & skill_name,
+      const ActionClientRegistry::GoalHandle<interfaces::action::VisionGraspObject>::WrappedResult & res) {
+      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
+      const std::string message = res.result ? res.result->message : std::string("无返回信息");
+      if (success) {
+        RCLCPP_INFO(this->get_logger(), "[%s] VisionGraspObject 成功: %s", skill_name.c_str(), message.c_str());
+      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
+        RCLCPP_WARN(this->get_logger(), "[%s] VisionGraspObject 被取消", skill_name.c_str());
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[%s] VisionGraspObject 失败 (code=%d): %s",
+          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
+      }
+    };
+  vision_hooks.on_goal_response = [log_goal_response](
+      const std::string & skill_name,
+      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::VisionGraspObject>> & handle) {
+      log_goal_response(skill_name, static_cast<bool>(handle), "VisionGraspObject");
+    };
+  skill_manager_->configure_action_hooks<interfaces::action::VisionGraspObject>("VisionGraspObject", std::move(vision_hooks));
+
+  // SlamMode (mapping toggle)
+  SkillManager::ActionHookOptions<interfaces::action::SlamMode> slam_hooks;
+  slam_hooks.make_goal = [this](const std::string & skill_name) {
+      interfaces::action::SlamMode::Goal goal{};
+      const std::string param_name = "slam_mode.enable_mapping";
+      if (!this->has_parameter(param_name)) {
+        this->declare_parameter<bool>(param_name, slam_enable_mapping_default_);
+      }
+      goal.enable_mapping = this->get_parameter(param_name).as_bool();
+      RCLCPP_INFO(this->get_logger(), "[%s] SlamMode enable_mapping=%s",
+        skill_name.c_str(), goal.enable_mapping ? "true" : "false");
+      return goal;
+    };
+  slam_hooks.on_feedback = [this](
+      const std::string & skill_name,
+      ActionClientRegistry::GoalHandle<interfaces::action::SlamMode>::SharedPtr,
+      const std::shared_ptr<const interfaces::action::SlamMode::Feedback> & feedback) {
+      if (!feedback) {return;}
+      RCLCPP_INFO(this->get_logger(), "[%s] SlamMode feedback 接收", skill_name.c_str());
+    };
+  slam_hooks.on_result = [this](
+      const std::string & skill_name,
+      const ActionClientRegistry::GoalHandle<interfaces::action::SlamMode>::WrappedResult & res) {
+      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
+      const std::string message = res.result ? res.result->message : std::string("无返回信息");
+      if (success) {
+        RCLCPP_INFO(this->get_logger(), "[%s] SlamMode 成功: %s", skill_name.c_str(), message.c_str());
+      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
+        RCLCPP_WARN(this->get_logger(), "[%s] SlamMode 被取消", skill_name.c_str());
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[%s] SlamMode 失败 (code=%d): %s",
+          skill_name.c_str(), static_cast<int>(res.code), message.c_str());
+      }
+    };
+  slam_hooks.on_goal_response = [log_goal_response](
+      const std::string & skill_name,
+      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::SlamMode>> & handle) {
+      log_goal_response(skill_name, static_cast<bool>(handle), "SlamMode");
+    };
+  skill_manager_->configure_action_hooks<interfaces::action::SlamMode>("SlamMode", std::move(slam_hooks));
+
+  // NavigateToPose
+  SkillManager::ActionHookOptions<nav2_msgs::action::NavigateToPose> nav_hooks;
+  nav_hooks.make_goal = [this](const std::string & skill_name) {
+      nav2_msgs::action::NavigateToPose::Goal goal{};
+      const std::string pose_param = "navigate_to_pose.default_goal";
+      const std::string frame_param = "navigate_to_pose.goal_frame";
+      if (!this->has_parameter(pose_param)) {
+        this->declare_parameter<std::vector<double>>(pose_param, {0.0, 0.0, 0.0});
+      }
+      if (!this->has_parameter(frame_param)) {
+        this->declare_parameter<std::string>(frame_param, "map");
+      }
+      std::vector<double> coords;
+      try {
+        coords = this->get_parameter(pose_param).as_double_array();
+      } catch (const rclcpp::ParameterTypeException &)
+      {
+        coords = {0.0, 0.0, 0.0};
+        RCLCPP_WARN(this->get_logger(), "[%s] 参数 %s 应为 double 数组，使用默认 (0,0,0)",
+          skill_name.c_str(), pose_param.c_str());
+      }
+      const auto stamp = this->get_clock()->now();
+      goal.pose.header.stamp = stamp;
+      goal.pose.header.frame_id = this->get_parameter(frame_param).as_string();
+      if (coords.size() >= 2) {
+        goal.pose.pose.position.x = coords[0];
+        goal.pose.pose.position.y = coords[1];
+      }
+      double yaw = (coords.size() >= 3) ? coords[2] : 0.0;
+      yaw += nav_goal_yaw_offset_;
+      const double half = yaw * 0.5;
+      goal.pose.pose.orientation.w = std::cos(half);
+      goal.pose.pose.orientation.x = 0.0;
+      goal.pose.pose.orientation.y = 0.0;
+      goal.pose.pose.orientation.z = std::sin(half);
+      nav2_last_distance_remaining_.store(std::numeric_limits<double>::quiet_NaN(), std::memory_order_relaxed);
+      nav2_last_goal_succeeded_.store(false, std::memory_order_relaxed);
+      RCLCPP_INFO(this->get_logger(), "[%s] 导航目标 (%.2f, %.2f) yaw=%.2f (offset=%.2f)",
+        skill_name.c_str(), goal.pose.pose.position.x, goal.pose.pose.position.y, yaw, nav_goal_yaw_offset_);
+      return goal;
+    };
+  nav_hooks.on_feedback = [this](
+      const std::string & skill_name,
+      ActionClientRegistry::GoalHandle<nav2_msgs::action::NavigateToPose>::SharedPtr,
+      const std::shared_ptr<const nav2_msgs::action::NavigateToPose::Feedback> & feedback) {
+      if (!feedback) {return;}
+      nav2_last_distance_remaining_.store(feedback->distance_remaining, std::memory_order_relaxed);
+      RCLCPP_INFO(this->get_logger(), "[%s] NavigateToPose feedback distance_remaining=%.2f",
+        skill_name.c_str(), feedback->distance_remaining);
+    };
+  nav_hooks.on_result = [this](
+      const std::string & skill_name,
+      const ActionClientRegistry::GoalHandle<nav2_msgs::action::NavigateToPose>::WrappedResult & res) {
+      const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED;
+      nav2_last_goal_succeeded_.store(success, std::memory_order_relaxed);
+      const double remaining = nav2_last_distance_remaining_.load(std::memory_order_relaxed);
+      if (success) {
+        if (std::isnan(remaining) || remaining <= nav_goal_distance_tolerance_) {
+          RCLCPP_INFO(this->get_logger(), "[%s] 导航完成，剩余距离=%.2f",
+            skill_name.c_str(), std::isnan(remaining) ? 0.0 : remaining);
+        } else {
+          RCLCPP_WARN(this->get_logger(), "[%s] 导航成功但剩余距离 %.2f 超过阈值 %.2f",
+            skill_name.c_str(), remaining, nav_goal_distance_tolerance_);
+        }
+      } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
+        RCLCPP_WARN(this->get_logger(), "[%s] 导航被取消", skill_name.c_str());
+      } else if (res.code == rclcpp_action::ResultCode::ABORTED) {
+        RCLCPP_ERROR(this->get_logger(), "[%s] 导航失败 (ABORT)", skill_name.c_str());
+      } else {
+        RCLCPP_ERROR(this->get_logger(), "[%s] 导航失败 result_code=%d",
+          skill_name.c_str(), static_cast<int>(res.code));
+      }
+    };
+  nav_hooks.on_goal_response = [log_goal_response](
+      const std::string & skill_name,
+      const std::shared_ptr<ActionClientRegistry::GoalHandle<nav2_msgs::action::NavigateToPose>> & handle) {
+      log_goal_response(skill_name, static_cast<bool>(handle), "NavigateToPose");
+    };
+  skill_manager_->configure_action_hooks<nav2_msgs::action::NavigateToPose>("NavigateToPose", std::move(nav_hooks));
+}
+
+void CerebellumNode::ConfigureServiceHooks()
+{
+  if (!skill_manager_) {
+    RCLCPP_WARN(this->get_logger(), "SkillManager 不可用，跳过服务钩子配置");
+    return;
+  }
+
+  using VisionSrv = interfaces::srv::VisionObjectRecognition;
+  {
+    SkillManager::ServiceHookOptions<VisionSrv> vision_hooks;
+    vision_hooks.wait_timeout = [](const std::string &) {
+        return std::chrono::seconds(2);
+      };
+    vision_hooks.call_timeout = [](const std::string &) {
+        return std::chrono::seconds(5);
+      };
+    vision_hooks.make_request = [this](const std::string & skill_name) {
+        (void)skill_name;
+        auto req = std::make_shared<VisionSrv::Request>();
+        const std::string param = "vision_object_recognition.camera_position";
+        if (!this->has_parameter(param)) {
+          this->declare_parameter<std::string>(param, std::string("left"));
+        }
+        req->camera_position = this->get_parameter(param).as_string();
+        return req;
+      };
+    vision_hooks.on_response = [this](
+        const std::string & skill_name,
+        const SkillManager::ServiceHookOptions<VisionSrv>::ClientPtr &,
+        const SkillManager::ServiceHookOptions<VisionSrv>::ResponsePtr & resp,
+        std::string & detail) {
+        if (!resp->success) {
+          detail = resp->info.empty() ? std::string("Not success") : resp->info;
+          RCLCPP_WARN(this->get_logger(), "[%s] VisionObjectRecognition failed: %s",
+            skill_name.c_str(), detail.c_str());
+          return false;
+        }
+        detail = resp->info.empty() ? std::string("success pose") : resp->info;
+        const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
+        RCLCPP_INFO(
+          this->get_logger(),
+          "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
+          skill_name.c_str(), resp->success, resp->objects.size(), resp->header.frame_id.c_str(),
+          stamp_sec, resp->info.c_str());
+        for (size_t i = 0; i < resp->objects.size(); ++i) {
+          const auto & obj = resp->objects[i];
+          RCLCPP_INFO(
+            this->get_logger(), "  [%zu] class='%s' id=%d poses=%zu", i,
+            obj.class_name.c_str(), obj.class_id, obj.pose_list.size());
+          for (size_t j = 0; j < obj.pose_list.size() && j < 2; ++j) {
+            const auto & p = obj.pose_list[j];
+            RCLCPP_INFO(
+              this->get_logger(),
+              "    pose[%zu]: pos(%.3f, %.3f, %.3f) quat(%.3f, %.3f, %.3f, %.3f)", j,
+              p.position.x, p.position.y, p.position.z,
+              p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
+          }
+          if (obj.pose_list.size() > 2) {
+            RCLCPP_INFO(
+              this->get_logger(), "    ... (%zu more poses omitted)", obj.pose_list.size() - 2);
+          }
+        }
+        return true;
+      };
+    skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(vision_hooks));
+  }
+
+  using MapSaveSrv = interfaces::srv::MapSave;
+  {
+    SkillManager::ServiceHookOptions<MapSaveSrv> save_hooks;
+    save_hooks.wait_timeout = [](const std::string &) {
+        return std::chrono::seconds(2);
+      };
+    save_hooks.call_timeout = [](const std::string &) {
+        return std::chrono::seconds(10);
+      };
+    save_hooks.make_request = [this](const std::string &) {
+        auto req = std::make_shared<MapSaveSrv::Request>();
+        req->map_url = map_save_url_;
+        return req;
+      };
+    save_hooks.on_response = [this](
+        const std::string & skill_name,
+        const SkillManager::ServiceHookOptions<MapSaveSrv>::ClientPtr &,
+        const SkillManager::ServiceHookOptions<MapSaveSrv>::ResponsePtr & resp,
+        std::string & detail) {
+        const bool ok = resp && resp->success;
+        detail = resp ? resp->message : std::string("Null response");
+        if (ok) {
+          RCLCPP_INFO(this->get_logger(), "[%s] MapSave ok: %s", skill_name.c_str(), detail.c_str());
+        } else {
+          RCLCPP_WARN(this->get_logger(), "[%s] MapSave failed: %s", skill_name.c_str(), detail.c_str());
+        }
+        return ok;
+      };
+    skill_manager_->configure_service_hooks<MapSaveSrv>("MapSave", std::move(save_hooks));
+  }
+
+  using MapLoadSrv = interfaces::srv::MapLoad;
+  {
+    SkillManager::ServiceHookOptions<MapLoadSrv> load_hooks;
+    load_hooks.wait_timeout = [](const std::string &) {
+        return std::chrono::seconds(2);
+      };
+    load_hooks.call_timeout = [](const std::string &) {
+        return std::chrono::seconds(10);
+      };
+    load_hooks.make_request = [this](const std::string &) {
+        auto req = std::make_shared<MapLoadSrv::Request>();
+        req->map_url = map_load_url_;
+        return req;
+      };
+    load_hooks.on_response = [this](
+        const std::string & skill_name,
+        const SkillManager::ServiceHookOptions<MapLoadSrv>::ClientPtr &,
+        const SkillManager::ServiceHookOptions<MapLoadSrv>::ResponsePtr & resp,
+        std::string & detail) {
+        const bool ok = resp && resp->success;
+        detail = resp ? resp->message : std::string("Null response");
+        if (ok) {
+          RCLCPP_INFO(this->get_logger(), "[%s] MapLoad ok: %s", skill_name.c_str(), detail.c_str());
+        } else {
+          RCLCPP_WARN(this->get_logger(), "[%s] MapLoad failed: %s", skill_name.c_str(), detail.c_str());
+        }
+        return ok;
+      };
+    skill_manager_->configure_service_hooks<MapLoadSrv>("MapLoad", std::move(load_hooks));
+  }
+}
+
 /**
 * @brief Declare all configurable ROS parameters and load their initial values.
 *
@@ -89,11 +613,31 @@ void CerebellumNode::DeclareAndLoadParameters()
    vision_grasp_object_timeout_sec_);
  this->declare_parameter<std::string>("skill_timeouts", ""); // format: SkillA=30,SkillB=10
  this->declare_parameter<double>("stats_log_interval_sec", stats_log_interval_sec_);
+  // this->declare_parameter<std::string>("skill_topic_prefix", "humanoid");
+  this->declare_parameter<std::string>("map_save_url", map_save_url_);
+  this->declare_parameter<std::string>("map_save_image_format", map_save_image_format_);
+  this->declare_parameter<double>("map_save_free_thresh", map_save_free_thresh_);
+  this->declare_parameter<double>("map_save_occupied_thresh", map_save_occupied_thresh_);
+  this->declare_parameter<std::string>("map_load_url", map_load_url_);
+  this->declare_parameter<double>("navigate_to_pose.yaw_offset", nav_goal_yaw_offset_);
+  this->declare_parameter<double>("navigate_to_pose.distance_tolerance", nav_goal_distance_tolerance_);
+  this->declare_parameter<bool>("slam_mode.enable_mapping_default", slam_enable_mapping_default_);
  // Read back
  this->get_parameter("abort_on_failure", abort_on_failure_);
  this->get_parameter("default_action_timeout_sec", default_action_timeout_sec_);
  this->get_parameter("vision_grasp_object_timeout_sec", vision_grasp_object_timeout_sec_);
  this->get_parameter("stats_log_interval_sec", stats_log_interval_sec_);
+  this->get_parameter("map_save_url", map_save_url_);
+  this->get_parameter("map_save_image_format", map_save_image_format_);
+  this->get_parameter("map_save_free_thresh", map_save_free_thresh_);
+  this->get_parameter("map_save_occupied_thresh", map_save_occupied_thresh_);
+  this->get_parameter("map_load_url", map_load_url_);
+  this->get_parameter("navigate_to_pose.yaw_offset", nav_goal_yaw_offset_);
+  this->get_parameter("navigate_to_pose.distance_tolerance", nav_goal_distance_tolerance_);
+  this->get_parameter("slam_mode.enable_mapping_default", slam_enable_mapping_default_);
+  map_save_free_thresh_ = std::clamp(map_save_free_thresh_, 0.0, 1.0);
+  map_save_occupied_thresh_ = std::clamp(map_save_occupied_thresh_, 0.0, 1.0);
+  nav_goal_distance_tolerance_ = std::max(0.0, nav_goal_distance_tolerance_);

  // Parse skill_timeouts param into map
  std::string raw;
@@ -365,7 +909,10 @@ bool CerebellumNode::ExecuteSingleSkill(
      skill, chosen_topic, spec, detail, index, total_skills, goal_handle);
  }
  if (kind == "service") {
-    return ExecuteServiceSkill(skill, detail, goal_handle);
+    const float base_progress =
+      (total_skills > 0) ? (static_cast<float>(index) / static_cast<float>(total_skills)) : 0.f;
+    PublishFeedbackStage(goal_handle, "START", skill, base_progress, "");
+    return ExecuteServiceSkill(skill, spec, detail, base_progress, goal_handle);
  }
  detail = "Unknown interface kind";
  PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
@@ -452,6 +999,7 @@ bool CerebellumNode::ExecuteActionSkill(
  (void)spec;  // Currently unused beyond interface kind selection.
  if (!action_clients_->has_client(topic)) {
    detail = "Client not registered";
+    RCLCPP_ERROR(this->get_logger(), "[%s] %s, TOPIC: %s", skill.c_str(), detail.c_str(), topic.c_str());
    return false;
  }
  const float base_progress =
@@ -523,90 +1071,45 @@ bool CerebellumNode::RunActionSkillWithProgress(
 * service names or failures emit END with FAIL classification.
 * @param skill Skill name.
 * @param detail (out) Status / diagnostic text.
- * @param goal_handle ExecuteBtAction goal handle.
+ * @param start_progress Sequence progress prior to invoking the service.
+ * @param goal_handle ExecuteBtAction goal handle used for feedback.
 * @return true if service call succeeds with positive result; false otherwise.
 */
 bool CerebellumNode::ExecuteServiceSkill(
  const std::string & skill,
+  const SkillSpec & spec,
  std::string & detail,
+  float start_progress,
  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
 {
-  if (skill == "VisionObjectRecognition") {
-    using Srv = interfaces::srv::VisionObjectRecognition;
-    auto client = skill_manager_->get_typed_service_client<Srv>(skill);
-    if (!client) {
-      detail = "Service client missing";
-      PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
-      return false;
+  (void)goal_handle;
+  (void)start_progress;
+  auto to_lower_str = [](std::string s) {
+      std::transform(s.begin(), s.end(), s.begin(), [](unsigned char c) {
+        return static_cast<char>(std::tolower(c));
+      });
+      return s;
+    };
+  std::string service_base;
+  for (const auto & iface : spec.interfaces) {
+    const auto pos = iface.rfind('.');
+    if (pos == std::string::npos || pos + 1 >= iface.size()) {continue;}
+    if (to_lower_str(iface.substr(pos + 1)) == "service") {
+      service_base = iface.substr(0, pos);
+      break;
    }
-    if (!client->wait_for_service(std::chrono::seconds(2))) {
-      detail = "Service unavailable";
-      PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
-      return false;
-    }
-    PublishFeedbackStage(goal_handle, "START", skill, 0.f, "");
-    auto req = std::make_shared<Srv::Request>();
-    req->camera_position = "left";
-    auto future = client->async_send_request(req);
-    // NOTE:
-    // Do NOT call rclcpp::spin_until_future_complete(this->get_node_base_interface(), ...)
-    // Doing so would attempt to add this node to a temporary executor a second time
-    // and trigger the runtime error: "Node has already been added".
-    // The action execute callback already runs in its own thread; the main executor
-    // is spinning elsewhere and will process the service response.
-    if (future.wait_for(std::chrono::seconds(5)) != std::future_status::ready) {
-      detail = "Service call timeout";
-      PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
-      return false;
-    }
-    auto resp = future.get();
-    if (!resp) {
-      detail = "Null response";
-      PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
-      return false;
-    }
-    if (!resp->success) {
-      detail = "Not success";
-      PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
-      return false;
-    }
-    detail = "success pose";
-    PublishFeedbackStage(goal_handle, "END", skill, 0.f, "OK");
-
-    // Print response (VisionObjectRecognition.srv)
-    if (resp) {
-      const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
-      RCLCPP_INFO(
-        this->get_logger(),
-        "[VisionObjectRecognition] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
-        resp->success, resp->objects.size(), resp->header.frame_id.c_str(), stamp_sec,
-        resp->info.c_str());
-      for (size_t i = 0; i < resp->objects.size(); ++i) {
-        const auto & obj = resp->objects[i];
-        RCLCPP_INFO(
-          this->get_logger(), "  [%zu] class='%s' id=%d poses=%zu", i,
-          obj.class_name.c_str(), obj.class_id, obj.pose_list.size());
-        // Print up to first 2 poses per class to avoid log spam
-        for (size_t j = 0; j < obj.pose_list.size() && j < 2; ++j) {
-          const auto & p = obj.pose_list[j];
-            RCLCPP_INFO(
-              this->get_logger(),
-              "    pose[%zu]: pos(%.3f, %.3f, %.3f) quat(%.3f, %.3f, %.3f, %.3f)", j,
-              p.position.x, p.position.y, p.position.z,
-              p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
-        }
-        if (obj.pose_list.size() > 2) {
-          RCLCPP_INFO(
-            this->get_logger(), "    ... (%zu more poses omitted)", obj.pose_list.size() - 2);
-        }
-      }
-    }
-
-    return true;
  }
-  detail = "Unsupported service skill";
-  PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
-  return false;
+
+  if (service_base.empty()) {
+    detail = "No service interface declared";
+    return false;
+  }
+
+  bool ok = skill_manager_->invoke_service_hook(service_base, skill, detail);
+  if (!ok && detail.empty()) {
+    detail = "Service hook execution failed";
+  }
+  return ok;
 }

 /**
@@ -848,6 +1351,7 @@ std::string CerebellumNode::ResolveTopicForSkill(const std::string & skill) cons
  const std::string snake = ToSnake(skill);
  if (action_clients_->has_client(snake)) {return snake;}
  if (action_clients_->has_client(skill)) {return skill;}
+  if (action_clients_->has_client(skill + "Action")) {return skill + "Action";}
  return snake;  // fallback
 }

--- a/src/brain/src/cerebrum_node.cpp
+++ b/src/brain/src/cerebrum_node.cpp
@@ -81,7 +81,8 @@ struct UpdatingFlagGuard
 * @thread_safety Construction is single-threaded; no concurrent access occurs.
 */
 CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
-: rclcpp::Node("cerebrum_node", 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"))
 {
  InitializeRegistries();
  DeclareParameters();
@@ -269,10 +270,15 @@ void CerebrumNode::CerebrumTask()
  if (bt_pending_run_ && !allow_bt_rebuild_during_execution_) {
    return;
  }
-  // Use correctly cased API methods
-  RunVlmModel();
-  CancelActiveExecuteBtGoal();
-  UpdateBehaviorTree();
+
+  if (!bt_config_file_path_.empty()) {
+    BuildBehaviorTreeFromFile(bt_config_file_path_);
+  } else {
+    // Use correctly cased API methods
+    RunVlmModel();
+    CancelActiveExecuteBtGoal();
+    UpdateBehaviorTree();
+  }
 }

 /**
@@ -298,6 +304,45 @@ void CerebrumNode::ExecuteBehaviorTree()
  }
 }

+/**
+ * @brief Build a behavior tree from an XML file, advancing the epoch.
+ *
+ * Resets per-node execution tracking and marks bt_pending_run_ to start ticking. Uses an
+ * atomic flag and RAII guard to prevent re-entrancy.
+ * @param xml_file Path to XML file.
+ * @thread_safety Not re-entrant (atomic gate); safe against concurrent timer callbacks.
+ */
+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());
+    return;
+  }
+  try {
+    tree_.haltTree();
+  } catch (...) {
+    // Swallow halt exceptions.
+    RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
+  }
+  tree_ = std::move(new_tree);
+  uint64_t new_epoch = epoch_filter_.epoch() + 1;
+  std::vector<std::string> seq_skills;
+  {
+    std::lock_guard<std::mutex> lk(exec_mutex_);
+    seq_skills = current_sequence_skills_;
+  }
+  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));
+}
+
 /**
 * @brief Rebuild the behavior tree for the active sequence, advancing the epoch.
 *
@@ -323,6 +368,7 @@ void CerebrumNode::UpdateBehaviorTree()
    tree_.haltTree();
  } catch (...) {
    // Swallow halt exceptions.
+    RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
  }
  tree_ = std::move(new_tree);
  uint64_t new_epoch = epoch_filter_.epoch() + 1;
@@ -419,7 +465,7 @@ void CerebrumNode::RegisterSkillBtActions()
        RCLCPP_INFO(this->get_logger(), "Node halted %s", bt_type.c_str());
      };
    try {RegisterBtAction(bt_type, handlers);} catch (const BT::BehaviorTreeException & e) {
-      RCLCPP_DEBUG(this->get_logger(), "BT type exists %s: %s", bt_type.c_str(), e.what());
+      RCLCPP_INFO(this->get_logger(), "BT type exists %s: %s", bt_type.c_str(), e.what());
    }
  }
 }
@@ -788,9 +834,15 @@ void CerebrumNode::CreateServices()
        resp->message = "BT running and rebuild not allowed";
        return;
      }
-      RunVlmModel();
-      CancelActiveExecuteBtGoal();
-      UpdateBehaviorTree();
+
+      if (!bt_config_file_path_.empty()) {
+        BuildBehaviorTreeFromFile(bt_config_file_path_);
+      } else {
+        RunVlmModel();
+        CancelActiveExecuteBtGoal();
+        UpdateBehaviorTree();
+      }
+
      resp->success = true;
      resp->message = "Rebuild triggered";
    });
--- a/src/brain/src/skill_manager.cpp
+++ b/src/brain/src/skill_manager.cpp
@@ -11,6 +11,8 @@

 #include <optional>
 #include <cctype>
+#include <cmath>
+#include <utility>
 #include <rclcpp_action/rclcpp_action.hpp>

 #include "interfaces/action/arm_space_control.hpp"
@@ -18,8 +20,14 @@
 #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;
@@ -153,120 +161,91 @@ std::vector<std::pair<std::string, std::string>> SkillManager::enumerate_action_
 */
 void SkillManager::register_interfaces_(const SkillSpec & s)
 {
-  // 由代码生成器根据 YAML 填充的注册器表（接口基名 -> 注册逻辑）
  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) {
-        if (action_clients_) {
-          action_clients_->register_client<ArmSpaceControl>(
-            topic,
-            []() {ArmSpaceControl::Goal g; return g;},
-            nullptr,
-            [this,
-            internal_skill](const ActionClientRegistry::GoalHandle<ArmSpaceControl>::WrappedResult
-            & res) {
-              if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
-              res.result->success)
-              {
-                RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
-              } else {
-                RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
-              }
-            }
-          );
-        }
+        register_action_client_default<ArmSpaceControl>("ArmSpaceControl", topic, internal_skill);
+      }},
+    {"Arm", [this](const std::string & topic, const std::string & internal_skill) {
+        register_action_client_default<Arm>("Arm", "ArmAction", internal_skill);
      }},
    {"HandControl", [this](const std::string & topic, const std::string & internal_skill) {
-        if (action_clients_) {
-          action_clients_->register_client<HandControl>(
-            topic,
-            []() {HandControl::Goal g; return g;},
-            nullptr,
-            [this,
-            internal_skill](const ActionClientRegistry::GoalHandle<HandControl>::WrappedResult &
-            res) {
-              if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
-              res.result->success)
-              {
-                RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
-              } else {
-                RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
-              }
-            }
-          );
-        }
+        register_action_client_default<HandControl>("HandControl", topic, internal_skill);
      }},
    {"LegControl", [this](const std::string & topic, const std::string & internal_skill) {
-        if (action_clients_) {
-          action_clients_->register_client<LegControl>(
-            topic,
-            []() {LegControl::Goal g; return g;},
-            nullptr,
-            [this,
-            internal_skill](const ActionClientRegistry::GoalHandle<LegControl>::WrappedResult &
-            res) {
-              if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
-              res.result->success)
-              {
-                RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
-              } else {
-                RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
-              }
-            }
-          );
-        }
+        register_action_client_default<LegControl>("LegControl", topic, internal_skill);
      }},
    {"VisionGraspObject", [this](const std::string & topic, const std::string & internal_skill) {
-        if (action_clients_) {
-          action_clients_->register_client<VisionGraspObject>(
-            topic,
-            []() {VisionGraspObject::Goal g; return g;},
-            nullptr,
-            [this,
-            internal_skill](const ActionClientRegistry::GoalHandle<VisionGraspObject>::WrappedResult
-            & res) {
-              if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
-              res.result->success)
-              {
-                RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
-              } else {
-                RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
-              }
-            }
-          );
-        }
+        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) {
-        // NOTE: Previously the client was only stored under the snake_case topic key.
-        // get_typed_service_client() queries by canonical skill name (CamelCase), which
-        // caused lookups like "VisionObjectRecognition" to fail (nullptr) and produced
-        // "Service client missing" at runtime. We now register under both the canonical
-        // skill name and the resolved topic alias to remain backward compatible.
-        auto client = node_->create_client<VisionObjectRecognition>(topic);
-        service_clients_.push_back(client);
-        // Store by canonical skill name
-        service_client_map_[internal_skill] = client;
-        // Also store by topic alias (snake_case) in case other code paths use it directly.
-        service_client_map_[topic] = client;
-      }},
+      {"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;
+        }},

  };

+  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;
+    };
+
  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());
      continue;
    }
-  // Strategy updated: publish/resolve topics using snake_case form with leading slash.
-  // Example: VisionObjectRecognition -> vision_object_recognition
-  std::string topic_name = to_snake_case(s.name);
-  if (parsed->kind == "action") {
+
+    std::string prefix_value;
+    node_->get_parameter("skill_topic_prefix", prefix_value);
+    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;
+      };
+
+    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);
@@ -276,6 +255,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
          parsed->base.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);
--- a/src/brain/test/test_bt_registry_xml.cpp
+++ b/src/brain/test/test_bt_registry_xml.cpp
@@ -0,0 +1,48 @@
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <behaviortree_cpp/bt_factory.h>
+#include <filesystem>
+#include "brain/bt_registry.hpp"
+
+TEST(BTRegistry, BuildTreeFromXmlFile)
+{
+  rclcpp::init(0, nullptr);
+  auto node = std::make_shared<rclcpp::Node>("test_bt_registry_xml_node");
+  BT::BehaviorTreeFactory factory;
+  BTRegistry reg(factory, node.get());
+
+  // Register a dummy TestAction handler so the XML leaf resolves.
+  BTActionHandlers handlers;
+  bool start_called = false;
+  handlers.on_start = [&](rclcpp::Node *, BT::TreeNode &) {
+    start_called = true; return BT::NodeStatus::SUCCESS; };
+  reg.register_bt_action("TestAction", handlers);
+
+  // Derive path to sample_tree.xml located alongside this test source file.
+  std::filesystem::path xml_path = std::filesystem::path(__FILE__).parent_path() / "sample_tree.xml";
+  BT::Tree tree;
+  ASSERT_TRUE(reg.build_tree_from_xml_file(xml_path.string(), tree));
+  tree.tickWhileRunning();
+  EXPECT_TRUE(start_called);
+  rclcpp::shutdown();
+}
+
+TEST(BTRegistry, BuildTreeFromXmlText)
+{
+  rclcpp::init(0, nullptr);
+  auto node = std::make_shared<rclcpp::Node>("test_bt_registry_xml_text_node");
+  BT::BehaviorTreeFactory factory;
+  BTRegistry reg(factory, node.get());
+  BTActionHandlers handlers;
+  bool running_called = false;
+  handlers.on_running = [&](rclcpp::Node *, BT::TreeNode &) {
+    running_called = true; return BT::NodeStatus::SUCCESS; };
+  reg.register_bt_action("TestAction", handlers);
+
+  const std::string xml = R"(<root BTCPP_format=\"4\"><BehaviorTree ID=\"T\"><Sequence name=\"root\"><TestAction name=\"X\"/></Sequence></BehaviorTree></root>)";
+  BT::Tree tree;
+  ASSERT_TRUE(reg.build_tree_from_xml_text(xml, tree));
+  tree.tickWhileRunning();
+  EXPECT_TRUE(running_called);
+  rclcpp::shutdown();
+}
--- a/src/modules/motion/arm_control/src/arm_control_node.cpp
+++ b/src/modules/motion/arm_control/src/arm_control_node.cpp
@@ -11,7 +11,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"

-#include "interfaces/action/arm_space_control.hpp"
+// #include "interfaces/action/arm_space_control.hpp"
+#include "interfaces/action/arm.hpp"

 using namespace std::chrono_literals;

@@ -21,22 +22,24 @@ using namespace std::chrono_literals;
 class MotionNode : public rclcpp::Node
 {
 public:
-  using ArmSpaceControl = interfaces::action::ArmSpaceControl;
-  using GoalHandleArmSpaceControl = rclcpp_action::ServerGoalHandle<ArmSpaceControl>;
+  // using ArmSpaceControl = interfaces::action::ArmSpaceControl;
+  using Arm = interfaces::action::Arm;
+  // using GoalHandleArmSpaceControl = rclcpp_action::ServerGoalHandle<ArmSpaceControl>;
+  using GoalHandleArm = rclcpp_action::ServerGoalHandle<Arm>;

  // 构造函数：
-  // - 声明参数 action_name（默认 "ArmSpaceControl"，统一使用首字母大写形式避免重复）
-  // - 创建 ArmSpaceControl 动作服务端，并绑定目标处理、取消处理和接收回调
+  // - 声明参数 action_name（默认 "Arm"，统一使用首字母大写形式避免重复）
+  // - 创建 Arm 动作服务端，并绑定目标处理、取消处理和接收回调
  MotionNode()
  : Node("motion_node")
  {
-  // Declare a parameter for action name (default: "ArmSpaceControl")
-  action_name_ = this->declare_parameter<std::string>("action_name", "arm_space_control");
+  // Declare a parameter for action name (default: "Arm")
+  action_name_ = this->declare_parameter<std::string>("action_name", "ArmAction");

    using std::placeholders::_1;
    using std::placeholders::_2;

-    server_ = rclcpp_action::create_server<ArmSpaceControl>(
+    server_ = rclcpp_action::create_server<Arm>(
      this->get_node_base_interface(),
      this->get_node_clock_interface(),
      this->get_node_logging_interface(),
@@ -47,7 +50,7 @@ public:
      std::bind(&MotionNode::handle_accepted, this, _1));

    RCLCPP_INFO(
-      this->get_logger(), "ArmSpaceControl action server started on '%s'",
+      this->get_logger(), "Arm action server started on '%s'",
      action_name_.c_str());
  }

@@ -58,13 +61,13 @@ private:
  // - 返回值：是否接受目标，这里直接接受并执行
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
-    std::shared_ptr<const ArmSpaceControl::Goal> goal)
+    std::shared_ptr<const Arm::Goal> goal)
  {
    (void)uuid;
    RCLCPP_INFO(
-      this->get_logger(), "Received ArmSpaceControl goal: frame=%s (x=%.3f,y=%.3f,z=%.3f)",
-      goal->target.header.frame_id.c_str(),
-      goal->target.pose.position.x, goal->target.pose.position.y, goal->target.pose.position.z);
+      this->get_logger(), "Received Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.1f, %.1f, %.1f]",
+      goal->body_id, goal->data_type, goal->data_length, goal->command_id, goal->frame_time_stamp,
+      goal->data_array[0], goal->data_array[1], goal->data_array[2]);
    // Accept all goals for now. You can validate mode/effort here.
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
@@ -72,15 +75,15 @@ private:
  // 取消处理回调：
  // - 当客户端请求取消时被调用，这里统一接受取消请求
  rclcpp_action::CancelResponse handle_cancel(
-    const std::shared_ptr<GoalHandleArmSpaceControl>/*goal_handle*/)
+    const std::shared_ptr<GoalHandleArm>/*goal_handle*/)
  {
-    RCLCPP_INFO(this->get_logger(), "Received request to cancel ArmSpaceControl goal");
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel Arm goal");
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  // 接受目标回调：
  // - 收到新的目标后，开启独立线程执行，避免阻塞执行器线程
-  void handle_accepted(const std::shared_ptr<GoalHandleArmSpaceControl> goal_handle)
+  void handle_accepted(const std::shared_ptr<GoalHandleArm> goal_handle)
  {
    // Execute goal in a new thread to avoid blocking executor thread
    std::thread{std::bind(&MotionNode::execute, this, std::placeholders::_1), goal_handle}.detach();
@@ -90,45 +93,47 @@ private:
  // - 模拟执行流程，按 10% 递增发布反馈(progress)
  // - 如果检测到取消请求，则返回取消结果
  // - 正常完成后返回 success=true，并附带简单描述信息
-  void execute(const std::shared_ptr<GoalHandleArmSpaceControl> goal_handle)
+  void execute(const std::shared_ptr<GoalHandleArm> goal_handle)
  {
    const auto goal = goal_handle->get_goal();

-    auto feedback = std::make_shared<ArmSpaceControl::Feedback>();
-    auto result = std::make_shared<ArmSpaceControl::Result>();
+    auto feedback = std::make_shared<Arm::Feedback>();
+    auto result = std::make_shared<Arm::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;
-        result->message = "ArmSpaceControl canceled";
+        result->result = false;
+        // result->message = "Arm canceled";
        goal_handle->canceled(result);
-        RCLCPP_INFO(this->get_logger(), "ArmSpaceControl goal canceled");
+        RCLCPP_INFO(this->get_logger(), "Arm goal canceled");
        return;
      }
-      feedback->progress = static_cast<float>(i);
+      // feedback->progress = static_cast<float>(i);
+      feedback->command_id = goal->command_id;
+      feedback->int_lenth = i;
      goal_handle->publish_feedback(feedback);
-      RCLCPP_DEBUG(this->get_logger(), "ArmSpaceControl progress: %.1f%%", feedback->progress);
+      RCLCPP_INFO(this->get_logger(), "Arm progress: %d", i);
      std::this_thread::sleep_for(100ms);
    }

    // Complete successfully
-    result->success = true;
-    result->message = std::string("Arm moved to target in frame '") +
-      goal->target.header.frame_id + "'";
+    result->result = true;
+    // result->message = std::string("Arm moved to target in frame '") +
+    //   goal->target.header.frame_id + "'";
    goal_handle->succeed(result);
-    RCLCPP_INFO(this->get_logger(), "ArmSpaceControl goal succeeded: %s", result->message.c_str());
+    RCLCPP_INFO(this->get_logger(), "Arm goal succeeded: %s", "arm");
  }

 private:
  std::string action_name_;
-  rclcpp_action::Server<ArmSpaceControl>::SharedPtr server_;
+  rclcpp_action::Server<Arm>::SharedPtr server_;
 };

 // 主函数：
 // - 初始化 ROS，启动 MotionNode，进入自旋，退出时关闭
 // 测试：
-// ros2 action send_goal --feedback /ArmSpaceControl interfaces/action/ArmSpaceControl "{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}}}}"
+// ros2 action send_goal --feedback /Arm interfaces/action/Arm "{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)
 {
--- a/src/scripts/gen_skill_manager.py
+++ b/src/scripts/gen_skill_manager.py