add bt_carry_boxes.xml

src/brain/config/bt_carry_boxes.xml

@@ -0,0 +1,25 @@
+<root BTCPP_format="4">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <!-- Retry waist + arm + hand until HandControl_H succeeds -->
+      <RetryUntilSuccessful name="retry_all_action" num_attempts="5">
+        <Sequence>
+          <WaistControl_H name="s1_waist_bend_down" /> <!--pitch w rad -->
+          <Arm_H name="s2_arm_stretch_out" />
+          <HandControl_H name="s3_hand_pickup" />
+          <Arm_H name="s4_arm_lift" />
+          <WaistControl_H name="s5_waist_bend_up" />
+          <WaistControl_H name="s6_waist_turn_around" />
+          <LegControl_H name="s7_leg_move_back" />
+          <WaistControl_H name="s8_waist_bend_down" />
+          <Arm_H name="s9_arm_stretch_out" />
+          <HandControl_H name="s10_hand_release" />
+          <Arm_H name="s11_arm_move_to_snapshot_pose" />
+          <CameraTakePhoto_H name="s12_camera_take_photo" />
+          <WaistControl_H name="s13_waist_bend_up" />
+          <Arm_H name="s14_arm_retract" />
+        </Sequence>
+      </RetryUntilSuccessful>      
+    </Sequence>
+  </BehaviorTree>
+</root>

src/brain/config/robot_skills.yaml

@@ -16,6 +16,18 @@
   interfaces:
     - Arm.action
 
+- name: WaistControl
+  version: 1.0.0
+  description: "腰部控制"
+  interfaces:
+    - WaistControl.action
+
+- name: CameraTakePhoto
+  version: 1.0.0
+  description: "相机拍照"
+  interfaces:
+    - CameraTakePhoto.action
+
 - name: HandControl
   version: 1.0.0
   description: "手部控制"

src/brain/include/brain/skill_manager.hpp

@@ -22,6 +22,8 @@
 #include "interfaces/action/arm.hpp"
 #include "interfaces/action/slam_mode.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "interfaces/action/camera_take_photo.hpp"
+#include "interfaces/action/waist_control.hpp"
 
 namespace brain
 {
@@ -360,6 +362,20 @@ struct SkillActionTrait<interfaces::action::HandControl>
   static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
 };
 template<>
+struct SkillActionTrait<interfaces::action::CameraTakePhoto>
+{
+  static constexpr const char * skill_name = "CameraTakePhoto";
+  static bool success(const interfaces::action::CameraTakePhoto::Result & r) {return r.success;}
+  static std::string message(const interfaces::action::CameraTakePhoto::Result & r) {return r.message;}
+};
+template<>
+struct SkillActionTrait<interfaces::action::WaistControl>
+{
+  static constexpr const char * skill_name = "WaistControl";
+  static bool success(const interfaces::action::WaistControl::Result & r) {return r.success;}
+  static std::string message(const interfaces::action::WaistControl::Result & r) {return r.message;}
+};
+template<>
 struct SkillActionTrait<interfaces::action::LegControl>
 {
   static constexpr const char * skill_name = "LegControl";
@@ -419,7 +435,9 @@ using SkillActionTypes = std::tuple<
   interfaces::action::LegControl,
   interfaces::action::VisionGraspObject,
   interfaces::action::SlamMode,
-  nav2_msgs::action::NavigateToPose
+  nav2_msgs::action::NavigateToPose,
+  interfaces::action::CameraTakePhoto,
+  interfaces::action::WaistControl
 >;
 
 /**

src/brain/src/cerebellum_node.cpp

@@ -75,9 +75,9 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
   skill_manager_ = std::make_unique<SkillManager>(this, action_clients_.get(), nullptr);
 
   DeclareAndLoadParameters();
+  LoadSkillsFile();
   ConfigureActionHooks();
   ConfigureServiceHooks();
-  LoadSkillsFile();
   SetupStatsTimerAndPublisher();
   SetupExecuteBtServer();
 

src/brain/src/cerebrum_node.cpp

@@ -82,7 +82,7 @@ struct UpdatingFlagGuard
  */
 CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("cerebrum_node", options),
-  bt_config_file_path_(ament_index_cpp::get_package_share_directory("brain") + std::string("/config/bt_vision_grasp.xml"))
+  bt_config_file_path_(ament_index_cpp::get_package_share_directory("brain") + std::string("/config/bt_carry_boxes.xml"))
 {
   InitializeRegistries();
   DeclareParameters();

src/brain/src/skill_manager.cpp

@@ -25,6 +25,8 @@
 #include "interfaces/srv/map_load.hpp"
 #include "interfaces/action/arm.hpp"
 #include "nav2_msgs/action/navigate_to_pose.hpp"
+#include "interfaces/action/waist_control.hpp"
+#include "interfaces/action/camera_take_photo.hpp"
 
 using interfaces::action::ArmSpaceControl;
 using interfaces::action::Arm;
@@ -32,6 +34,8 @@ using interfaces::action::HandControl;
 using interfaces::action::LegControl;
 using interfaces::action::VisionGraspObject;
 using interfaces::srv::VisionObjectRecognition;
+using interfaces::action::CameraTakePhoto;
+using interfaces::action::WaistControl;
 
 namespace brain
 {
@@ -173,6 +177,12 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
     {"HandControl", [this](const std::string & topic, const std::string & internal_skill) {
         register_action_client_default<HandControl>("HandControl", topic, internal_skill);
       }},
+    {"WaistControl", [this](const std::string & topic, const std::string & internal_skill) {
+        register_action_client_default<WaistControl>("WaistControl", topic, internal_skill);
+      }},
+    {"CameraTakePhoto", [this](const std::string & topic, const std::string & internal_skill) {
+        register_action_client_default<CameraTakePhoto>("CameraTakePhoto", topic, internal_skill);
+      }},
     {"LegControl", [this](const std::string & topic, const std::string & internal_skill) {
         register_action_client_default<LegControl>("LegControl", topic, internal_skill);
       }},