brain与vision测试OK

src/brain/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(rclcpp_components REQUIRED)
 # find_package(<dependency> REQUIRED)
 
 find_package(interfaces REQUIRED)
+find_package(brain_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(smacc2 REQUIRED)
 find_package(smacc2_msgs REQUIRED)
@@ -46,6 +47,7 @@ ament_target_dependencies(brain_node
   rclcpp_action
   behaviortree_cpp
   interfaces
+  brain_interfaces
   std_msgs
   smacc2
   smacc2_msgs
@@ -93,6 +95,7 @@ if(BUILD_TESTING)
       rclcpp
       rclcpp_action
       interfaces
+      brain_interfaces
       std_srvs
     )
     target_include_directories(test_action_registry PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
@@ -125,6 +128,7 @@ if(BUILD_TESTING)
       smacc2_msgs
       ament_index_cpp
       interfaces
+      brain_interfaces
       std_srvs
     )
     target_include_directories(test_cerebellum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
@@ -145,6 +149,7 @@ if(BUILD_TESTING)
       rclcpp_action
       behaviortree_cpp
       interfaces
+      brain_interfaces
       smacc2
       smacc2_msgs
       ament_index_cpp
@@ -165,6 +170,7 @@ if(BUILD_TESTING)
       smacc2
       smacc2_msgs
       interfaces
+      brain_interfaces
       std_srvs
     )
     target_include_directories(test_sm_cerebellum PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
@@ -183,6 +189,7 @@ if(BUILD_TESTING)
       rclcpp_action
       behaviortree_cpp
       interfaces
+      brain_interfaces
       smacc2
       smacc2_msgs
       ament_index_cpp
@@ -205,6 +212,7 @@ if(BUILD_TESTING)
       rclcpp_action
       behaviortree_cpp
       interfaces
+      brain_interfaces
       smacc2
       smacc2_msgs
       ament_index_cpp
@@ -225,7 +233,7 @@ if(BUILD_TESTING)
     if(TARGET ${test_name})
       target_compile_definitions(${test_name} PRIVATE BRAIN_DISABLE_SM=1)
       ament_target_dependencies(${test_name}
-        rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp std_srvs)
+        rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp std_srvs)
       target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
       target_link_libraries(${test_name} Boost::thread yaml-cpp)
     endif()
@@ -240,7 +248,7 @@ if(BUILD_TESTING)
   if(TARGET test_cerebrum_utils)
     target_compile_definitions(test_cerebrum_utils PRIVATE BRAIN_DISABLE_SM=1)
     ament_target_dependencies(test_cerebrum_utils
-      rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp std_srvs)
+      rclcpp rclcpp_action behaviortree_cpp interfaces brain_interfaces smacc2 smacc2_msgs ament_index_cpp std_srvs)
     target_include_directories(test_cerebrum_utils PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
     target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp)
   endif()

src/brain/include/brain/cerebellum_node.hpp

@@ -3,7 +3,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/action/execute_bt_action.hpp"
 #include "brain/action_registry.hpp"
 #include "brain/skill_manager.hpp"
 #include <smacc2/smacc_signal_detector.hpp>
@@ -100,7 +100,7 @@ private:
    * @param detail Additional free-form detail (may be empty).
    */
   void PublishFeedbackStage(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & gh,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & gh,
     const std::string & stage,
     const std::string & current_skill,
     float progress,
@@ -120,7 +120,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<interfaces::action::ExecuteBtAction>> & goal_handle);
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
   /**
    * @brief Determine interface kind from first interface entry.
    * Example: some_pkg/DoThing.action => "action".
@@ -142,7 +142,7 @@ private:
     const std::string & topic,
     const std::string & skill,
     float base_progress,
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
     std::string & detail) const;
   /**
    * @brief Execute an action-type skill.
@@ -163,7 +163,7 @@ private:
     std::string & detail,
     int index,
     int total_skills,
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_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.
@@ -182,7 +182,7 @@ private:
     std::chrono::nanoseconds timeout_ns,
     int index,
     int total_skills,
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
     std::string & detail);
 
   /**
@@ -195,7 +195,7 @@ private:
   bool ExecuteServiceSkill(
     const std::string & skill,
     std::string & detail,
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
   /**
    * @brief Record per-skill success/failure counts (thread-safe via mutex).
    * @param skill Skill name.
@@ -211,7 +211,7 @@ private:
    * @param goal_handle Goal handle.
    */
   void RunExecuteBtGoal(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle);
 
   /**
    * @brief Execute the full skill sequence building a CerebellumData::ExecResult summary.
@@ -221,7 +221,7 @@ private:
    * @return CerebellumData::ExecResult aggregated result.
    */
   brain::CerebellumData::ExecResult ExecuteSequence(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
     const std::vector<std::string> & skills);
   // --- Decomposed helpers for RunExecuteBtGoal ---
   /**
@@ -231,7 +231,7 @@ private:
    * @return true if goal is valid; false otherwise (goal aborted internally).
    */
   bool ValidateAndParseGoal(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
     std::vector<std::string> & skills_out) const;
   /**
    * @brief Resolve action topic name for a skill.
@@ -250,7 +250,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<interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
     bool overall_ok,
     int32_t succeeded_skills,
     int32_t total_skills,

src/brain/include/brain/constants.hpp

@@ -14,7 +14,7 @@ namespace brain
 {
 
 /// Canonical action name used for ExecuteBtAction (historical misspelling fixed).
-inline constexpr const char * kExecuteBtActionName = "ExecuteBtAction";
+inline constexpr const char * kExecuteBtActionName = "/execute_bt_action";
 
 /// Suffix appended to skill names to form BT node type identifiers.
 inline constexpr const char * kBtNodeSuffix = "_H";

src/brain/include/brain/skill_manager.hpp

@@ -14,9 +14,10 @@
 #include "interfaces/action/hand_control.hpp"
 #include "interfaces/action/leg_control.hpp"
 #include "interfaces/action/vision_grasp_object.hpp"
-#include "interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
 #include "interfaces/srv/vision_object_recognition.hpp"
 
+#include "brain_interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
+
 namespace brain
 {
 

src/brain/package.xml

@@ -13,6 +13,7 @@
   <depend>rclcpp_components</depend>
   <depend>behaviortree_cpp</depend>
   <depend>interfaces</depend>
+  <depend>brain_interfaces</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>rclcpp_action</depend>
   <depend>smacc2</depend>

src/brain/src/cerebellum_node.cpp

@@ -31,7 +31,7 @@
 #include "interfaces/action/hand_control.hpp"
 #include "interfaces/action/leg_control.hpp"
 #include "interfaces/action/vision_grasp_object.hpp"
-#include "interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
+#include "brain_interfaces/action/execute_bt_action.hpp"  // ExecuteBtAction bridge
 #include "interfaces/srv/vision_object_recognition.hpp"
 #include <std_msgs/msg/string.hpp>
 
@@ -196,7 +196,7 @@ void CerebellumNode::SetupStatsTimerAndPublisher()
  */
 void CerebellumNode::SetupExecuteBtServer()
 {
-  using EBA = interfaces::action::ExecuteBtAction;
+  using EBA = brain_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;
@@ -321,13 +321,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<interfaces::action::ExecuteBtAction>> & gh,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_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<interfaces::action::ExecuteBtAction::Feedback>();
+  auto fb = std::make_shared<brain_interfaces::action::ExecuteBtAction::Feedback>();
   fb->epoch = current_epoch_;
   fb->stage = stage;
   fb->current_skill = current_skill;
@@ -353,7 +353,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<interfaces::action::ExecuteBtAction>> & goal_handle)
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
 {
   if (spec.interfaces.empty()) {
     detail = "No interfaces";
@@ -408,7 +408,7 @@ bool CerebellumNode::WaitForActionServer(
   const std::string & topic,
   const std::string & skill,
   float base_progress,
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
   std::string & detail) const
 {
   constexpr int kMaxAttempts = 8;
@@ -447,7 +447,7 @@ bool CerebellumNode::ExecuteActionSkill(
   std::string & detail,
   int index,
   int total_skills,
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
 {
   (void)spec;  // Currently unused beyond interface kind selection.
   if (!action_clients_->has_client(topic)) {
@@ -487,7 +487,7 @@ bool CerebellumNode::RunActionSkillWithProgress(
   std::chrono::nanoseconds timeout_ns,
   int index,
   int total_skills,
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
   std::string & detail)
 {
   std::atomic<bool> finished{false};
@@ -529,7 +529,7 @@ bool CerebellumNode::RunActionSkillWithProgress(
 bool CerebellumNode::ExecuteServiceSkill(
   const std::string & skill,
   std::string & detail,
-  const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
 {
   if (skill == "VisionObjectRecognition") {
     using Srv = interfaces::srv::VisionObjectRecognition;
@@ -546,7 +546,7 @@ bool CerebellumNode::ExecuteServiceSkill(
     }
     PublishFeedbackStage(goal_handle, "START", skill, 0.f, "");
     auto req = std::make_shared<Srv::Request>();
-    req->object_id = "demo_object";
+    req->camera_position = "left";
     auto future = client->async_send_request(req);
     // NOTE:
     // Do NOT call rclcpp::spin_until_future_complete(this->get_node_base_interface(), ...)
@@ -565,13 +565,43 @@ bool CerebellumNode::ExecuteServiceSkill(
       PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
       return false;
     }
-    if (!resp->found) {
-      detail = "Not found";
+    if (!resp->success) {
+      detail = "Not success";
       PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
       return false;
     }
-    detail = "found pose";
+    detail = "success pose";
     PublishFeedbackStage(goal_handle, "END", skill, 0.f, "OK");
+
+    // Print response (VisionObjectRecognition.srv)
+    if (resp) {
+      const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
+      RCLCPP_INFO(
+        this->get_logger(),
+        "[VisionObjectRecognition] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
+        resp->success, resp->objects.size(), resp->header.frame_id.c_str(), stamp_sec,
+        resp->info.c_str());
+      for (size_t i = 0; i < resp->objects.size(); ++i) {
+        const auto & obj = resp->objects[i];
+        RCLCPP_INFO(
+          this->get_logger(), "  [%zu] class='%s' id=%d poses=%zu", i,
+          obj.class_name.c_str(), obj.class_id, obj.pose_list.size());
+        // Print up to first 2 poses per class to avoid log spam
+        for (size_t j = 0; j < obj.pose_list.size() && j < 2; ++j) {
+          const auto & p = obj.pose_list[j];
+            RCLCPP_INFO(
+              this->get_logger(),
+              "    pose[%zu]: pos(%.3f, %.3f, %.3f) quat(%.3f, %.3f, %.3f, %.3f)", j,
+              p.position.x, p.position.y, p.position.z,
+              p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
+        }
+        if (obj.pose_list.size() > 2) {
+          RCLCPP_INFO(
+            this->get_logger(), "    ... (%zu more poses omitted)", obj.pose_list.size() - 2);
+        }
+      }
+    }
+
     return true;
   }
   detail = "Unsupported service skill";
@@ -635,7 +665,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<interfaces::action::ExecuteBtAction>> & goal_handle)
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle)
 {
   std::vector<std::string> skills;
   if (!ValidateAndParseGoal(goal_handle, skills)) {
@@ -650,7 +680,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 = interfaces::action::ExecuteBtAction;
+      using EBA = brain_interfaces::action::ExecuteBtAction;
       auto res = std::make_shared<EBA::Result>();
       res->success = d.last_success.load();
       res->total_skills = d.last_total_skills;
@@ -687,7 +717,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<interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
   const std::vector<std::string> & skills)
 {
   brain::CerebellumData::ExecResult result;
@@ -753,8 +783,8 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
     if (loop_index + 1 < skills.size()) summary << ", ";
     ++loop_index;
     if (!ok) {
-      RCLCPP_ERROR(this->get_logger(), "Skill %s failed%s", skill.c_str(),
-        abort_on_failure_ ? ", abort sequence" : " (continue)");
+      RCLCPP_ERROR(this->get_logger(), "Skill %s failed%s, detail: %s", skill.c_str(),
+        abort_on_failure_ ? ", abort sequence" : " (continue)", detail.c_str());
       if (abort_on_failure_) break;
     }
   }
@@ -778,10 +808,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<interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
   std::vector<std::string> & skills_out) const
 {
-  using EBA = interfaces::action::ExecuteBtAction;
+  using EBA = brain_interfaces::action::ExecuteBtAction;
   auto logger = this->get_logger();
   const auto goal = goal_handle->get_goal();
   if (!goal) {
@@ -833,13 +863,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<interfaces::action::ExecuteBtAction>> & goal_handle,
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<brain_interfaces::action::ExecuteBtAction>> & goal_handle,
   bool overall_ok,
   int32_t succeeded_skills,
   int32_t total_skills,
   const std::ostringstream & summary_stream)
 {
-  using EBA = interfaces::action::ExecuteBtAction;
+  using EBA = brain_interfaces::action::ExecuteBtAction;
   auto res = std::make_shared<EBA::Result>();
   res->success = overall_ok;
   res->message = summary_stream.str();

src/brain/src/cerebrum_node.cpp

@@ -23,7 +23,7 @@
 
 // Project headers
 #include "brain/constants.hpp"
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/action/execute_bt_action.hpp"
 
 using namespace std::chrono_literals; // NOLINT
 
@@ -215,7 +215,7 @@ void CerebrumNode::SendExecuteBt(const std::string & bt_action_name)
  */
 void CerebrumNode::RegisterActionClient()
 {
-  using EBA = interfaces::action::ExecuteBtAction;
+  using EBA = brain_interfaces::action::ExecuteBtAction;
   if (registered_actions_.count(brain::kExecuteBtActionName)) {
     return;
   }

src/brain/src/skill_manager.cpp

@@ -242,9 +242,16 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
     const std::string &)>> service_registrars = {
     {"VisionObjectRecognition",
       [this](const std::string & topic, const std::string & internal_skill) {
-        (void)internal_skill;
+        // NOTE: Previously the client was only stored under the snake_case topic key.
+        // get_typed_service_client() queries by canonical skill name (CamelCase), which
+        // caused lookups like "VisionObjectRecognition" to fail (nullptr) and produced
+        // "Service client missing" at runtime. We now register under both the canonical
+        // skill name and the resolved topic alias to remain backward compatible.
         auto client = node_->create_client<VisionObjectRecognition>(topic);
         service_clients_.push_back(client);
+        // Store by canonical skill name
+        service_client_map_[internal_skill] = client;
+        // Also store by topic alias (snake_case) in case other code paths use it directly.
         service_client_map_[topic] = client;
       }},
 
@@ -256,9 +263,10 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
       RCLCPP_WARN(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
       continue;
     }
-    // Strategy: use the skill's CamelCase name directly as the action/service topic.
-    const std::string topic_name = s.name;
-    if (parsed->kind == "action") {
+  // Strategy updated: publish/resolve topics using snake_case form with leading slash.
+  // Example: VisionObjectRecognition -> vision_object_recognition
+  std::string topic_name = to_snake_case(s.name);
+  if (parsed->kind == "action") {
       auto it = action_registrars.find(parsed->base);
       if (it != action_registrars.end()) {
         it->second(topic_name, s.name);

src/brain/test/test_cerebellum_node.cpp

@@ -9,9 +9,9 @@
 #include "brain/cerebellum_node.hpp"
 // Legacy MoveToPosition action was removed from automatic server startup.
 // Adapt test to validate that CerebellumNode brings up the unified ExecuteBtAction server.
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/action/execute_bt_action.hpp"
 
-using ExecuteBtAction = interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
 
 TEST(CerebellumNode, StartsAndProvidesActionServer)
 {

src/brain/test/test_execute_bt_action.cpp

@@ -6,10 +6,10 @@
 #include <rclcpp_action/rclcpp_action.hpp>
 
 #include "brain/cerebellum_node.hpp"
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 
-using ExecuteBtAction = interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;
 
 namespace

src/brain/test/test_execute_bt_action_cancel.cpp

@@ -3,10 +3,10 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "brain/cerebellum_node.hpp"
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 
-using ExecuteBtAction = interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;
 
 namespace

src/brain/test/test_execute_bt_action_extended.cpp

@@ -8,12 +8,12 @@
 // no stats topic usage here
 
 #include "brain/cerebellum_node.hpp"
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_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 = interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;
 using HandControl = interfaces::action::HandControl;
 using LegControl = interfaces::action::LegControl;

src/brain/test/test_execute_bt_action_run_interp.cpp

@@ -3,10 +3,10 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "brain/cerebellum_node.hpp"
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 
-using ExecuteBtAction = interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;
 
 namespace

src/brain/test/test_execute_bt_action_stats.cpp

@@ -4,10 +4,10 @@
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <std_msgs/msg/string.hpp>
 #include "brain/cerebellum_node.hpp"
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 
-using ExecuteBtAction = interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;
 
 namespace

src/brain/test/test_execute_bt_action_timeout_continue.cpp

@@ -3,11 +3,11 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "brain/cerebellum_node.hpp"
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 #include "interfaces/action/hand_control.hpp"
 
-using ExecuteBtAction = interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;
 using HandControl = interfaces::action::HandControl;
 

src/brain/test/test_execute_bt_action_unsupported.cpp

@@ -3,10 +3,10 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include "brain/cerebellum_node.hpp"
-#include "interfaces/action/execute_bt_action.hpp"
+#include "brain_interfaces/action/execute_bt_action.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 
-using ExecuteBtAction = interfaces::action::ExecuteBtAction;
+using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
 using ArmSpaceControl = interfaces::action::ArmSpaceControl;
 
 namespace

src/brain_interfaces/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(interfaces)
+project(brain_interfaces)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -14,20 +14,16 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
+find_package(interfaces REQUIRED)
 
 set(msg_files
   msg/Movej.msg
 )
 set(action_files
-  action/MoveToPosition.action
-  action/ArmSpaceControl.action
-  action/HandControl.action
-  action/LegControl.action
-  action/VisionGraspObject.action
   action/ExecuteBtAction.action
 )
 set(srv_files
-  srv/VisionObjectRecognition.srv
+  srv/VisionDemo.srv
 )
 
 # uncomment the following section in order to fill in
@@ -42,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     std_msgs
     geometry_msgs
     builtin_interfaces
+    interfaces
 )
 
 if(BUILD_TESTING)

src/brain_interfaces/package.xml

@@ -1,7 +1,7 @@
 <?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>interfaces</name>
+  <name>brain_interfaces</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="root@todo.todo">root</maintainer>
@@ -11,6 +11,7 @@
   <build_depend>rosidl_default_generators</build_depend>
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>interfaces</depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/brain_interfaces/srv/VisionDemo.srv

@@ -0,0 +1,8 @@
+string camera_position
+
+---
+
+std_msgs/Header header
+string info
+bool success
+interfaces/PoseClassAndID[] objects

src/interfaces/action/ArmSpaceControl.action

@@ -1,10 +0,0 @@
-# Goal: target pose in space
-# Example fields; adjust as needed for your robot
-geometry_msgs/PoseStamped target
----
-# Result
-bool success
-string message
----
-# Feedback
-float32 progress

src/interfaces/action/HandControl.action

@@ -1,8 +0,0 @@
-# Goal: hand control parameters
-int32 mode  # 0=open, 1=close
-float32 effort
----
-bool success
-string message
----
-float32 progress

src/interfaces/action/LegControl.action

@@ -1,7 +0,0 @@
-# Goal: leg control parameters
-geometry_msgs/TwistStamped target
----
-bool success
-string message
----
-float32 progress

src/interfaces/action/MoveToPosition.action

@@ -1,12 +0,0 @@
-# Goal definition
-float32 target_x
-float32 target_y
----
-# Result definition
-bool success
-string message
----
-# Feedback definition
-float32 current_x
-float32 current_y
-float32 progress

src/interfaces/action/VisionGraspObject.action

@@ -1,7 +0,0 @@
-# Goal: specify object id/name
-string object_id
----
-bool success
-string message
----
-float32 grasp_progress

src/interfaces/srv/VisionObjectRecognition.srv

@@ -1,6 +0,0 @@
-# Request: specify object name or class
-string object_id
----
-# Response: detection result
-bool found
-geometry_msgs/PoseStamped pose

src/modules/motion/arm_control/src/arm_control_node.cpp

@@ -31,7 +31,7 @@ public:
   : Node("motion_node")
   {
   // Declare a parameter for action name (default: "ArmSpaceControl")
-  action_name_ = this->declare_parameter<std::string>("action_name", "ArmSpaceControl");
+  action_name_ = this->declare_parameter<std::string>("action_name", "arm_space_control");
 
     using std::placeholders::_1;
     using std::placeholders::_2;

src/modules/motion/hand_control/src/hand_control_node.cpp

@@ -21,7 +21,7 @@ public:
     using namespace std::placeholders;
     server_ = rclcpp_action::create_server<HandControl>(
       this,
-      "HandControl",
+      "hand_control",
       std::bind(&HandControlNode::handle_goal, this, _1, _2),
       std::bind(&HandControlNode::handle_cancel, this, _1),
       std::bind(&HandControlNode::handle_accepted, this, _1));

src/modules/motion/leg_control/src/leg_control_node.cpp

@@ -16,11 +16,11 @@ public:
     using namespace std::placeholders;
     action_server_ = rclcpp_action::create_server<LegControl>(
       this,
-      "LegControl",  // 话题/动作名称，与技能 snake_case 一致
+      "leg_control",  // 话题/动作名称，与技能 snake_case 一致
       std::bind(&LegControlServer::handle_goal, this, _1, _2),
       std::bind(&LegControlServer::handle_cancel, this, _1),
       std::bind(&LegControlServer::handle_accepted, this, _1));
-    RCLCPP_INFO(get_logger(), "LegControl action server ready on /LegControl");
+    RCLCPP_INFO(get_logger(), "LegControl action server ready on /leg_control");
   }
 
 private:

src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp

@@ -1,9 +1,18 @@
 // vision_object_recg_node.cpp
 // 提供 VisionObjectRecognition.srv 服务，实现简单的目标识别逻辑
 
+// ROS2 core
 #include <rclcpp/rclcpp.hpp>
+
+// Service and custom msg
 #include "interfaces/srv/vision_object_recognition.hpp"
+#include "interfaces/msg/pose_class_and_id.hpp"
+
+// Std / geometry msgs
+#include <std_msgs/msg/header.hpp>
+#include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <sstream>
 
 using interfaces::srv::VisionObjectRecognition;
 
@@ -12,43 +21,76 @@ public:
   VisionObjectRecgNode(): Node("vision_object_recg") {
     // 服务名称与 SkillManager 中 to_snake_case("VisionObjectRecognition") 保持一致: VisionObjectRecognition
     service_ = create_service<VisionObjectRecognition>(
-      "VisionObjectRecognition",
+      "vision_object_recognition",
       std::bind(&VisionObjectRecgNode::handle_service, this, std::placeholders::_1, std::placeholders::_2));
-    RCLCPP_INFO(get_logger(), "VisionObjectRecognition service server ready on /VisionObjectRecognition");
+    RCLCPP_INFO(get_logger(), "VisionObjectRecognition service server ready on /vision_object_recognition");
   }
 
 private:
   void handle_service(const std::shared_ptr<VisionObjectRecognition::Request> req,
                       std::shared_ptr<VisionObjectRecognition::Response> res) {
-    RCLCPP_INFO(get_logger(), "Received recognition request for object_id='%s'", req->object_id.c_str());
+    RCLCPP_INFO(get_logger(), "Received recognition request for camera_position='%s'", req->camera_position.c_str());
 
-    // 这里放置实际的识别逻辑。当前示例: 当 object_id 非空且不是 'unknown' 时假设识别成功。
-    if (!req->object_id.empty() && req->object_id != "unknown") {
-      res->found = true;
-      // 构造一个示例位姿 (可根据实际视觉结果替换)
-      res->pose.header.stamp = now();
-      res->pose.header.frame_id = "map";  // 或 camera_link/base_link 等
-      res->pose.pose.position.x = 1.0;
-      res->pose.pose.position.y = 0.2;
-      res->pose.pose.position.z = 0.8;
-      res->pose.pose.orientation.w = 1.0; // 单位四元数
-      RCLCPP_INFO(get_logger(), "Object '%s' found at (%.2f, %.2f, %.2f)", req->object_id.c_str(),
-                  res->pose.pose.position.x,
-                  res->pose.pose.position.y,
-                  res->pose.pose.position.z);
+    // 填充 header
+    res->header.stamp = now();
+    // 若调用方传 camera_position 作为一个逻辑位姿来源，可作为 frame_id；否则给一个默认
+    res->header.frame_id = req->camera_position.empty() ? "camera_link" : ("camera_" + req->camera_position);
+
+    // 简单逻辑: 只要 camera_position 非空且不是 unknown 就认为识别到两个目标
+    if (!req->camera_position.empty() && req->camera_position != "unknown") {
+      res->success = true;
+
+      // 伪造两类目标 (示例: cup / bottle)，每类给出 1~2 个 Pose
+      interfaces::msg::PoseClassAndID obj1;
+      obj1.class_name = "cup";
+      obj1.class_id = 1;
+
+      geometry_msgs::msg::Pose pose1; // 基本位姿
+      pose1.position.x = 0.3;
+      pose1.position.y = 0.1;
+      pose1.position.z = 0.8;
+      pose1.orientation.x = 0.0;
+      pose1.orientation.y = 0.0;
+      pose1.orientation.z = 0.0;
+      pose1.orientation.w = 1.0;
+      obj1.pose_list.push_back(pose1);
+
+      // 第二个同类实例(示例)
+      geometry_msgs::msg::Pose pose1b = pose1;
+      pose1b.position.y += 0.05;
+      obj1.pose_list.push_back(pose1b);
+
+      interfaces::msg::PoseClassAndID obj2;
+      obj2.class_name = "bottle";
+      obj2.class_id = 2;
+      geometry_msgs::msg::Pose pose2;
+      pose2.position.x = 0.5;
+      pose2.position.y = -0.2;
+      pose2.position.z = 0.78;
+      pose2.orientation.w = 1.0;
+      obj2.pose_list.push_back(pose2);
+
+      res->objects.clear();
+      res->objects.emplace_back(obj1);
+      res->objects.emplace_back(obj2);
+
+      std::ostringstream oss;
+      oss << "Recognized " << res->objects.size() << " object classes (total instances: "
+          << (obj1.pose_list.size() + obj2.pose_list.size()) << ")";
+      res->info = oss.str();
     } else {
-      res->found = false;
-      res->pose = geometry_msgs::msg::PoseStamped();
-      res->pose.header.stamp = now();
-      res->pose.header.frame_id = "map";
-      RCLCPP_WARN(get_logger(), "Object '%s' not found", req->object_id.c_str());
+      res->success = false;
+      res->objects.clear();
+      res->info = "No objects recognized: invalid or unknown camera_position.";
     }
+
+    RCLCPP_INFO(get_logger(), "Response: success=%d, info=%s, classes=%zu", res->success, res->info.c_str(), res->objects.size());
   }
 
   rclcpp::Service<VisionObjectRecognition>::SharedPtr service_;
 };
 
-//测试： ros2 service call /vision_object_recognition interfaces/srv/VisionObjectRecognition '{object_id: "cup"}'
+//ros2 service call /vision_object_recognition interfaces/srv/VisionObjectRecognition "{camera_position: 'left'}"
 
 int main(int argc, char ** argv) {
   rclcpp::init(argc, argv);

src/scripts/build.sh

@@ -1,4 +1,4 @@
 #!/bin/bash
 
-colcon build --packages-select interfaces smacc2_msgs smacc2 brain
-#colcon build --packages-select interfaces smacc2_msgs smacc2 brain arm_control leg_control vision_object_recg hand_control
+colcon build --packages-select brain_interfaces smacc2_msgs smacc2 brain
+#colcon build --packages-select brain_interfaces smacc2_msgs smacc2 brain arm_control leg_control vision_object_recg hand_control