payload transfer between cc test ok

src/brain/include/brain/cerebellum_node.hpp

@@ -116,9 +116,10 @@ private:
         try {
           auto node = YAML::Load(c.payload_yaml);
           out = brain::from_yaml<T>(node);
+          RCLCPP_INFO(rclcpp::Node::get_logger(), "[%s] payload_yaml parse success", skill_name.c_str());
           return true;
         } catch (const std::exception & e) {
-          RCLCPP_WARN(rclcpp::Node::get_logger(), "[%s] payload_yaml parse failed: %s",
+          RCLCPP_ERROR(rclcpp::Node::get_logger(), "[%s] payload_yaml parse failed: %s",
             skill_name.c_str(), e.what());
         }
         break;

src/brain/src/cerebellum_node.cpp

@@ -76,9 +76,11 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
   skill_manager_ = std::make_unique<SkillManager>(this, action_clients_.get(), nullptr);
 
   DeclareAndLoadParameters();
-  LoadSkillsFile();
+  // Register action/service hook overrides first so SkillManager will pick them up
   ConfigureActionHooks();
   ConfigureServiceHooks();
+  // Then load skills which will register action/service clients and honor the overrides
+  LoadSkillsFile();
   SetupStatsTimerAndPublisher();
   SetupExecuteBtServer();
 
@@ -835,8 +837,19 @@ void CerebellumNode::SetupExecuteBtServer()
       if (sm_exec_ && sm_exec_->signalDetector) {
         sm_exec_->signalDetector->postEvent(new brain::EvGoalReceived());
       }
-      RCLCPP_INFO(
-        this->get_logger(), "ExecuteBtAction GoalReceived goal: %s", goal->action_name.c_str());
+      // Log detailed goal contents to help debugging call propagation from Cerebrum
+      RCLCPP_INFO(this->get_logger(), "ExecuteBtAction GoalReceived epoch=%u action_name=%s calls_count=%zu",
+        goal->epoch, goal->action_name.c_str(), goal->calls.size());
+      for (size_t i = 0; i < goal->calls.size(); ++i) {
+        const auto & c = goal->calls[i];
+        RCLCPP_INFO(this->get_logger(), "  call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
+          i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
+        if (!c.payload_yaml.empty()) {
+          // Log payload content (trim if very long)
+          const std::string payload = c.payload_yaml.size() > 512 ? c.payload_yaml.substr(0, 512) + "..." : c.payload_yaml;
+          RCLCPP_INFO(this->get_logger(), "    payload: %s", payload.c_str());
+        }
+      }
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
   // Accept cancel requests: set runtime preempt flag; the execute loop will notice and stop
@@ -1342,6 +1355,16 @@ void CerebellumNode::RunExecuteBtGoal(
   // Cache calls for this goal (Plan A minimal support)
   last_calls_.clear();
   for (const auto & c : goal_handle->get_goal()->calls) { last_calls_.push_back(c); }
+  // Log cached calls for verification
+  if (!last_calls_.empty()) {
+    std::ostringstream oss;
+    oss << "Cached last_calls_ (count=" << last_calls_.size() << "): ";
+    for (size_t i = 0; i < last_calls_.size(); ++i) {
+      if (i) oss << ", ";
+      oss << last_calls_[i].name;
+    }
+    RCLCPP_INFO(this->get_logger(), "%s", oss.str().c_str());
+  }
   // Install execute_fn & complete_cb into runtime so state machine owns lifecycle.
   auto & d = brain::runtime();
   d.execute_fn = [this, goal_handle, skills](rclcpp::Logger logger) -> brain::CerebellumData::ExecResult {

src/brain/src/cerebrum_node.cpp

@@ -168,6 +168,13 @@ void CerebrumNode::RegisterActionClient()
           g.calls.push_back(BuildSkillCallForSkill(s));
         }
       }
+      // Debug: log outgoing calls being sent to Cerebellum
+      RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal with calls_count=%zu", g.calls.size());
+      for (size_t i = 0; i < g.calls.size(); ++i) {
+        const auto & c = g.calls[i];
+        RCLCPP_INFO(this->get_logger(), "  out_call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
+          i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
+      }
       RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction]Sending goal epoch=%u skills=%s",
         g.epoch, g.action_name.c_str());
       return g;

src/modules/motion/move_waist/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.8)
+project(move_waist)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(move_waist_node src/move_waist_node.cpp)
+target_include_directories(move_waist_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(move_waist_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+ament_target_dependencies(move_waist_node rclcpp rclcpp_action interfaces geometry_msgs)
+
+install(TARGETS move_waist_node
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/modules/motion/move_waist/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>move_waist</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>interfaces</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/modules/motion/move_waist/src/move_waist_node.cpp

@@ -0,0 +1,136 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "interfaces/action/move_waist.hpp"
+
+using namespace std::chrono_literals;
+
+// MoveWaistNode 节点：
+// - 启动一个 MoveWaist 动作服务端（默认话题名称：/move_waist，可通过参数 action_name 修改）
+// - 接受目标后模拟进度反馈，支持取消，并在完成后返回结果
+class MoveWaistNode : public rclcpp::Node
+{
+public:
+  using MoveWaist = interfaces::action::MoveWaist;
+  using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
+
+  // 构造函数：
+  // - 声明参数 action_name（默认 "move_waist"，统一使用首字母大写形式避免重复）
+  // - 创建 MoveWaist 动作服务端，并绑定目标处理、取消处理和接收回调
+  MoveWaistNode()
+  : Node("move_waist_node")
+  {
+  // Declare a parameter for action name (default: "MoveWaist")
+  action_name_ = this->declare_parameter<std::string>("action_name", "move_waist");
+
+    using std::placeholders::_1;
+    using std::placeholders::_2;
+
+    server_ = rclcpp_action::create_server<MoveWaist>(
+      this->get_node_base_interface(),
+      this->get_node_clock_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      action_name_,
+      std::bind(&MoveWaistNode::handle_goal, this, _1, _2),
+      std::bind(&MoveWaistNode::handle_cancel, this, _1),
+      std::bind(&MoveWaistNode::handle_accepted, this, _1));
+
+    RCLCPP_INFO(
+      this->get_logger(), "MoveWaist action server started on '%s'",
+      action_name_.c_str());
+  }
+
+private:
+  // 目标处理回调：
+  // - 参数 uuid：目标的唯一标识
+  // - 参数 goal：ArmSpaceControl 的目标数据（target pose）
+  // - 返回值：是否接受目标，这里直接接受并执行
+  rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const MoveWaist::Goal> goal)
+  {
+    (void)uuid;
+    RCLCPP_INFO(
+      this->get_logger(), "Received MoveWaist goal: move_pitch_degree=%f, move_yaw_degree=%f",
+      goal->move_pitch_degree, goal->move_yaw_degree);
+    // Accept all goals for now. You can validate mode/effort here.
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  // 取消处理回调：
+  // - 当客户端请求取消时被调用，这里统一接受取消请求
+  rclcpp_action::CancelResponse handle_cancel(
+    const std::shared_ptr<GoalHandleMoveWaist>/*goal_handle*/)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel MoveWaist goal");
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  // 接受目标回调：
+  // - 收到新的目标后，开启独立线程执行，避免阻塞执行器线程
+  void handle_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+  {
+    // Execute goal in a new thread to avoid blocking executor thread
+    std::thread{std::bind(&MoveWaistNode::execute, this, std::placeholders::_1), goal_handle}.detach();
+  }
+
+  // 执行函数：
+  // - 模拟执行流程，按 10% 递增发布反馈(progress)
+  // - 如果检测到取消请求，则返回取消结果
+  // - 正常完成后返回 success=true，并附带简单描述信息
+  void execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+  {
+    const auto goal = goal_handle->get_goal();
+
+    auto feedback = std::make_shared<MoveWaist::Feedback>();
+    auto result = std::make_shared<MoveWaist::Result>();
+
+    // Simulate progress from 0 to 100 with small delay; react to cancel requests
+    for (int i = 0; i <= 100; i += 10) {
+      if (goal_handle->is_canceling()) {
+        result->success = false;
+        goal_handle->canceled(result);
+        RCLCPP_INFO(this->get_logger(), "MoveWaist goal canceled");
+        return;
+      }
+      // feedback->progress = static_cast<float>(i);
+      // feedback->command_id = goal->command_id;
+      // feedback->int_lenth = i;
+      goal_handle->publish_feedback(feedback);
+      RCLCPP_INFO(this->get_logger(), "MoveWaist progress: %d", i);
+      std::this_thread::sleep_for(100ms);
+    }
+
+    // Complete successfully
+    result->success = true;
+    // result->message = std::string("MoveWaist moved to target in frame '") +
+    //   goal->target.header.frame_id + "'";
+    goal_handle->succeed(result);
+    RCLCPP_INFO(this->get_logger(), "MoveWaist goal succeeded");
+  }
+
+private:
+  std::string action_name_;
+  rclcpp_action::Server<MoveWaist>::SharedPtr server_;
+};
+
+// 主函数：
+// - 初始化 ROS，启动 MotionNode，进入自旋，退出时关闭
+// 测试：
+// ros2 action send_goal --feedback /MoveWaist interfaces/action/MoveWaist "{target: {header: {frame_id: base_link}, pose: {position: {x: 0.5, y: 0.0, z: 0.3}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}}}"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<MoveWaistNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}