optimize code for ctrlgui

2025-10-25 17:51:14 +08:00
parent 32100b2cf8
commit 0d3cdf386f
22 changed files with 442 additions and 182 deletions
--- a/src/brain/config/robot_skills.yaml
+++ b/src/brain/config/robot_skills.yaml
@@ -40,25 +40,25 @@
  description: "腰部控制"
  interfaces:
    - name: MoveWaist.action
-      default_topic: ""
+      default_topic: "MoveWaist"

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

 - name: MoveWheel
  version: 1.0.0
  description: "轮子控制"
  interfaces:
    - name: MoveWheel.action
-      default_topic: ""
+      default_topic: "MoveWheel"

 - name: MoveHome
  version: 1.0.0
  description: "回到原点位姿"
  interfaces:
    - name: MoveHome.action
-      default_topic: ""
+      default_topic: "MoveHome"
--- a/src/brain/include/brain/cerebellum_node.hpp
+++ b/src/brain/include/brain/cerebellum_node.hpp
@@ -11,8 +11,8 @@
 #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 "interfaces/action/execute_bt_action.hpp"
+#include "interfaces/msg/skill_call.hpp"
 #include "brain/action_registry.hpp"
 #include "brain/skill_manager.hpp"
 #include <smacc2/smacc_signal_detector.hpp>
@@ -90,10 +90,10 @@ 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_;
+  std::vector<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
+  const interfaces::msg::SkillCall * FindCallForSkill(const std::string & name) const
  {
    for (const auto & c : last_calls_) {
      if (c.name == name) return &c;
@@ -190,7 +190,7 @@ private:
   * @param detail Additional free-form detail (may be empty).
   */
  void PublishFeedbackStage(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & gh,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & gh,
    const std::string & stage,
    const std::string & current_skill,
    float progress,
@@ -210,7 +210,7 @@ private:
  bool ExecuteSingleSkill(
    const std::string & skill, const std::string & chosen_topic,
    const SkillSpec & spec, std::string & detail, int index, int total_skills,
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
  /**
   * @brief Determine interface kind from first interface entry.
   * Example: some_pkg/DoThing.action => "action".
@@ -232,7 +232,7 @@ private:
    const std::string & topic,
    const std::string & skill,
    float base_progress,
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
    std::string & detail) const;
  /**
   * @brief Execute an action-type skill.
@@ -253,7 +253,7 @@ private:
    std::string & detail,
    int index,
    int total_skills,
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
  /**
   * @brief Internal helper: run the action worker thread and emit RUN feedback until completion.
   * Extracted to reduce cyclomatic complexity of ExecuteActionSkill.
@@ -272,7 +272,7 @@ private:
    std::chrono::nanoseconds timeout_ns,
    int index,
    int total_skills,
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
    std::string & detail);

  /**
@@ -289,7 +289,7 @@ private:
    const SkillSpec & spec,
    std::string & detail,
    float start_progress,
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
  /**
   * @brief Record per-skill success/failure counts (thread-safe via mutex).
   * @param skill Skill name.
@@ -305,7 +305,7 @@ private:
   * @param goal_handle Goal handle.
   */
  void RunExecuteBtGoal(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);

  /**
   * @brief Execute the full skill sequence building a CerebellumData::ExecResult summary.
@@ -315,7 +315,7 @@ private:
   * @return CerebellumData::ExecResult aggregated result.
   */
  brain::CerebellumData::ExecResult ExecuteSequence(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
    const std::vector<std::string> & skills);
  // --- Decomposed helpers for RunExecuteBtGoal ---
  /**
@@ -325,7 +325,7 @@ private:
   * @return true if goal is valid; false otherwise (goal aborted internally).
   */
  bool ValidateAndParseGoal(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
    std::vector<std::string> & skills_out) const;
  /**
   * @brief Resolve action topic name for a skill.
@@ -344,7 +344,7 @@ private:
   */
  [[deprecated("Result finalization now handled inside state machine complete_cb; kept for backward compatibility")]]
  void FinalizeExecuteBtResult(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
    bool overall_ok,
    int32_t succeeded_skills,
    int32_t total_skills,
--- a/src/brain/include/brain/cerebrum_node.hpp
+++ b/src/brain/include/brain/cerebrum_node.hpp
@@ -18,10 +18,11 @@
 #include "brain/constants.hpp"
 #include "brain/epoch_filter.hpp"
 #include <std_srvs/srv/trigger.hpp>
-#include "brain_interfaces/msg/skill_call.hpp"
+#include "interfaces/msg/skill_call.hpp"
 #include <rclcpp/node_interfaces/node_parameters_interface.hpp>
 #include "brain/robot_config.hpp"
 #include "interfaces/msg/robot_work_info.hpp"
+#include "interfaces/srv/bt_rebuild.hpp"

 namespace brain
 {
@@ -169,23 +170,14 @@ private:
   */
  void LoadParameters();

-  /**
-   * Create external service interfaces (currently a Trigger to force BT rebuild).
-   *
-   * The rebuild service coordinates with atomic flags to avoid unsafe rebuilds
-   * when disallowed. Does not itself perform heavy work; instead triggers the
-   * same path as the periodic task.
-   */
-  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;
+  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;
+  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();

@@ -208,7 +200,7 @@ private:
  rclcpp::TimerBase::SharedPtr bt_timer_;
  rclcpp::TimerBase::SharedPtr pub_robot_work_info_timer_;
  // Service to trigger immediate rebuild of sequence + BT
-  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr rebuild_service_;
+  rclcpp::Service<interfaces::srv::BtRebuild>::SharedPtr rebuild_service_;
  std::atomic<bool> bt_updating_{false};
  std::atomic<bool> bt_pending_run_{false};
  // Encapsulated epoch+skill filter
@@ -221,6 +213,13 @@ private:
  rclcpp::Publisher<interfaces::msg::RobotWorkInfo>::SharedPtr robot_work_info_pub_;
  interfaces::msg::RobotWorkInfo robot_work_info_;

+  struct WorkTime
+  {
+    rclcpp::Time start;
+    double total_seconds {0.0};  //seconds
+  };
+  WorkTime robot_work_time_;
+
  struct PerNodeExecInfo
  {
    bool in_flight {false};
@@ -241,7 +240,7 @@ private:
  // Set of already registered action client names (prevents duplicate registration & duplicate feedback).
  std::unordered_set<std::string> registered_actions_;
  // Per-BT-node execution timeout (seconds).
-  double node_timeout_sec_ {10.0};
+  double node_timeout_sec_ {30.0};
  // Warning threshold: END(SUCCESS) 与最终 result 回调之间超过该秒数则输出警告日志。
  double result_wait_warn_sec_ {2.0};
  // Allow rebuilding the BT while it is still running (default false to avoid preemption).
@@ -350,6 +349,9 @@ private:
   */
  int SimulatedBatteryCapacity();

+  int GetBatteryPercentage();
+  float GetWorkingTime();
+
  /**
   * @brief Get the Future Time object
   * 
@@ -358,6 +360,22 @@ private:
   */
  std::string GetFutureTime(int32_t seconds);

+  // ---- Service handling ----
+  /** Create external service interfaces (currently a Trigger to force BT rebuild). */
+  void CreateServices();
+  
+  /** Handle Trigger type rebuild request */
+  void HandleTriggerRebuild(
+    const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp);
+  
+  /** Handle Remote type rebuild request */
+  void HandleRemoteRebuild(
+    const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
+    const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp);
+  
+  /** Extract filename without extension from a path */
+  std::string ExtractFilenameWithoutExtension(const std::string & path);
+
  // 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_;
--- a/src/brain/src/cerebellum_node.cpp
+++ b/src/brain/src/cerebellum_node.cpp
@@ -37,7 +37,7 @@
 #include "interfaces/action/leg_control.hpp"
 #include "interfaces/action/arm.hpp"
 #include "interfaces/action/vision_grasp_object.hpp"
-#include "brain_interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
+#include "interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
 #include "brain/payload_converters.hpp"
 #include "interfaces/action/slam_mode.hpp"
 #include "interfaces/srv/vision_object_recognition.hpp"
@@ -49,6 +49,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include "arm_action_define.h"

 namespace brain
 {
@@ -125,20 +126,22 @@ void CerebellumNode::ConfigureArmHooks()
        RCLCPP_INFO(this->get_logger(), "%.6f", static_cast<double>(val));
      }

-      goal.body_id = (tls_arm_body_id == 0 || tls_arm_body_id == 1) ? tls_arm_body_id : 0;   // LEFT_ARM=0, RIGHT_ARM=1
-      goal.data_type = 3; //ARM_COMMAND_TYPE_POSE_DIRECT_MOVE
+      goal.body_id = (tls_arm_body_id == arm::LEFT_ARM || 
+        tls_arm_body_id == arm::RIGHT_ARM) ? tls_arm_body_id : arm::LEFT_ARM;
+
+      goal.data_type = arm::ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
      goal.command_id = ++command_id_;

      if (goal.data_length == 14 && goal.data_array.size() == 14) {
-        if (goal.body_id == 0) {
+        if (goal.body_id == arm::LEFT_ARM) {
          goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
              goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]};
-        } else if (goal.body_id == 1) {
+        } else if (goal.body_id == arm::RIGHT_ARM) {
          goal.data_array = {goal.data_array[7], goal.data_array[8], goal.data_array[9],
              goal.data_array[10], goal.data_array[11], goal.data_array[12], goal.data_array[13]};
        }
      }
-      goal.data_length = 7;
+      goal.data_length = arm::POSE_DIMENSION;
      goal.frame_time_stamp = this->get_clock()->now().nanoseconds();

      RCLCPP_INFO(this->get_logger(), "[%s] Control params: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
@@ -164,7 +167,7 @@ void CerebellumNode::ConfigureArmHooks()
  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 == 0);
+      const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == arm::OK);
      const std::string message = res.result ? std::string("action end") : std::string("无返回信息");

      geometry_msgs::msg::Pose pose = res.result->pose;
@@ -794,18 +797,20 @@ void CerebellumNode::SetupStatsTimerAndPublisher()
 */
 void CerebellumNode::SetupExecuteBtServer()
 {
-  using EBA = brain_interfaces::action::ExecuteBtAction;
+  using EBA = interfaces::action::ExecuteBtAction;
  ActionServerRegistry::Handlers<EBA> h;
  // Track active goal handle for cancellation handling
  static std::weak_ptr<rclcpp_action::ServerGoalHandle<EBA>> active_goal_wptr;
  h.on_goal = [this](const rclcpp_action::GoalUUID &, std::shared_ptr<const EBA::Goal> goal) {
      if (!goal) {return rclcpp_action::GoalResponse::REJECT;}
+#ifdef ENABLE_EPOCH_CHECK
      if (goal->epoch < current_epoch_) {
        RCLCPP_WARN(
          this->get_logger(), "Reject stale ExecuteBtAction goal epoch=%u < current=%u", goal->epoch,
          current_epoch_.load());
        return rclcpp_action::GoalResponse::REJECT;
      }
+#endif
      if (goal->epoch > current_epoch_) {
        current_epoch_ = goal->epoch;
        RCLCPP_INFO(this->get_logger(), "Switch to new epoch %u", current_epoch_.load());
@@ -962,13 +967,13 @@ double CerebellumNode::GetTimeoutForSkill(const std::string & skill) const
 * @param detail Optional detail string (may encode status or diagnostics).
 */
 void CerebellumNode::PublishFeedbackStage(
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & gh,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & gh,
  const std::string & stage,
  const std::string & current_skill,
  float progress,
  const std::string & detail) const
 {
-  auto fb = std::make_shared<brain_interfaces::action::ExecuteBtAction::Feedback>();
+  auto fb = std::make_shared<interfaces::action::ExecuteBtAction::Feedback>();
  fb->epoch = current_epoch_;
  fb->stage = stage;
  fb->current_skill = current_skill;
@@ -994,7 +999,7 @@ void CerebellumNode::PublishFeedbackStage(
 bool CerebellumNode::ExecuteSingleSkill(
  const std::string & skill, const std::string & chosen_topic, const SkillSpec & spec,
  std::string & detail, int index, int total_skills,
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
 {
  if (spec.interfaces.empty()) {
    detail = "No interfaces";
@@ -1055,7 +1060,7 @@ bool CerebellumNode::WaitForActionServer(
  const std::string & topic,
  const std::string & skill,
  float base_progress,
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
  std::string & detail) const
 {
  constexpr int kMaxAttempts = 8;
@@ -1094,7 +1099,7 @@ bool CerebellumNode::ExecuteActionSkill(
  std::string & detail,
  int index,
  int total_skills,
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
 {
  (void)spec;  // Currently unused beyond interface kind selection.
  if (!action_clients_->has_client(topic)) {
@@ -1189,7 +1194,7 @@ bool CerebellumNode::RunActionSkillWithProgress(
  std::chrono::nanoseconds timeout_ns,
  int index,
  int total_skills,
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
  std::string & detail)
 {
  std::atomic<bool> finished{false};
@@ -1234,7 +1239,7 @@ bool CerebellumNode::ExecuteServiceSkill(
  const SkillSpec & spec,
  std::string & detail,
  float start_progress,
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
 {
  (void)goal_handle;
  (void)start_progress;
@@ -1322,7 +1327,7 @@ void CerebellumNode::LogStatsPeriodic()
 * @param goal_handle Goal handle provided by the action server layer.
 */
 void CerebellumNode::RunExecuteBtGoal(
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
 {
  std::vector<std::string> skills;
  if (!ValidateAndParseGoal(goal_handle, skills)) {
@@ -1350,7 +1355,7 @@ void CerebellumNode::RunExecuteBtGoal(
  d.complete_cb = [this, goal_handle](bool /*success*/, const std::string & message) {
      // Build result message from runtime latched fields
      auto & d = brain::runtime();
-      using EBA = brain_interfaces::action::ExecuteBtAction;
+      using EBA = interfaces::action::ExecuteBtAction;
      auto res = std::make_shared<EBA::Result>();
      res->success = d.last_success.load();
      res->total_skills = d.last_total_skills;
@@ -1387,7 +1392,7 @@ void CerebellumNode::RunExecuteBtGoal(
 * @return brain::CerebellumData::ExecResult Aggregated outcome for the state machine.
 */
 brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
  const std::vector<std::string> & skills)
 {
  brain::CerebellumData::ExecResult result;
@@ -1479,10 +1484,10 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
 * @return true if validation succeeds and skills_out is populated; false otherwise.
 */
 bool CerebellumNode::ValidateAndParseGoal(
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
  std::vector<std::string> & skills_out) const
 {
-  using EBA = brain_interfaces::action::ExecuteBtAction;
+  using EBA = interfaces::action::ExecuteBtAction;
  auto logger = this->get_logger();
  const auto goal = goal_handle->get_goal();
  if (!goal) {
@@ -1556,13 +1561,13 @@ std::string CerebellumNode::ResolveTopicForSkill(const std::string & skill) cons
 * @param summary_stream Stream containing the pre-built textual summary.
 */
 void CerebellumNode::FinalizeExecuteBtResult(
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
  bool overall_ok,
  int32_t succeeded_skills,
  int32_t total_skills,
  const std::ostringstream & summary_stream)
 {
-  using EBA = brain_interfaces::action::ExecuteBtAction;
+  using EBA = interfaces::action::ExecuteBtAction;
  auto res = std::make_shared<EBA::Result>();
  res->success = overall_ok;
  res->message = summary_stream.str();
--- a/src/brain/src/cerebrum_node.cpp
+++ b/src/brain/src/cerebrum_node.cpp
@@ -24,7 +24,8 @@

 // Project headers
 #include "brain/constants.hpp"
-#include "brain_interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
+#include "interfaces/srv/bt_rebuild.hpp"

 using namespace std::chrono_literals; // NOLINT

@@ -162,7 +163,7 @@ void CerebrumNode::SendExecuteBt(const std::string & bt_action_name)
 */
 void CerebrumNode::RegisterActionClient()
 {
-  using EBA = brain_interfaces::action::ExecuteBtAction;
+  using EBA = interfaces::action::ExecuteBtAction;
  if (registered_actions_.count(brain::kExecuteBtActionName)) {
    return;
  }
@@ -328,6 +329,7 @@ void CerebrumNode::CerebrumTask()
    }
    robot_work_info_.working_state = "RUNNING";
    robot_work_info_.expt_completion_time = GetFutureTime(300);
+    robot_work_time_.start = rclcpp::Clock().now();

    RCLCPP_WARN(this->get_logger(), "CerebrumTask Switching to BT config file path: %s", path_param.config.c_str());
    return;
@@ -366,6 +368,8 @@ void CerebrumNode::ExecuteBehaviorTree()
      robot_work_info_.task[2] = "None";
    }
    robot_work_info_.working_state = BT::toStr(status, false);
+    robot_work_time_.total_seconds += (rclcpp::Clock().now() - robot_work_time_.start).seconds();
+
    RCLCPP_INFO(this->get_logger(), "BT finished status=%s, scheduled=%s", BT::toStr(status, true).c_str(), scheduled.c_str());
  }
 }
@@ -606,26 +610,20 @@ void CerebrumNode::DeclareBtActionParamsForSkillInstance(
    return;
  }

-  //SET DEFAULT PARAMS HERE
-  // 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";
-  // }
+  RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill_name=%s, instance_name=%s", 
+    skill_name.c_str(), instance_name.c_str());
+

-  //READ PARAMS FROM ROBOT CONFIG FILE
  std::string instance_params;
-  if (current_bt_config_params_path_.param.empty()) {
-    RCLCPP_WARN(this->get_logger(), "BT params file path is empty; cannot load sample params");
+  if (current_bt_config_params_path_.config == skill_name) {
+    instance_params = current_bt_config_params_path_.param;
+    RCLCPP_INFO(this->get_logger(), "Remote params: %s", instance_params.c_str());
  } else {
+    //READ PARAMS FROM ROBOT CONFIG FILE    
+    if (current_bt_config_params_path_.param.empty()) {
+      RCLCPP_WARN(this->get_logger(), "BT params file path is empty; cannot load sample params");
+      return;
+    }
    try {
      robot_config_params_ = std::make_unique<brain::robot_config::RobotConfig>(current_bt_config_params_path_.param);
      auto params = robot_config_params_->GetValue(instance_name, "params");
@@ -714,11 +712,11 @@ void CerebrumNode::SetupDynamicParameterHandling()
 * @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.
+ * @return interfaces::msg::SkillCall The constructed SkillCall message.
 */
-brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::string & skill_name) const
+interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::string & skill_name) const
 {
-  brain_interfaces::msg::SkillCall call;
+  interfaces::msg::SkillCall call;
  call.name = skill_name;
  // Interface type/kind inference from SkillSpec
  if (!skill_manager_) {return call;}
@@ -755,9 +753,9 @@ brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std:
 * 
 * @param skill_name 
 * @param instance_name 
- * @return brain_interfaces::msg::SkillCall 
+ * @return interfaces::msg::SkillCall 
 */
-brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
+interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
  const std::string & skill_name,
  const std::optional<std::string> & instance_name) const
 {
@@ -1045,49 +1043,130 @@ void CerebrumNode::LoadParameters()
 /**
 * @brief Create externally callable services.
 *
- * Currently exposes a Trigger service to force an immediate sequence rebuild.
+ * Currently exposes a BtRebuild service to force an immediate sequence rebuild.
 * The callback enforces rebuild policy (respecting allow_bt_rebuild_during_execution_)
 * and follows the same steps as the periodic task: model run -> cancel goal -> rebuild.
 */
 void CerebrumNode::CreateServices()
 {
-  rebuild_service_ = this->create_service<std_srvs::srv::Trigger>(
+  rebuild_service_ = this->create_service<interfaces::srv::BtRebuild>(
    "cerebrum/rebuild_now",
    [this](
-      const std::shared_ptr<std_srvs::srv::Trigger::Request>,
-      std::shared_ptr<std_srvs::srv::Trigger::Response> resp) {
+      const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
+      std::shared_ptr<interfaces::srv::BtRebuild::Response> resp) {
+      // Check if rebuild is allowed
      if (bt_pending_run_ && !allow_bt_rebuild_during_execution_) {
        resp->success = false;
        resp->message = "BT running and rebuild not allowed";
        return;
      }

-      for (const auto & path_param : bt_config_params_paths_) {
-        // path_param is a pair {config, param}; compare the config path string against the current path
-        if (path_param.config == current_bt_config_params_path_.config) {
-          continue;
-        }
-        BuildBehaviorTreeFromFile(path_param.config);
-        current_bt_config_params_path_ = path_param;
+      if (req->type == "Trigger") {
+        HandleTriggerRebuild(resp);
+      } else if (req->type == "Remote") {
+        HandleRemoteRebuild(req, resp);
+      } else if (req->type == "Local") {
+        // TODO: Implement local rebuild
+        resp->success = false;
+        resp->message = "Local rebuild not implemented";
+        RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Local rebuild not implemented");
+      } else {
+        resp->success = false;
+        resp->message = "Unknown rebuild type";
+        RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Unknown rebuild type: %s", req->type.c_str());
+      }

-        //update working info
-        auto scheduled = path_param.config;
-        scheduled = scheduled.substr(scheduled.find_last_of('/')+1, scheduled.find_last_of('.') - scheduled.find_last_of('/')-1);
-        if (robot_work_info_.task.size() > 2) {
-          robot_work_info_.task[2] = (scheduled); //current
-        }
+      // Update work info for successful rebuilds
+      if (resp->success) {
        robot_work_info_.working_state = "RUNNING";
        robot_work_info_.expt_completion_time = GetFutureTime(300);
-
-        RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Switching to BT config file path: %s", path_param.config.c_str());
-        break;
+        robot_work_time_.start = rclcpp::Clock().now();
      }
-      resp->success = true;
-      resp->message = "Rebuild triggered";
-      return;
    });
 }

+/**
+ * @brief Handle Trigger type rebuild request
+ * 
+ * @param resp 
+ */
+void CerebrumNode::HandleTriggerRebuild(
+  const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp)
+{
+  for (const auto & path_param : bt_config_params_paths_) {
+    // path_param is a pair {config, param}; compare the config path string against the current path
+    if (path_param.config == current_bt_config_params_path_.config) {
+      continue;
+    }
+    
+    BuildBehaviorTreeFromFile(path_param.config);
+    current_bt_config_params_path_ = path_param;
+
+    // Update working info
+    std::string scheduled = ExtractFilenameWithoutExtension(path_param.config);
+    if (robot_work_info_.task.size() > 2) {
+      robot_work_info_.task[2] = scheduled; // current
+    }
+    
+    RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Switching to BT config file path: %s", path_param.config.c_str());
+    break;
+  }
+  
+  resp->success = true;
+  resp->message = "Rebuild triggered";
+}
+
+/**
+ * @brief Handle Remote type rebuild request
+ * 
+ * @param req 
+ * @param resp 
+ */
+void CerebrumNode::HandleRemoteRebuild(
+  const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
+  const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp)
+{
+  auto tmp = ParseListString(req->config);
+  if (!tmp.empty()) {
+    fixed_skill_sequence_ = tmp;
+    current_bt_config_params_path_.config = req->config;
+    current_bt_config_params_path_.param = req->param;
+    RunVlmModel();
+    CancelActiveExecuteBtGoal();
+    UpdateBehaviorTree();
+
+    if (robot_work_info_.task.size() > 2) {
+      robot_work_info_.task[2] = req->config;
+    }
+
+    resp->success = true;
+    resp->message = "Remote rebuild triggered";
+    RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Remote rebuild triggered");
+  } else {
+    resp->success = false;
+    resp->message = "Remote rebuild failed";
+    RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Failed to parse remote rebuild config");
+  }
+}
+
+/**
+ * @brief Extract filename without extension from a path
+ */
+std::string CerebrumNode::ExtractFilenameWithoutExtension(const std::string & path)
+{
+  size_t lastSlash = path.find_last_of('/');
+  size_t lastDot = path.find_last_of('.');
+  if (lastSlash == std::string::npos) {
+    lastSlash = 0;
+  } else {
+    lastSlash++;
+  }
+  if (lastDot == std::string::npos || lastDot < lastSlash) {
+    return path.substr(lastSlash);
+  }
+  return path.substr(lastSlash, lastDot - lastSlash);
+}
+
 /**
 * @brief Load the robot configuration file.
 * 
@@ -1095,7 +1174,7 @@ void CerebrumNode::CreateServices()
 * @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) {
@@ -1104,8 +1183,6 @@ bool CerebrumNode::LoadRobotConfiguration()
  }
  robot_skill_file_path_ = share_directory_ + *skill_file;
  RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
-
-
  auto path = robot_config_->ConfigParamsPath("cerebrum_node");
  if (path != std::nullopt) {
    for (const auto & p : *path) {
@@ -1124,10 +1201,10 @@ bool CerebrumNode::LoadRobotConfiguration()
 * @return int 
 */
 int CerebrumNode::SimulatedBatteryCapacity()
-{ 
+{
  static int simulated_battery_capacity = 85;
  static rclcpp::Time last_battery_update = rclcpp::Time(0, 0, this->get_clock()->get_clock_type());
-  static constexpr double kBatteryDecrementIntervalSec = 60.0; // 1 minute
+  static constexpr double kBatteryDecrementIntervalSec = 180.0; // 3 minutes
  // Initialize last_battery_update on first run
  if (last_battery_update.nanoseconds() == 0) {
    last_battery_update = this->now();
@@ -1143,6 +1220,25 @@ int CerebrumNode::SimulatedBatteryCapacity()
  return simulated_battery_capacity;
 }

+/**
+ * @brief Get the battery percentage.
+ * 
+ * @return int 
+ */
+int CerebrumNode::GetBatteryPercentage()
+{
+  return SimulatedBatteryCapacity();
+}
+
+/**
+ * @brief Get the working time in hours.
+ * 
+ * @return float 
+ */
+float CerebrumNode::GetWorkingTime()
+{
+  return (float)(robot_work_time_.total_seconds / 3600);  //hours
+}

 /**
 * @brief Publish robot work information.
@@ -1167,8 +1263,8 @@ void CerebrumNode::RobotWorkInfoPublish()
    interfaces::msg::RobotWorkInfo msg { robot_work_info_ };
    
    msg.msg_id = robot_work_info_pub_msg_id++;
-    msg.battery_capacity = SimulatedBatteryCapacity();
-    msg.working_time = 1.5f;
+    msg.battery_capacity = GetBatteryPercentage();
+    msg.working_time = GetWorkingTime();
    msg.nav_state = "Normal";
    msg.comm_quality = "Good";
    msg.expt_completion_time = robot_work_info_.expt_completion_time;
--- a/src/brain/test/test_cerebellum_node.cpp
+++ b/src/brain/test/test_cerebellum_node.cpp
@@ -10,9 +10,9 @@
 #include "brain/constants.hpp"
 // Legacy MoveToPosition action was removed from automatic server startup.
 // Adapt test to validate that CerebellumNode brings up the unified ExecuteBtAction server.
-#include "brain_interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/execute_bt_action.hpp"

-using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = interfaces::action::ExecuteBtAction;

 TEST(CerebellumNode, StartsAndProvidesActionServer)
 {
--- a/src/brain/test/test_execute_bt_action.cpp
+++ b/src/brain/test/test_execute_bt_action.cpp
@@ -7,10 +7,10 @@

 #include "brain/cerebellum_node.hpp"
 #include "brain/constants.hpp"
-#include "brain_interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"

-using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;

 namespace
--- a/src/brain/test/test_execute_bt_action_cancel.cpp
+++ b/src/brain/test/test_execute_bt_action_cancel.cpp
@@ -4,10 +4,10 @@
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "brain/cerebellum_node.hpp"
 #include "brain/constants.hpp"
-#include "brain_interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"

-using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;

 namespace
--- a/src/brain/test/test_execute_bt_action_extended.cpp
+++ b/src/brain/test/test_execute_bt_action_extended.cpp
@@ -9,12 +9,12 @@

 #include "brain/cerebellum_node.hpp"
 #include "brain/constants.hpp"
-#include "brain_interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 #include "interfaces/action/hand_control.hpp"
 #include "interfaces/action/leg_control.hpp"

-using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;
 using HandControl = interfaces::action::HandControl;
 using LegControl = interfaces::action::LegControl;
--- a/src/brain/test/test_execute_bt_action_run_interp.cpp
+++ b/src/brain/test/test_execute_bt_action_run_interp.cpp
@@ -4,10 +4,10 @@
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "brain/cerebellum_node.hpp"
 #include "brain/constants.hpp"
-#include "brain_interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"

-using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;

 namespace
--- a/src/brain/test/test_execute_bt_action_stats.cpp
+++ b/src/brain/test/test_execute_bt_action_stats.cpp
@@ -5,10 +5,10 @@
 #include <std_msgs/msg/string.hpp>
 #include "brain/cerebellum_node.hpp"
 #include "brain/constants.hpp"
-#include "brain_interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"

-using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;

 namespace
--- a/src/brain/test/test_execute_bt_action_timeout_continue.cpp
+++ b/src/brain/test/test_execute_bt_action_timeout_continue.cpp
@@ -4,11 +4,11 @@
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "brain/cerebellum_node.hpp"
 #include "brain/constants.hpp"
-#include "brain_interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 #include "interfaces/action/hand_control.hpp"

-using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;
 using HandControl = interfaces::action::HandControl;

--- a/src/brain/test/test_execute_bt_action_unsupported.cpp
+++ b/src/brain/test/test_execute_bt_action_unsupported.cpp
@@ -4,10 +4,10 @@
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "brain/cerebellum_node.hpp"
 #include "brain/constants.hpp"
-#include "brain_interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"

-using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;

 namespace
--- a/src/brain_interfaces/CMakeLists.txt
+++ b/src/brain_interfaces/CMakeLists.txt
@@ -18,10 +18,8 @@ find_package(interfaces REQUIRED)

 set(msg_files
  msg/Movej.msg
-  msg/SkillCall.msg
 )
 set(action_files
-  action/ExecuteBtAction.action
 )
 set(srv_files
  srv/VisionDemo.srv
--- a/src/brain_interfaces/action/ExecuteBtAction.action
+++ b/src/brain_interfaces/action/ExecuteBtAction.action
@@ -1,47 +0,0 @@
-###############################################################
-# ExecuteBtAction
-#
-# Goal:
-#   action_name: A skill sequence or single skill name. For a single
-#                skill this is typically the canonical CamelCase name
-#                (e.g. "ArmSpaceControl"). Multiple skills may be
-#                separated by commas/semicolons/spaces.
-#
-# Result:
-#   success:     Overall success flag for the entire (possibly multi)
-#                skill sequence.
-#   message:     Human readable summary (e.g. "Seq{A,B}: A(OK), B(FAIL)[detail]").
-#   total_skills:How many skills were requested (after parsing).
-#   succeeded_skills: Number of skills that finished with success.
-#
-# Feedback (streamed):
-#   stage:        High‑level stage label: START | WAIT | RUN | END.
-#   current_skill:Name of the skill currently being processed.
-#   progress:     Float in [0,1]. START of first skill => 0.0, END of last
-#                 skill => 1.0. Intermediate values are proportional to
-#                 (completed_skills + stage_offset)/total_skills where
-#                 stage_offset is 0 for START/WAIT/RUN and 1 for END of a skill.
-#   detail:       Optional detail (e.g. retry attempt, OK/FAIL, waiting cause).
-###############################################################
-
-# Goal
-# epoch: Sequence epoch (monotonically increasing id assigned by Cerebrum). Used so the
-#        server can reject stale goals and stop publishing feedback from older epochs.
-uint32 epoch
-string action_name
-brain_interfaces/SkillCall[] calls
-
---
-# Result
-bool success
-string message
-int32 total_skills
-int32 succeeded_skills
-
---
-# Feedback
-uint32 epoch
-string stage
-string current_skill
-float32 progress
-string detail
--- a/src/brain_interfaces/msg/SkillCall.msg
+++ b/src/brain_interfaces/msg/SkillCall.msg
@@ -1,6 +0,0 @@
-# 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/move_home/CMakeLists.txt
+++ b/src/modules/motion/move_home/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.8)
+project(move_home)
+
+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_home_node src/move_home_node.cpp)
+target_include_directories(move_home_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(move_home_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(move_home_node rclcpp rclcpp_action interfaces geometry_msgs)
+
+install(TARGETS move_home_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_home/package.xml
+++ b/src/modules/motion/move_home/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_home</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_home/src/move_home_node.cpp
+++ b/src/modules/motion/move_home/src/move_home_node.cpp
@@ -0,0 +1,134 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "interfaces/action/move_home.hpp"
+
+using namespace std::chrono_literals;
+
+// MoveHomeNode 节点：
+// - 启动一个 MoveHome 动作服务端（默认话题名称：/move_waist，可通过参数 action_name 修改）
+// - 接受目标后模拟进度反馈，支持取消，并在完成后返回结果
+class MoveHomeNode : public rclcpp::Node
+{
+public:
+  using MoveHome = interfaces::action::MoveHome;
+  using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
+
+  // 构造函数：
+  // - 声明参数 action_name（默认 "move_home"，统一使用首字母大写形式避免重复）
+  // - 创建 MoveHome 动作服务端，并绑定目标处理、取消处理和接收回调
+  MoveHomeNode()
+  : Node("move_home_node")
+  {
+  // Declare a parameter for action name (default: "MoveHome")
+  action_name_ = this->declare_parameter<std::string>("action_name", "MoveHome");
+
+    using std::placeholders::_1;
+    using std::placeholders::_2;
+
+    server_ = rclcpp_action::create_server<MoveHome>(
+      this->get_node_base_interface(),
+      this->get_node_clock_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      action_name_,
+      std::bind(&MoveHomeNode::handle_goal, this, _1, _2),
+      std::bind(&MoveHomeNode::handle_cancel, this, _1),
+      std::bind(&MoveHomeNode::handle_accepted, this, _1));
+
+    RCLCPP_INFO(
+      this->get_logger(), "MoveHome 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 MoveHome::Goal> goal)
+  {
+    (void)uuid;
+    RCLCPP_INFO(this->get_logger(), "Received MoveHome goal");
+    // 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<GoalHandleMoveHome>/*goal_handle*/)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel MoveHome goal");
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  // 接受目标回调：
+  // - 收到新的目标后，开启独立线程执行，避免阻塞执行器线程
+  void handle_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
+  {
+    // Execute goal in a new thread to avoid blocking executor thread
+    std::thread{std::bind(&MoveHomeNode::execute, this, std::placeholders::_1), goal_handle}.detach();
+  }
+
+  // 执行函数：
+  // - 模拟执行流程，按 10% 递增发布反馈(progress)
+  // - 如果检测到取消请求，则返回取消结果
+  // - 正常完成后返回 success=true，并附带简单描述信息
+  void execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
+  {
+    const auto goal = goal_handle->get_goal();
+
+    auto feedback = std::make_shared<MoveHome::Feedback>();
+    auto result = std::make_shared<MoveHome::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(), "MoveHome 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(), "MoveHome progress: %d", i);
+      std::this_thread::sleep_for(100ms);
+    }
+
+    // Complete successfully
+    result->success = true;
+    // result->message = std::string("MoveHome moved to target in frame '") +
+    //   goal->target.header.frame_id + "'";
+    goal_handle->succeed(result);
+    RCLCPP_INFO(this->get_logger(), "MoveHome goal succeeded");
+  }
+
+private:
+  std::string action_name_;
+  rclcpp_action::Server<MoveHome>::SharedPtr server_;
+};
+
+// 主函数：
+// - 初始化 ROS，启动 MotionNode，进入自旋，退出时关闭
+// 测试：
+// ros2 action send_goal --feedback /MoveHome interfaces/action/MoveHome "{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<MoveHomeNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
--- a/src/modules/motion/move_leg/src/move_leg_node.cpp
+++ b/src/modules/motion/move_leg/src/move_leg_node.cpp
@@ -27,7 +27,7 @@ public:
  : Node("move_leg_node")
  {
  // Declare a parameter for action name (default: "MoveLeg")
-  action_name_ = this->declare_parameter<std::string>("action_name", "move_leg");
+  action_name_ = this->declare_parameter<std::string>("action_name", "MoveLeg");

    using std::placeholders::_1;
    using std::placeholders::_2;
--- a/src/modules/motion/move_waist/src/move_waist_node.cpp
+++ b/src/modules/motion/move_waist/src/move_waist_node.cpp
@@ -27,7 +27,7 @@ public:
  : Node("move_waist_node")
  {
  // Declare a parameter for action name (default: "MoveWaist")
-  action_name_ = this->declare_parameter<std::string>("action_name", "move_waist");
+  action_name_ = this->declare_parameter<std::string>("action_name", "MoveWaist");

    using std::placeholders::_1;
    using std::placeholders::_2;
--- a/src/modules/motion/move_wheel/src/move_wheel_node.cpp
+++ b/src/modules/motion/move_wheel/src/move_wheel_node.cpp
@@ -27,7 +27,7 @@ public:
  : Node("move_wheel_node")
  {
  // Declare a parameter for action name (default: "MoveWheel")
-  action_name_ = this->declare_parameter<std::string>("action_name", "move_wheel");
+  action_name_ = this->declare_parameter<std::string>("action_name", "MoveWheel");

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