merge feature-carry-boxes

2025-10-21 18:04:58 +08:00
parent 6eb7407100 424260a1ba
commit 30f7001bbc
15 changed files with 706 additions and 94 deletions
--- a/src/brain/config/bt_carry_boxes.params.yaml
+++ b/src/brain/config/bt_carry_boxes.params.yaml
@@ -7,9 +7,9 @@

    '
 - name: s1_waist_bend_down
-  params: 'move_pitch_degree: 0.0
+  params: 'move_pitch_degree: 2.5

-    move_yaw_degree: 0.0
+    move_yaw_degree: -10.0

    '
 - name: s2_arm_stretch_out
@@ -17,13 +17,13 @@

    data_type: 0

-    data_length: 0
+    data_length: 14

    command_id: 0

    frame_time_stamp: 0

-    data_array: []
+    data_array: [0.222853, 0.514124, 0.261742, -0.63142093, 0.18186004, -0.12407289, 0.74353241, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123]

    '
 - name: s3_hand_pickup
@@ -37,39 +37,39 @@

    data_type: 0

-    data_length: 0
+    data_length: 14

    command_id: 0

    frame_time_stamp: 0

-    data_array: []
+    data_array: [0.198632, 0.513333, 0.072526, -0.71159701, 0.13862565, -0.14251799, 0.67387035, 0.253501, 0.476683, 0.265433, -0.51013004, -0.02328561, -0.48576245, 0.70940817]

    '
 - name: s5_waist_bend_up
-  params: 'move_pitch_degree: 0.0
+  params: 'move_pitch_degree: 1.0

-    move_yaw_degree: 0.0
+    move_yaw_degree: 3.0

    '
 - name: s6_waist_turn_around
-  params: 'move_pitch_degree: 0.0
+  params: 'move_pitch_degree: 180.0

    move_yaw_degree: 0.0

    '
 - name: s7_leg_move_back
-  params: 'move_up_distance: 0.0
+  params: 'move_up_distance: 0.5

    '
 - name: s8_wheel_move_back
-  params: 'move_distance: 0.0
+  params: 'move_distance: 1.0

    move_angle: 0.0

    '
 - name: s9_waist_bend_down
-  params: 'move_pitch_degree: 0.0
+  params: 'move_pitch_degree: 5.0

    move_yaw_degree: 0.0

@@ -79,17 +79,17 @@

    data_type: 0

-    data_length: 0
+    data_length: 14

    command_id: 0

    frame_time_stamp: 0

-    data_array: []
+    data_array: [0.222853, 0.514124, 0.261742, -0.63142093, 0.18186004, -0.12407289, 0.74353241, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123]

    '
 - name: s11_hand_release
-  params: 'mode: 0
+  params: 'mode: 1

    effort: 0.0

@@ -99,25 +99,25 @@

    data_type: 0

-    data_length: 0
+    data_length: 14

    command_id: 0

    frame_time_stamp: 0

-    data_array: []
+    data_array: [0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123, 0.222853, 0.514124, 0.261742, -0.63142093, 0.18186004, -0.12407289, 0.74353241]

    '
 - name: s13_camera_take_photo
-  params: 'mode: 0
+  params: 'mode: 1

    effort: 0.0

    '
 - name: s14_waist_bend_up
-  params: 'move_pitch_degree: 0.0
+  params: 'move_pitch_degree: -5.0

-    move_yaw_degree: 0.0
+    move_yaw_degree: 4.0

    '
 - name: s15_arm_retract
@@ -125,12 +125,12 @@

    data_type: 0

-    data_length: 0
+    data_length: 14

    command_id: 0

    frame_time_stamp: 0

-    data_array: []
+    data_array: [0.123562, 0.555266, 0.062529, -0.75662638, 0.02276690, -0.14333775, 0.63753626, 0.082317, 0.557912, 0.123469, -0.63065177, -0.03018818, -0.18611339, 0.75281394]

    '
--- a/src/brain/include/brain/cerebrum_node.hpp
+++ b/src/brain/include/brain/cerebrum_node.hpp
@@ -195,6 +195,7 @@ private:
  std::unique_ptr<BTRegistry> bt_registry_;
  std::unique_ptr<SkillManager> skill_manager_;
  std::unique_ptr<brain::robot_config::RobotConfig> robot_config_;
+  std::unique_ptr<brain::robot_config::RobotConfig> robot_config_params_;
  std::string active_sequence_;
  std::string share_directory_;
  std::string robot_skill_file_path_;
@@ -226,12 +227,16 @@ private:
    ExecResultCode result_code {ExecResultCode::UNKNOWN};
    bool awaiting_result {false};              ///< 已收到 END(SUCCESS) 反馈，等待最终 action result
    rclcpp::Time end_feedback_time;            ///< 记录 END(SUCCESS) 到达时间
+    // Per-instance execution memory: which BT node instances (by node.name()) already finished SUCCESS
+    std::unordered_set<std::string> done_instances;
+    // The instance currently in flight for this skill type (only one at a time due to Sequence)
+    std::optional<std::string> current_instance;
  };
  std::unordered_map<std::string, PerNodeExecInfo> per_node_exec_;
  // 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_ {5.0};
+  double node_timeout_sec_ {10.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).
--- a/src/brain/include/brain/robot_config.hpp
+++ b/src/brain/include/brain/robot_config.hpp
@@ -24,8 +24,12 @@ struct Entry {
 // Loader and query interface for robot configuration.
 class RobotConfig {
 public:
-  RobotConfig() {
-    LoadFromFile(ament_index_cpp::get_package_share_directory("brain") + std::string("/config/robot_config.yaml"));
+  RobotConfig(std::string path = "") {
+    if (path.empty()) {
+      LoadFromFile(ament_index_cpp::get_package_share_directory("brain") + std::string("/config/robot_config.yaml"));
+    } else {
+      LoadFromFile(path);
+    }
  }
  // Parse YAML from a file path. Returns true on success.
  bool LoadFromFile(const std::string& path) {
--- a/src/brain/src/cerebellum_node.cpp
+++ b/src/brain/src/cerebellum_node.cpp
@@ -116,64 +116,32 @@ void CerebellumNode::ConfigureArmHooks()
  SkillManager::ActionHookOptions<interfaces::action::Arm> hooks;
  hooks.make_goal = [this](const std::string & skill_name) {
      interfaces::action::Arm::Goal goal{};
-      // Plan A: per-call YAML overrides
      (void)TryParseCallPayload<interfaces::action::Arm::Goal>(skill_name, goal);

-      RCLCPP_INFO(
-        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, %.1f, %.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], goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]);
+      RCLCPP_INFO(this->get_logger(), "[%s]Received Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=",
+        skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);

+      for (auto & val : goal.data_array) {
+        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.command_id = ++command_id_;

-      if (target_pose_.empty()) {
-        RCLCPP_WARN(this->get_logger(), "[%s] 未检测到目标物体，使用默认位置", skill_name.c_str());
-        // default position
-        // goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
-        goal.data_array = {0.222853, 0.514124, 0.261742,
-          -0.65115316, 0.05180144, -0.19539139, 0.73153153};
-        goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
-      } else {
-        RCLCPP_INFO(this->get_logger(), "[%s] 使用检测到的目标物体位置", skill_name.c_str());
-        const auto pose_it = target_pose_.find(target_frame_);
-        if (pose_it == target_pose_.end()) {
-          RCLCPP_WARN(this->get_logger(), "[%s] 未找到目标 %s 的姿态缓存，使用默认坐标",
-            skill_name.c_str(), target_frame_.c_str());
-          // goal.data_array = {-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
-          goal.data_array = {0.222853, 0.514124, 0.261742,
-            -0.65115316, 0.05180144, -0.19539139, 0.73153153};
-          goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
-        } else {
-          geometry_msgs::msg::PoseStamped pose_in_arm;
-          const bool transformed = TransformPoseToArmFrame(pose_it->second, pose_in_arm);
-          const auto & pose_ref = transformed ? pose_in_arm : pose_it->second;
-          if (!transformed) {
-            RCLCPP_WARN(this->get_logger(), "[%s] 坐标系转换失败，继续使用 %s 坐标系数据",
-              skill_name.c_str(), pose_ref.header.frame_id.c_str());
-          } else {
-            RCLCPP_INFO(this->get_logger(), "[%s] 目标姿态已转换到 %s 坐标系",
-              skill_name.c_str(), arm_target_frame_.c_str());
-          }
-          goal.data_array = {
-            pose_ref.pose.position.x,
-            pose_ref.pose.position.y,
-            pose_ref.pose.position.z,
-            pose_ref.pose.orientation.x,
-            pose_ref.pose.orientation.y,
-            pose_ref.pose.orientation.z,
-            pose_ref.pose.orientation.w
-          };
-          goal.frame_time_stamp = pose_ref.header.stamp.sec * 1000000000LL
-            + pose_ref.header.stamp.nanosec;
+      if (goal.data_length == 14 && goal.data_array.size() == 14) {
+        if (goal.body_id == 0) {
+          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) {
+          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]};
        }
-        target_pose_.clear();
      }
-      goal.data_length = static_cast<int32_t>(goal.data_array.size());
+      goal.data_length = 7;
+      goal.frame_time_stamp = this->get_clock()->now().nanoseconds();

-      RCLCPP_INFO(this->get_logger(), "[%s] 目标参数 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]",
+      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]",
        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], goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]);

--- a/src/brain/src/cerebrum_node.cpp
+++ b/src/brain/src/cerebrum_node.cpp
@@ -108,7 +108,7 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
  RegisterActionClient();

  bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
-  task_timer_ = this->create_wall_timer(5000ms, [this]() {CerebrumTask();});
+  task_timer_ = this->create_wall_timer(30000ms, [this]() {CerebrumTask();});

  CreateServices();

@@ -430,8 +430,16 @@ void CerebrumNode::RegisterSkillBtActions()
    BTActionHandlers handlers;
  handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode & node) {
        auto & info = per_node_exec_[bt_type];
-        if (info.in_flight || info.result) {
-          return info.result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::RUNNING;
+        const std::string instance_name = node.name();
+        // If this specific BT node instance already finished successfully earlier in this tree run,
+        // short-circuit to SUCCESS so that retries only re-execute the failed node.
+        if (info.done_instances.find(instance_name) != info.done_instances.end()) {
+          RCLCPP_INFO(this->get_logger(), "[%s/%s] already succeeded, skip", bt_type.c_str(), instance_name.c_str());
+          return BT::NodeStatus::SUCCESS;
+        }
+        // If a previous instance for this skill type is still in flight, keep running.
+        if (info.in_flight) {
+          return BT::NodeStatus::RUNNING;
        }
        info.in_flight = true;
        info.result = false;
@@ -440,8 +448,7 @@ void CerebrumNode::RegisterSkillBtActions()
        info.skill_name = skill.name;
        info.last_progress = 0.0;
        info.cancel_state = PerNodeExecInfo::CancelState::NONE;
-
-        const std::string instance_name = node.name();
+        info.current_instance = instance_name;
        DeclareBtActionParamsForSkillInstance(skill.name, instance_name);
        // Override goal builder to send only this skill.
        single_skill_goal_override_ = skill.name;
@@ -452,26 +459,32 @@ void CerebrumNode::RegisterSkillBtActions()
          info.result = false;
          info.result_code = ExecResultCode::FAILURE;
          single_skill_goal_override_.reset();
+          info.current_instance.reset();
          return BT::NodeStatus::FAILURE;
        }
        action_registry_->send(brain::kExecuteBtActionName, this->get_logger());
-        RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_start", bt_type.c_str());
+        RCLCPP_INFO(this->get_logger(), "[%s/%s] BTAction on_start", bt_type.c_str(), instance_name.c_str());
        // Clear override after send to avoid affecting other triggers.
        single_skill_goal_override_.reset();
        single_skill_instance_override_.reset();
        return BT::NodeStatus::RUNNING;
      };
-    handlers.on_running = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
+    handlers.on_running = [this, bt_type](rclcpp::Node *, BT::TreeNode & node) {
        auto it = per_node_exec_.find(bt_type);
        if (it == per_node_exec_.end()) {return BT::NodeStatus::FAILURE;}
        auto & info = it->second;
+        const std::string instance_name = node.name();
+        // If this instance is already marked done (from a previous retry cycle), succeed immediately.
+        if (info.done_instances.find(instance_name) != info.done_instances.end()) {
+          return BT::NodeStatus::SUCCESS;
+        }
        if (info.in_flight) {
          double elapsed = (this->now() - info.start_time).seconds();
          if (elapsed > node_timeout_sec_) {
            info.in_flight = false;
            info.result = false;
            info.result_code = ExecResultCode::FAILURE;
-            RCLCPP_WARN(this->get_logger(), "[%s] BTAction on_running timeout after %.2f sec", bt_type.c_str(), elapsed);
+            RCLCPP_WARN(this->get_logger(), "[%s/%s] BTAction on_running timeout after %.2f sec", bt_type.c_str(), instance_name.c_str(), elapsed);
            action_registry_->cancel(brain::kExecuteBtActionName, this->get_logger());
            // return BT::NodeStatus::FAILURE;
          }
@@ -487,7 +500,17 @@ void CerebrumNode::RegisterSkillBtActions()
          }
          return BT::NodeStatus::RUNNING;
        }
-        return info.result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+        // Terminal state for this instance
+        if (info.result) {
+          // Mark this instance as done so future retries skip it
+          if (!instance_name.empty()) {
+            info.done_instances.insert(instance_name);
+          }
+          info.current_instance.reset();
+          return BT::NodeStatus::SUCCESS;
+        }
+        info.current_instance.reset();
+        return BT::NodeStatus::FAILURE;
      };
    handlers.on_halted = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
        RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_halted", bt_type.c_str());
@@ -548,18 +571,38 @@ 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";
+  // }
+
+  //READ PARAMS FROM ROBOT CONFIG FILE
  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";
+  if (bt_params_file_path_.empty()) {
+    RCLCPP_WARN(this->get_logger(), "BT params file path is empty; cannot load sample params");
+  } else {
+    try {
+      robot_config_params_ = std::make_unique<brain::robot_config::RobotConfig>(bt_params_file_path_);
+      auto params = robot_config_params_->GetValue(instance_name, "params");
+      if (params == std::nullopt) {
+        RCLCPP_WARN(this->get_logger(), "BT params file %s does not contain params for %s", bt_params_file_path_.c_str(), instance_name.c_str());
+      } else {
+        instance_params = *params;
+        RCLCPP_INFO(this->get_logger(), "Loaded BT params for %s, instance_params: %s", instance_name.c_str(), instance_params.c_str());
+      }
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
+    }
  }

  UpdateBtActionParamsForSkillInstance(skill_name, instance_name, instance_params);
--- a/src/modules/camera_take_photo/CMakeLists.txt
+++ b/src/modules/camera_take_photo/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.8)
+project(camera_take_photo)
+
+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(camera_take_photo_node src/camera_take_photo_node.cpp)
+target_include_directories(camera_take_photo_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(camera_take_photo_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(camera_take_photo_node rclcpp rclcpp_action interfaces geometry_msgs)
+
+install(TARGETS camera_take_photo_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/camera_take_photo/package.xml
+++ b/src/modules/camera_take_photo/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>camera_take_photo</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/camera_take_photo/src/camera_take_photo_node.cpp
+++ b/src/modules/camera_take_photo/src/camera_take_photo_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/camera_take_photo.hpp"
+
+using namespace std::chrono_literals;
+
+// CameraTakePhotoNode 节点：
+// - 启动一个 CameraTakePhoto 动作服务端（默认话题名称：/move_waist，可通过参数 action_name 修改）
+// - 接受目标后模拟进度反馈，支持取消，并在完成后返回结果
+class CameraTakePhotoNode : public rclcpp::Node
+{
+public:
+  using CameraTakePhoto = interfaces::action::CameraTakePhoto;
+  using GoalHandleCameraTakePhoto = rclcpp_action::ServerGoalHandle<CameraTakePhoto>;
+
+  // 构造函数：
+  // - 声明参数 action_name（默认 "camera_take_photo"，统一使用首字母大写形式避免重复）
+  // - 创建 CameraTakePhoto 动作服务端，并绑定目标处理、取消处理和接收回调
+  CameraTakePhotoNode()
+  : Node("camera_take_photo_node")
+  {
+  // Declare a parameter for action name (default: "CameraTakePhoto")
+  action_name_ = this->declare_parameter<std::string>("action_name", "camera_take_photo");
+
+    using std::placeholders::_1;
+    using std::placeholders::_2;
+
+    server_ = rclcpp_action::create_server<CameraTakePhoto>(
+      this->get_node_base_interface(),
+      this->get_node_clock_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      action_name_,
+      std::bind(&CameraTakePhotoNode::handle_goal, this, _1, _2),
+      std::bind(&CameraTakePhotoNode::handle_cancel, this, _1),
+      std::bind(&CameraTakePhotoNode::handle_accepted, this, _1));
+
+    RCLCPP_INFO(
+      this->get_logger(), "CameraTakePhoto 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 CameraTakePhoto::Goal> goal)
+  {
+    (void)uuid;
+    RCLCPP_INFO(this->get_logger(), "Received CameraTakePhoto goal: mode=%d", goal->mode);
+    // 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<GoalHandleCameraTakePhoto>/*goal_handle*/)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel CameraTakePhoto goal");
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  // 接受目标回调：
+  // - 收到新的目标后，开启独立线程执行，避免阻塞执行器线程
+  void handle_accepted(const std::shared_ptr<GoalHandleCameraTakePhoto> goal_handle)
+  {
+    // Execute goal in a new thread to avoid blocking executor thread
+    std::thread{std::bind(&CameraTakePhotoNode::execute, this, std::placeholders::_1), goal_handle}.detach();
+  }
+
+  // 执行函数：
+  // - 模拟执行流程，按 10% 递增发布反馈(progress)
+  // - 如果检测到取消请求，则返回取消结果
+  // - 正常完成后返回 success=true，并附带简单描述信息
+  void execute(const std::shared_ptr<GoalHandleCameraTakePhoto> goal_handle)
+  {
+    const auto goal = goal_handle->get_goal();
+
+    auto feedback = std::make_shared<CameraTakePhoto::Feedback>();
+    auto result = std::make_shared<CameraTakePhoto::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(), "CameraTakePhoto 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(), "CameraTakePhoto progress: %d", i);
+      std::this_thread::sleep_for(100ms);
+    }
+
+    // Complete successfully
+    result->success = true;
+    // result->message = std::string("CameraTakePhoto moved to target in frame '") +
+    //   goal->target.header.frame_id + "'";
+    goal_handle->succeed(result);
+    RCLCPP_INFO(this->get_logger(), "CameraTakePhoto goal succeeded");
+  }
+
+private:
+  std::string action_name_;
+  rclcpp_action::Server<CameraTakePhoto>::SharedPtr server_;
+};
+
+// 主函数：
+// - 初始化 ROS，启动 MotionNode，进入自旋，退出时关闭
+// 测试：
+// ros2 action send_goal --feedback /CameraTakePhoto interfaces/action/CameraTakePhoto "{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<CameraTakePhotoNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
--- a/src/modules/motion/move_leg/CMakeLists.txt
+++ b/src/modules/motion/move_leg/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.8)
+project(move_leg)
+
+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_leg_node src/move_leg_node.cpp)
+target_include_directories(move_leg_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(move_leg_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(move_leg_node rclcpp rclcpp_action interfaces geometry_msgs)
+
+install(TARGETS move_leg_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_leg/package.xml
+++ b/src/modules/motion/move_leg/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_leg</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_leg/src/move_leg_node.cpp
+++ b/src/modules/motion/move_leg/src/move_leg_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_leg.hpp"
+
+using namespace std::chrono_literals;
+
+// MoveLegNode 节点：
+// - 启动一个 MoveLeg 动作服务端（默认话题名称：/move_waist，可通过参数 action_name 修改）
+// - 接受目标后模拟进度反馈，支持取消，并在完成后返回结果
+class MoveLegNode : public rclcpp::Node
+{
+public:
+  using MoveLeg = interfaces::action::MoveLeg;
+  using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
+
+  // 构造函数：
+  // - 声明参数 action_name（默认 "move_leg"，统一使用首字母大写形式避免重复）
+  // - 创建 MoveLeg 动作服务端，并绑定目标处理、取消处理和接收回调
+  MoveLegNode()
+  : Node("move_leg_node")
+  {
+  // Declare a parameter for action name (default: "MoveLeg")
+  action_name_ = this->declare_parameter<std::string>("action_name", "move_leg");
+
+    using std::placeholders::_1;
+    using std::placeholders::_2;
+
+    server_ = rclcpp_action::create_server<MoveLeg>(
+      this->get_node_base_interface(),
+      this->get_node_clock_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      action_name_,
+      std::bind(&MoveLegNode::handle_goal, this, _1, _2),
+      std::bind(&MoveLegNode::handle_cancel, this, _1),
+      std::bind(&MoveLegNode::handle_accepted, this, _1));
+
+    RCLCPP_INFO(
+      this->get_logger(), "MoveLeg 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 MoveLeg::Goal> goal)
+  {
+    (void)uuid;
+    RCLCPP_INFO(this->get_logger(), "Received MoveLeg goal: move_up_distance=%f", goal->move_up_distance);
+    // 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<GoalHandleMoveLeg>/*goal_handle*/)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel MoveLeg goal");
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  // 接受目标回调：
+  // - 收到新的目标后，开启独立线程执行，避免阻塞执行器线程
+  void handle_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+  {
+    // Execute goal in a new thread to avoid blocking executor thread
+    std::thread{std::bind(&MoveLegNode::execute, this, std::placeholders::_1), goal_handle}.detach();
+  }
+
+  // 执行函数：
+  // - 模拟执行流程，按 10% 递增发布反馈(progress)
+  // - 如果检测到取消请求，则返回取消结果
+  // - 正常完成后返回 success=true，并附带简单描述信息
+  void execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+  {
+    const auto goal = goal_handle->get_goal();
+
+    auto feedback = std::make_shared<MoveLeg::Feedback>();
+    auto result = std::make_shared<MoveLeg::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(), "MoveLeg 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(), "MoveLeg progress: %d", i);
+      std::this_thread::sleep_for(100ms);
+    }
+
+    // Complete successfully
+    result->success = true;
+    // result->message = std::string("MoveLeg moved to target in frame '") +
+    //   goal->target.header.frame_id + "'";
+    goal_handle->succeed(result);
+    RCLCPP_INFO(this->get_logger(), "MoveLeg goal succeeded");
+  }
+
+private:
+  std::string action_name_;
+  rclcpp_action::Server<MoveLeg>::SharedPtr server_;
+};
+
+// 主函数：
+// - 初始化 ROS，启动 MotionNode，进入自旋，退出时关闭
+// 测试：
+// ros2 action send_goal --feedback /MoveLeg interfaces/action/MoveLeg "{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<MoveLegNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
--- a/src/modules/motion/move_wheel/CMakeLists.txt
+++ b/src/modules/motion/move_wheel/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.8)
+project(move_wheel)
+
+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_wheel_node src/move_wheel_node.cpp)
+target_include_directories(move_wheel_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(move_wheel_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(move_wheel_node rclcpp rclcpp_action interfaces geometry_msgs)
+
+install(TARGETS move_wheel_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_wheel/package.xml
+++ b/src/modules/motion/move_wheel/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_wheel</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_wheel/src/move_wheel_node.cpp
+++ b/src/modules/motion/move_wheel/src/move_wheel_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_wheel.hpp"
+
+using namespace std::chrono_literals;
+
+// MoveWheelNode 节点：
+// - 启动一个 MoveWheel 动作服务端（默认话题名称：/move_waist，可通过参数 action_name 修改）
+// - 接受目标后模拟进度反馈，支持取消，并在完成后返回结果
+class MoveWheelNode : public rclcpp::Node
+{
+public:
+  using MoveWheel = interfaces::action::MoveWheel;
+  using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveWheel>;
+
+  // 构造函数：
+  // - 声明参数 action_name（默认 "move_wheel"，统一使用首字母大写形式避免重复）
+  // - 创建 MoveWheel 动作服务端，并绑定目标处理、取消处理和接收回调
+  MoveWheelNode()
+  : Node("move_wheel_node")
+  {
+  // Declare a parameter for action name (default: "MoveWheel")
+  action_name_ = this->declare_parameter<std::string>("action_name", "move_wheel");
+
+    using std::placeholders::_1;
+    using std::placeholders::_2;
+
+    server_ = rclcpp_action::create_server<MoveWheel>(
+      this->get_node_base_interface(),
+      this->get_node_clock_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      action_name_,
+      std::bind(&MoveWheelNode::handle_goal, this, _1, _2),
+      std::bind(&MoveWheelNode::handle_cancel, this, _1),
+      std::bind(&MoveWheelNode::handle_accepted, this, _1));
+
+    RCLCPP_INFO(
+      this->get_logger(), "MoveWheel 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 MoveWheel::Goal> goal)
+  {
+    (void)uuid;
+    RCLCPP_INFO(this->get_logger(), "Received MoveWheel goal: move_angle=%f, move_distance=%f", goal->move_angle, goal->move_distance);
+    // 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<GoalHandleMoveLeg>/*goal_handle*/)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel MoveWheel goal");
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  // 接受目标回调：
+  // - 收到新的目标后，开启独立线程执行，避免阻塞执行器线程
+  void handle_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+  {
+    // Execute goal in a new thread to avoid blocking executor thread
+    std::thread{std::bind(&MoveWheelNode::execute, this, std::placeholders::_1), goal_handle}.detach();
+  }
+
+  // 执行函数：
+  // - 模拟执行流程，按 10% 递增发布反馈(progress)
+  // - 如果检测到取消请求，则返回取消结果
+  // - 正常完成后返回 success=true，并附带简单描述信息
+  void execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+  {
+    const auto goal = goal_handle->get_goal();
+
+    auto feedback = std::make_shared<MoveWheel::Feedback>();
+    auto result = std::make_shared<MoveWheel::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(), "MoveWheel 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(), "MoveWheel progress: %d", i);
+      std::this_thread::sleep_for(100ms);
+    }
+
+    // Complete successfully
+    result->success = true;
+    // result->message = std::string("MoveWheel moved to target in frame '") +
+    //   goal->target.header.frame_id + "'";
+    goal_handle->succeed(result);
+    RCLCPP_INFO(this->get_logger(), "MoveWheel goal succeeded");
+  }
+
+private:
+  std::string action_name_;
+  rclcpp_action::Server<MoveWheel>::SharedPtr server_;
+};
+
+// 主函数：
+// - 初始化 ROS，启动 MotionNode，进入自旋，退出时关闭
+// 测试：
+// ros2 action send_goal --feedback /MoveWheel interfaces/action/MoveWheel "{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<MoveWheelNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}
--- a/src/scripts/gen_robot_config.py
+++ b/src/scripts/gen_robot_config.py
@@ -106,10 +106,14 @@ def main() -> None:
        out.write("// Loader and query interface for robot configuration.\n")
        out.write("class RobotConfig {\n")
        out.write(" public:\n")
-        out.write("  RobotConfig() {\n")
+        out.write("  RobotConfig(std::string path = \"\") {\n")
+        out.write("    if (path.empty()) {\n")
        out.write(
-            "    LoadFromFile(ament_index_cpp::get_package_share_directory(\"brain\") + std::string(\"/config/robot_config.yaml\"));\n"
+            "      LoadFromFile(ament_index_cpp::get_package_share_directory(\"brain\") + std::string(\"/config/robot_config.yaml\"));\n"
        )
+        out.write("    } else {\n")
+        out.write("      LoadFromFile(path);\n")
+        out.write("    }\n")
        out.write("  }\n")
        out.write("  // Parse YAML from a file path. Returns true on success.\n")
        out.write("  bool LoadFromFile(const std::string& path) {\n")