chore: upgrade hivecore logger SDK from 1.0.0 to 1.0.1

- Update hivecore_logger_cpp_DIR and include paths in brain and smacc2
  CMakeLists.txt to reference SDK version 1.0.1
- Fix unused parameter warning in CbRosStop2 constructor by casting
  launchPid to void

.gitignore

@@ -1,6 +1,7 @@
 build/
 install/
 log/
+logs/
 
 .vscode/
 .venv/

src/brain/CMakeLists.txt

@@ -8,13 +8,16 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+
+# hivecore logger SDK
+set(hivecore_logger_cpp_DIR "/opt/hivecore/logger-sdk/1.0.1/lib/cmake/hivecore_logger_cpp")
+find_package(hivecore_logger_cpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # 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)
@@ -35,13 +38,18 @@ find_package(tf2_geometry_msgs REQUIRED)
 add_executable(brain_node
   src/brain_node.cpp
   src/cerebellum_node.cpp
+  src/cerebellum_hooks.cpp
   src/cerebrum_node.cpp
   src/skill_manager.cpp
+  src/calc_grasp_pose.cpp
 )
 
 target_include_directories(brain_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
+  $<INSTALL_INTERFACE:include>
+  interfaces/include
+  /opt/hivecore/logger-sdk/1.0.1/include
+)
 
 target_compile_features(brain_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 
@@ -51,7 +59,6 @@ ament_target_dependencies(brain_node
   rclcpp_action
   behaviortree_cpp
   interfaces
-  brain_interfaces
   std_msgs
   smacc2
   smacc2_msgs
@@ -68,6 +75,7 @@ ament_target_dependencies(brain_node
 target_link_libraries(brain_node
   Boost::thread
   yaml-cpp
+  hivecore_logger_cpp::hivecore_logger_cpp
 )
 
 install(TARGETS brain_node
@@ -76,6 +84,11 @@ install(TARGETS brain_node
 install(DIRECTORY config/
   DESTINATION share/${PROJECT_NAME}/config)
 
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch)
+
+set (BUILD_TESTING OFF)
+
 if(BUILD_TESTING)
   if(NOT DEFINED ENV{SKIP_LINT})
     find_package(ament_lint_auto REQUIRED)
@@ -103,7 +116,6 @@ if(BUILD_TESTING)
       rclcpp
       rclcpp_action
       interfaces
-      brain_interfaces
       nav2_msgs
       std_srvs
       tf2
@@ -158,14 +170,15 @@ if(BUILD_TESTING)
       smacc2_msgs
       ament_index_cpp
       interfaces
-      brain_interfaces
       nav2_msgs
       std_srvs
       tf2
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_cerebellum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_include_directories(test_cerebellum_node PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+    )
     target_link_libraries(test_cerebellum_node Boost::thread)
     target_link_libraries(test_cerebellum_node yaml-cpp)
   endif()
@@ -183,7 +196,6 @@ if(BUILD_TESTING)
       rclcpp_action
       behaviortree_cpp
       interfaces
-      brain_interfaces
       smacc2
       smacc2_msgs
       ament_index_cpp
@@ -193,7 +205,9 @@ if(BUILD_TESTING)
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_cerebrum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_include_directories(test_cerebrum_node PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+    )
     target_link_libraries(test_cerebrum_node Boost::thread)
     target_link_libraries(test_cerebrum_node yaml-cpp)
   endif()
@@ -208,14 +222,16 @@ if(BUILD_TESTING)
       smacc2
       smacc2_msgs
       interfaces
-      brain_interfaces
       nav2_msgs
       std_srvs
       tf2
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_sm_cerebellum PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_include_directories(test_sm_cerebellum PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+    )
+    target_link_libraries(test_sm_cerebellum)
   endif()
 
   # ExecuteBtAction end-to-end single skill test
@@ -231,7 +247,6 @@ if(BUILD_TESTING)
       rclcpp_action
       behaviortree_cpp
       interfaces
-      brain_interfaces
       smacc2
       smacc2_msgs
       ament_index_cpp
@@ -241,7 +256,9 @@ if(BUILD_TESTING)
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_execute_bt_action PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_include_directories(test_execute_bt_action PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+    )
     target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp)
   endif()
 
@@ -258,7 +275,6 @@ if(BUILD_TESTING)
       rclcpp_action
       behaviortree_cpp
       interfaces
-      brain_interfaces
       smacc2
       smacc2_msgs
       ament_index_cpp
@@ -268,7 +284,9 @@ if(BUILD_TESTING)
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_execute_bt_action_extended PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_include_directories(test_execute_bt_action_extended PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+    )
     target_link_libraries(test_execute_bt_action_extended Boost::thread yaml-cpp)
   endif()
 
@@ -283,8 +301,10 @@ 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 brain_interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
-      target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+        rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
+      target_include_directories(${test_name} PRIVATE
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+      )
       target_link_libraries(${test_name} Boost::thread yaml-cpp)
     endif()
   endforeach()
@@ -298,8 +318,10 @@ 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 brain_interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
-    target_include_directories(test_cerebrum_utils PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+      rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
+    target_include_directories(test_cerebrum_utils PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+    )
     target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp)
   endif()
 endif()

src/brain/config/brain_node_params.yaml

@@ -0,0 +1,116 @@
+# Configuration file for brain node parameters
+# This file contains configurable parameters for cerebrum and cerebellum nodes
+
+cerebrum_node:
+  ros__parameters:
+    # Timing parameters
+    tick_rate_hz: 10.0
+    rebuild_interval_ms: 5000
+    stats_publish_interval_ms: 10000
+    
+    # Configuration paths
+    # robot_config_path: ""
+    # skill_file_path: ""
+    
+    # Behavior tree parameters
+    default_bt_timeout: 300.0
+    enable_sequence_rebuild: true
+    
+    # Original cerebrum parameters
+    node_timeout_sec: 60.0
+    allow_bt_rebuild_during_execution: false
+    allowed_action_skills: ""
+    fixed_skill_sequence: ""
+    random_skill_pick_count: 5
+    
+    # Dynamic skill parameters (examples - these would be set per skill)
+    # cerebrum:
+    #   bt:
+    #     VisionObjectRecognition:
+    #       topic: "vision_object_recognition"
+    #       payload_yaml: ""
+    #       timeout_ms: "10000"
+    #     Arm:
+    #       topic: "arm_control"
+    #       payload_yaml: ""
+    #       timeout_ms: "30000"
+    #     # Example instance-specific overrides
+    #     Arm:
+    #       s1_arm_move_to_object:
+    #         topic: "arm_control_specific"
+    #         payload_yaml: "{target_pose: {x: 0.1, y: 0.2, z: 0.3}}"
+    #         timeout_ms: "45000"
+
+cerebellum_node:
+  ros__parameters:
+    # Action execution parameters
+    abort_on_failure: true
+    default_action_timeout_sec: 45.0
+    vision_grasp_object_timeout_sec: 120.0
+    
+    # Map parameters
+    map_save_url: "/tmp/humanoid_map"
+    map_save_image_format: "pgm"
+    map_save_free_thresh: 0.25
+    map_save_occupied_thresh: 0.65
+    map_load_url: ""
+    nav_goal_yaw_offset: 0.0
+    nav_goal_distance_tolerance: 0.5
+    slam_enable_mapping_default: true
+    
+    # Configuration paths
+    # robot_config_path: ""
+    # robot_skill_file_path: ""
+    
+    # Statistics parameters
+    enable_stats_publisher: true
+    stats_publish_interval_ms: 5000
+    
+    # Skill-specific timeouts (format: "SkillA=30,SkillB=10")
+    skill_timeouts: ""
+    
+    # Arm-specific parameters
+    arm_target_frame: "base_link_rightarm"  # 右臂基座坐标系
+    arm_transform_timeout_sec: 2.0          # 坐标变换超时时间
+
+    # Grasp pose calculation parameters
+    top_cam_right_arm_z_threshold: 0.05   #满足右臂抓取的头部相机输出的z坐标阈值
+    top_cam_right_arm_x_offset: 0.10      #右臂相机视觉识别时的x坐标偏移
+    top_cam_right_arm_y: 0.33             #右臂相机视觉识别时的y坐标
+    top_cam_right_arm_orientation: [-0.7071, 0.0, 0.0, 0.7071]    #右臂相机视觉识别时的四元数
+    top_cam_left_arm_z_threshold: -0.10   #满足左臂抓取的头部相机输出的z坐标阈值
+    top_cam_left_arm_x_offset: 0.10       #左臂相机视觉识别时的x坐标偏移
+    top_cam_left_arm_y: -0.33             #左臂相机视觉识别时的y坐标
+    top_cam_left_arm_orientation: [0.7071, 0.0, 0.0, 0.7071]      #左臂相机视觉识别时的四元数
+    side_cam_right_arm_y_offset: 0.02     # 补偿右臂y轴方向的抓取偏差（前后）
+    side_cam_right_arm_z_offset: 0.02     # 补偿右臂z轴方向的抓取偏差（左右）
+    side_cam_left_arm_y_offset: -0.02     # 补偿左臂y轴方向的抓取偏差（前后）
+    side_cam_left_arm_z_offset: 0.01      # 补偿左臂z轴方向的抓取偏差（左右）
+    take_object_arm_x: -0.05              # 抓取物体时x坐标
+    left_camera_frame: "LeftLink6"        # 左臂相机坐标系
+    right_camera_frame: "RightLink6"      # 右臂相机坐标系
+    target_frames: ["apple", "bottle", "medicine_box"]  # 识别物体的列表
+    target_frame: "medicine_box"          # 默认识别物体
+    grasp_palm_facings: ["down"]          # 抓取时手掌朝向
+    grasp_best_angles: [0.0]              # 抓取时最佳角度
+    grasp_type: "side"                    # 抓取类型
+    grasp_safety_dist: 0.10               # 抓取安全距离
+    grasp_finger_length: 0.17             # 抓取手指长度
+    hand_control_default_mode: 1          # 手控制默认模式
+    gripper_default_loc: 30               # 夹爪默认位置
+    arm_cmd_type_take_photo: 100          # 拍照
+    arm_cmd_type_pre_grasp: 101           # 预抓取
+    arm_cmd_type_grasp: 102               # 抓取
+    arm_cmd_type_take_object: 110         # 拿取动作
+    arm_cmd_type_custom_min: 100          # 自定义动作最小值
+    arm_cmd_type_custom_max: 120          # 自定义动作最大值
+    dual_arm_motion_type: "MOVEJ"         # DualArm motion_type: MOVEJ or MOVEL
+    gripper_default_speed: 0              # 夹爪默认速度
+    gripper_default_torque: 0             # 夹爪默认力矩
+    gripper_default_mode: 0               # 夹爪默认模式
+
+    # Service hook timeouts (seconds)
+    vision_object_recognition_wait_timeout_sec: 2.0   # 物体识别服务调用超时时间
+    vision_object_recognition_call_timeout_sec: 5.0   # 物体识别服务等待超时时间
+    map_load_wait_timeout_sec: 2.0        # 地图加载服务调用超时时间
+    map_load_call_timeout_sec: 5.0        # 地图加载服务等待超时时间

src/brain/config/bt_carry_boxes_sch1.params.yaml

@@ -11,7 +11,7 @@
 
     '
 - name: s1_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -21,11 +21,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-5, -60, -30, 0, 0, -180, -175, 60, 30, 0, 0, 0]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
 
     '
 - name: s2_arm_move_to_snapshot_pose
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -35,17 +35,17 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -35, -40, 1, -70, -180, -95, 35, 40, -1, 70, -10]
+    data_array: [-90, -30, -45, 0, 0, -180, -90, 30, 45, 0, 0, 0]
 
     '
 - name: s3_wheel_move_to_pickup_position
-  params: 'move_distance: 0.5
+  params: 'move_distance: 1.5
 
     move_angle: 0.0
 
     '
-- name: s4_arm_grasp_box
-  params: 'body_id: 0
+- name: s3_snapshot_action1
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -55,11 +55,95 @@
 
     frame_time_stamp: 0
 
-    data_array: [-80, -70, -25, 1, -85, -170, -100, 70, 25, -1, 85, -10]
+    data_array: [-85, -35, -60, 1, -90, -175, -95, 35, 60, -1, 90, -5]
+    
+    '
+- name: s3_snapshot_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -95, 40, 40, -1, 70, -10]
+    
+    '
+- name: s3_snapshot_top_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 50, 35, -40, 95, -60]
+    
+    '
+- name: s3_snapshot_top_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 70, 35, -40, 75, -60]
+    
+    '
+- name: s3_snapshot_top_action3
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 50, 35, -40, 95, -60]
+    
+    '
+- name: s4_arm_prepare_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]
+
+    '
+- name: s4_arm_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -81.8, 0, 0, -100, -80, -100, 81.8, 0, 0, 100, -100]
 
     '
 - name: s5_arm_pickup_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -69,7 +153,7 @@
 
     frame_time_stamp: 0
 
-    data_array: [-110, -70, -25, 1, -85, -170, -70, 70, 25, -1, 85, -10]
+    data_array: [-110, -81.8, 0, 0, -100, -80, -70, 81.8, 0, 0, 100, -100]
 
     '
 - name: s6_wheel_move_back_to_second_position
@@ -85,13 +169,13 @@
 
     '
 - name: s8_waist_bend_down
-  params: 'move_pitch_degree: 25.0
+  params: 'move_pitch_degree: 37.0
 
     move_yaw_degree: 0
 
     '
 - name: s9_arm_put_down_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -101,11 +185,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -70, -25, 1, -85, -150, -95, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]
 
     '
 - name: s10_arm_release
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -115,11 +199,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -60, -25, 1, -85, -150, -95, 60, 25, -1, 85, -30]
+    data_array: [-110, -78, 0, 0, -100, -65, -70, 78, 0, 0, 100, -115]
 
     '
 - name: s10_arm_raise_up
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -129,13 +213,13 @@
 
     frame_time_stamp: 0
 
-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-110, -78, 0, 0, -100, -65, -70, 78, 0, 0, 100, -115]
 
     '
-- name: s11_waist_bend_up_and_turn_around
-  params: 'move_pitch_degree: -25.0
+- name: s11_waist_bend_up
+  params: 'move_pitch_degree: -37.0
 
-    move_yaw_degree: -90.0
+    move_yaw_degree: 0.0
 
     '
 - name: s12_waist_turn_around
@@ -145,7 +229,7 @@
 
     '
 - name: s13_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -155,11 +239,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-5, -60, -30, 0, 0, -180, -175, 60, 30, 0, 0, 0]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
 
     '
 - name: s14_arm_move_to_snapshot_pose
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -169,17 +253,45 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -35, -40, 1, -70, -180, -95, 35, 40, -1, 70, -10]
+    data_array: [-90, -30, -45, 0, 0, -180, -90, 30, 45, 0, 0, 0]
+
+    '
+- name: s14_arm_move_pre_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-30, -55, -30, 0, -100, -65, -150, 55, 30, 0, 100, -115]
+
+    '
+- name: s14_arm_move_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
 
     '
 - name: s15_wheel_move_to_pickup_position
-  params: 'move_distance: 0.5
+  params: 'move_distance: 1.5
 
     move_angle: 0.0
 
     '
-- name: s16_arm_grasp_box
-  params: 'body_id: 0
+- name: s15_snapshot_action1
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -189,11 +301,95 @@
 
     frame_time_stamp: 0
 
-    data_array: [-80, -70, -25, 1, -85, -170, -100, 70, 25, -1, 85, -10]
+    data_array: [-85, -35, -60, 1, -90, -175, -95, 35, 60, -1, 90, -5]
+    
+    '
+- name: s15_snapshot_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -95, 40, 40, -1, 70, -10]
+    
+    '
+- name: s15_snapshot_top_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 50, 35, -40, 95, -60]
+    
+    '
+- name: s15_snapshot_top_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 70, 35, -40, 75, -60]
+    
+    '
+- name: s15_snapshot_top_action3
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 50, 35, -40, 95, -60]
+    
+    '
+- name: s16_arm_prepare_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]
+
+    '
+- name: s16_arm_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -81.8, 0, 0, -100, -80, -100, 81.8, 0, 0, 100, -100]
 
     '
 - name: s17_arm_pickup_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -203,7 +399,7 @@
 
     frame_time_stamp: 0
 
-    data_array: [-110, -70, -25, 1, -85, -170, -70, 70, 25, -1, 85, -10]
+    data_array: [-110, -81.8, 0, 0, -100, -80, -70, 81.8, 0, 0, 100, -100]
 
     '
 - name: s18_wheel_move_back_to_origin
@@ -219,13 +415,13 @@
 
     '
 - name: s20_waist_bend_down
-  params: 'move_pitch_degree: 25.0
+  params: 'move_pitch_degree: 37.0
 
     move_yaw_degree: 0
 
     '
 - name: s21_arm_put_down_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -235,11 +431,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -70, -25, 1, -85, -150, -95, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]
 
     '
 - name: s22_arm_release
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -249,11 +445,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -60, -25, 1, -85, -150, -95, 60, 25, -1, 85, -30]
+    data_array: [-110, -78, 0, 0, -100, -65, -70, 78, 0, 0, 100, -115]
 
     '
 - name: s22_arm_raise_up
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -263,13 +459,13 @@
 
     frame_time_stamp: 0
 
-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-110, -78, 0, 0, -100, -65, -70, 78, 0, 0, 100, -115]
 
     '
-- name: s23_waist_bend_up_and_turn_around
-  params: 'move_pitch_degree: -25.0
+- name: s23_waist_bend_up
+  params: 'move_pitch_degree: -37.0
 
-    move_yaw_degree: -90.0
+    move_yaw_degree: 0.0
 
     '
 - name: s24_waist_turn_around
@@ -279,7 +475,7 @@
 
     '
 - name: s25_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -289,7 +485,7 @@
 
     frame_time_stamp: 0
 
-    data_array: [-5, -60, -30, 0, 0, -180, -175, 60, 30, 0, 0, 0]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
 
     '
 - name: s26_wheel_move_back_to_origin
@@ -297,4 +493,36 @@
 
     move_angle: 0.0
 
+    '
+- name: s26_arm_move_pre_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-30, -55, -30, 0, -100, -65, -150, 55, 30, 0, 100, -115]
+
+    '
+- name: s26_arm_move_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
+
+    '
+- name: clear_done_instances_retry
+  params: '{}
+
     '

src/brain/config/bt_carry_boxes_sch1.xml

@@ -1,37 +1,67 @@
+<!--
+  功能描述: 搬运箱子流程1（单臂 + 底盘；无视觉；基于预设位抓取/放置；整体3次重试）。
+  运行概览:
+    1) 每次重试先清理已完成标记（ClearDoneInstances_H），随后机械臂回零/回初始位。
+    2) 第一次搬运循环：底盘前往抓取点 -> 依次执行多组“拍照/定位”预设位（s2/s3/s3_top）->
+       机械臂准备抓取/抓取/抬起 -> 底盘回到中间放置位 -> 腰部下俯与放置动作并行 -> 松爪 ->
+       抬臂与腰部上仰并行 -> 机械臂回预备位/回初始位。
+    3) 第二次搬运循环：重复“拍照位 -> 抓取/抬起 -> 回放置位 -> 放置/松爪 -> 抬臂/上仰”流程，
+       最后底盘回原点、机械臂回初始位。
+  说明: 全流程仅使用 Arm_H 与 MoveWheel_H/MoveWaist_H 的预设动作序列，无相机识别与条件分支。
+-->
 <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">
+      <RetryUntilSuccessful name="retry_all_action" num_attempts="3">
         <Sequence>
+          <ClearDoneInstances_H name="clear_done_instances_retry" />
           <!-- <MoveHome_H name="s0_motion_move_home" /> -->
-          <!-- <Arm_H name="s1_arm_move_origin_position" /> -->
-          <Arm_H name="s2_arm_move_to_snapshot_pose" />
+          <Arm_H name="s1_arm_move_origin_position" />
           <MoveWheel_H name="s3_wheel_move_to_pickup_position" />
+          <Arm_H name="s2_arm_move_to_snapshot_pose" />
+          <Arm_H name="s3_snapshot_action1" />
+          <Arm_H name="s3_snapshot_action2" />
+          <Arm_H name="s3_snapshot_top_action1" />
+          <Arm_H name="s3_snapshot_top_action2" />
+          <Arm_H name="s3_snapshot_top_action3" />
+          <Arm_H name="s4_arm_prepare_grasp_box" />
           <Arm_H name="s4_arm_grasp_box" />
           <Arm_H name="s5_arm_pickup_box" />
           <MoveWheel_H name="s6_wheel_move_back_to_second_position" />
-          <MoveWaist_H name="s7_waist_turn_around" />
-          <MoveWaist_H name="s8_waist_bend_down" />
-          <Arm_H name="s9_arm_put_down_box" />
+          <Parallel name="parallel_action_3">
+            <MoveWaist_H name="s8_waist_bend_down" />
+            <Arm_H name="s9_arm_put_down_box" />
+          </Parallel>
           <Arm_H name="s10_arm_release" />
-          <Arm_H name="s10_arm_raise_up" />
-          <MoveWaist_H name="s11_waist_bend_up_and_turn_around" />
-          <!-- <MoveWaist_H name="s12_waist_turn_around" /> -->
-          <!-- <Arm_H name="s13_arm_move_origin_position" /> -->
-          <Arm_H name="s14_arm_move_to_snapshot_pose" />
+          <Parallel name="parallel_action_4">
+            <Arm_H name="s10_arm_raise_up" />
+            <MoveWaist_H name="s11_waist_bend_up" />
+          </Parallel>
+          <Arm_H name="s14_arm_move_pre_origin_position" />
+          <Arm_H name="s14_arm_move_origin_position" />
           <MoveWheel_H name="s15_wheel_move_to_pickup_position" />
+          <Arm_H name="s14_arm_move_to_snapshot_pose" />
+          <Arm_H name="s15_snapshot_action1" />
+          <Arm_H name="s15_snapshot_action2" />
+          <Arm_H name="s15_snapshot_top_action1" />
+          <Arm_H name="s15_snapshot_top_action2" />
+          <Arm_H name="s15_snapshot_top_action3" />
+          <Arm_H name="s16_arm_prepare_grasp_box" />
           <Arm_H name="s16_arm_grasp_box" />
           <Arm_H name="s17_arm_pickup_box" />
           <MoveWheel_H name="s18_wheel_move_back_to_origin" />
-          <MoveWaist_H name="s19_waist_turn_around" />
-          <MoveWaist_H name="s20_waist_bend_down" />
-          <Arm_H name="s21_arm_put_down_box" />
+          <Parallel name="parallel_action_7">
+            <MoveWaist_H name="s20_waist_bend_down" />
+            <Arm_H name="s21_arm_put_down_box" />
+          </Parallel>
           <Arm_H name="s22_arm_release" />
-          <Arm_H name="s22_arm_raise_up" />
-          <MoveWaist_H name="s23_waist_bend_up_and_turn_around" />
-          <!-- <MoveWaist_H name="s24_waist_turn_around" /> -->
-          <!-- <Arm_H name="s25_arm_move_origin_position" /> -->
+          <Parallel name="parallel_action_8">
+            <Arm_H name="s22_arm_raise_up" />
+            <MoveWaist_H name="s23_waist_bend_up" />
+          </Parallel>
+            <Arm_H name="s26_arm_move_pre_origin_position" />
+          <Arm_H name="s26_arm_move_origin_position" />
           <MoveWheel_H name="s26_wheel_move_back_to_origin" />
         </Sequence>
       </RetryUntilSuccessful>

src/brain/config/bt_carry_boxes_sch2.params.yaml

@@ -7,7 +7,7 @@
 
     '
 - name: s1_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -17,7 +17,7 @@
 
     frame_time_stamp: 0
 
-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
 
     '
 - name: s2_wheel_move_to_origin_pickup_position
@@ -33,13 +33,13 @@
 
     '
 - name: s4_waist_bend_down
-  params: 'move_pitch_degree: 25.0
+  params: 'move_pitch_degree: 38.0
 
     move_yaw_degree: 0.0
 
     '
-- name: s5_arm_prepare_to_grasp
-  params: 'body_id: 0
+- name: s4_arm_move_pre_origin_position
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -49,11 +49,109 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -60, -25, 1, -85, -150, -95, 60, 25, -1, 85, -30]
+    data_array: [-30, -55, -30, 0, -100, -65, -150, 55, 30, 0, 100, -115]
+
+    '
+- name: s4_arm_move_to_snapshot_pose
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -68, 70, 0, 0, 80, -20]
+
+    '
+- name: s4_snapshot_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-105, -55, -35, 0, -110, -160, -75, 55, 35, 0, 110, -20]
+
+    '
+- name: s4_snapshot_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -68, 70, 0, 0, 80, -20]
+
+    '
+- name: s4_snapshot_top_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 60, 35, -40, 85, -60]
+
+    '
+- name: s4_snapshot_top_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 80, 35, -40, 65, -55]
+
+    '
+- name: s4_snapshot_top_action3
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 60, 35, -40, 85, -60]
+
+    '
+- name: s5_arm_prepare_to_grasp
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-110, -75, 0, 0, -100, -65, -70, 75, 0, 0, 100, -115]
 
     '
 - name: s6_arm_grasp_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -63,11 +161,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -70, -25, 1, -85, -150, -95, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]
 
     '
 - name: s7_arm_pickup_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -77,13 +175,13 @@
 
     frame_time_stamp: 0
 
-    data_array: [-110, -70, -25, 1, -85, -150, -70, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]
 
     '
-- name: s8_waist_bend_up_and_turn_around
-  params: 'move_pitch_degree: -25.0
+- name: s8_waist_bend_up
+  params: 'move_pitch_degree: -38.0
 
-    move_yaw_degree: -90.0
+    move_yaw_degree: 0.0
 
     '
 - name: s9_waist_turn_around
@@ -93,13 +191,13 @@
 
     '
 - name: s10_wheel_move_to_putdown_position
-  params: 'move_distance: 0.5
+  params: 'move_distance: 1.5
 
     move_angle: 0.0
 
     '
 - name: s11_arm_putdown_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -109,11 +207,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-80, -70, -25, 1, -85, -170, -100, 70, 25, -1, 85, -10]
+    data_array: [-80, -81.8, 0, 0, -100, -80, -100, 81.8, 0, 0, 100, -100]
 
     '
 - name: s12_arm_release_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -123,11 +221,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-80, -60, -25, 1, -85, -170, -100, 60, 25, -1, 85, -10]
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]
 
     '
 - name: s12_arm_raise_up
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -137,7 +235,35 @@
 
     frame_time_stamp: 0
 
-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-110, -75, 0, 0, -100, -80, -70, 75, 0, 0, 100, -100]
+
+    '
+- name: s13_arm_move_to_snapshot_pose
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-90, -30, -45, 0, 0, -180, -90, 30, 45, 0, 0, 0]
+
+    '
+- name: s13_arm_move_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
 
     '
 - name: s13_wheel_move_to_origin_pickup_position
@@ -159,13 +285,13 @@
 
     '
 - name: s15_waist_bend_down
-  params: 'move_pitch_degree: 25.0
+  params: 'move_pitch_degree: 38.0
 
     move_yaw_degree: 0.0
 
     '
-- name: s16_arm_prepare_to_grasp
-  params: 'body_id: 0
+- name: s15_arm_move_pre_origin_position
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -175,11 +301,109 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -60, -25, 1, -85, -150, -95, 60, 25, -1, 85, -30]
+    data_array: [-30, -55, -30, 0, -100, -65, -150, 55, 30, 0, 100, -115]
+
+    '
+- name: s15_arm_move_to_snapshot_pose
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -68, 70, 0, 0, 80, -20]
+
+    '
+- name: s15_snapshot_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-105, -55, -35, 0, -110, -160, -75, 55, 35, 0, 110, -20]
+
+    '
+- name: s15_snapshot_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -68, 70, 0, 0, 80, -20]
+
+    '
+- name: s15_snapshot_top_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 60, 35, -40, 85, -60]
+
+    '
+- name: s15_snapshot_top_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 80, 35, -40, 65, -55]
+
+    '
+- name: s15_snapshot_top_action3
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 60, 35, -40, 85, -60]
+
+    '
+- name: s16_arm_prepare_to_grasp
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-110, -75, 0, 0, -100, -65, -70, 75, 0, 0, 100, -115]
 
     '
 - name: s17_arm_grasp_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -189,11 +413,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-85, -70, -25, 1, -85, -150, -95, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]
 
     '
 - name: s18_arm_pickup_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -203,13 +427,13 @@
 
     frame_time_stamp: 0
 
-    data_array: [-110, -70, -25, 1, -85, -150, -70, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]
 
     '
-- name: s19_waist_bend_up_and_turn_around
-  params: 'move_pitch_degree: -25.0
+- name: s19_waist_bend_up
+  params: 'move_pitch_degree: -38.0
 
-    move_yaw_degree: -90.0
+    move_yaw_degree: 0.0
 
     '
 - name: s20_waist_turn_around
@@ -219,13 +443,13 @@
 
     '
 - name: s21_wheel_move_to_putdown_position
-  params: 'move_distance: 0.5
+  params: 'move_distance: 1.5
 
     move_angle: 0.0
 
     '
 - name: s22_arm_putdown_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -235,11 +459,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-80, -70, -25, 1, -85, -170, -100, 70, 25, -1, 85, -10]
+    data_array: [-80, -81.8, 0, 0, -100, -80, -100, 81.8, 0, 0, 100, -100]
 
     '
 - name: s23_arm_release_box
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -249,11 +473,11 @@
 
     frame_time_stamp: 0
 
-    data_array: [-80, -60, -25, 1, -85, -170, -100, 60, 25, -1, 85, -10]
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]
 
     '
 - name: s23_arm_raise_up
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -263,7 +487,35 @@
 
     frame_time_stamp: 0
 
-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-110, -75, 0, 0, -100, -80, -70, 75, 0, 0, 100, -100]
+
+    '
+- name: s23_arm_move_to_snapshot_pose
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-90, -30, -45, 0, 0, -180, -90, 30, 45, 0, 0, 0]
+
+    '
+- name: s23_arm_move_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
 
     '
 - name: s24_wheel_move_to_origin_pickup_position
@@ -273,7 +525,7 @@
 
     '
 - name: s25_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2
 
     data_type: 2
 
@@ -283,6 +535,10 @@
 
     frame_time_stamp: 0
 
-    data_array: [-5, -60, -30, 0, 0, -180, -175, 60, 30, 0, 0, 0]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
+
+    '
+- name: clear_done_instances_retry
+  params: '{}
 
     '

src/brain/config/bt_carry_boxes_sch2.xml

@@ -1,37 +1,66 @@
+<!--
+  功能描述: 搬运箱子流程2（单臂 + 底盘 + 腰部俯仰；无视觉；适配高低位；整体3次重试）。
+  运行概览:
+    1) 每次重试先清理已完成标记（ClearDoneInstances_H），机械臂回零后底盘移动到抓取点。
+    2) 第一次抓取：机械臂到预抓取位与拍照位 -> 腰部下俯 -> 依次执行多组“拍照位”预设动作
+       -> 抓取准备/抓取 -> 抬起与腰部上仰并行 -> 底盘移动到放置位 -> 放置/松爪/抬臂
+       -> 机械臂回拍照位并回初始位。
+    3) 第二次抓取：底盘移动到第二抓取点 -> 重复“预抓取位 -> 下俯 -> 拍照位序列 -> 抓取/抬起”
+       -> 底盘到放置位 -> 放置/松爪/抬臂 -> 回拍照位与初始位 -> 底盘回原点。
+  说明: 全流程动作由 Arm_H、MoveWheel_H、MoveWaist_H 的预设位驱动，无相机识别与条件分支。
+-->
 <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">
+      <RetryUntilSuccessful name="retry_all_action" num_attempts="3">
         <Sequence>
+          <ClearDoneInstances_H name="clear_done_instances_retry" />
           <Arm_H name="s1_arm_move_origin_position" />
           <MoveWheel_H name="s2_wheel_move_to_origin_pickup_position" />
-          <MoveWaist_H name="s3_waist_turn_around" />
+          <Arm_H name="s4_arm_move_pre_origin_position" />
+          <Arm_H name="s4_arm_move_to_snapshot_pose" />
           <MoveWaist_H name="s4_waist_bend_down" />
+          <Arm_H name="s4_snapshot_action1" />
+          <Arm_H name="s4_snapshot_action2" />
+          <Arm_H name="s4_snapshot_top_action1" />
+          <Arm_H name="s4_snapshot_top_action2" />
+          <Arm_H name="s4_snapshot_top_action3" />
           <Arm_H name="s5_arm_prepare_to_grasp" />
           <Arm_H name="s6_arm_grasp_box" />
-          <Arm_H name="s7_arm_pickup_box" />
-          <MoveWaist_H name="s8_waist_bend_up_and_turn_around" />
-          <!-- <MoveWaist_H name="s9_waist_turn_around" /> -->
+          <Parallel name="parallel_action_3">
+            <Arm_H name="s7_arm_pickup_box" />
+            <MoveWaist_H name="s8_waist_bend_up" />
+          </Parallel>
           <MoveWheel_H name="s10_wheel_move_to_putdown_position" />
           <Arm_H name="s11_arm_putdown_box" />
           <Arm_H name="s12_arm_release_box" />
           <Arm_H name="s12_arm_raise_up" />
-          <MoveWheel_H name="s13_wheel_move_to_origin_pickup_position" />
-          <MoveWaist_H name="s14_waist_turn_around" />
+          <Arm_H name="s13_arm_move_to_snapshot_pose" />
+          <Arm_H name="s13_arm_move_origin_position" />
           <MoveWheel_H name="s15_wheel_move_to_second_pickup_position" />
+          <Arm_H name="s15_arm_move_pre_origin_position" />
+          <Arm_H name="s15_arm_move_to_snapshot_pose" />
           <MoveWaist_H name="s15_waist_bend_down" />
+          <Arm_H name="s15_snapshot_action1" />
+          <Arm_H name="s15_snapshot_action2" />
+          <Arm_H name="s15_snapshot_top_action1" />
+          <Arm_H name="s15_snapshot_top_action2" />
+          <Arm_H name="s15_snapshot_top_action3" />
           <Arm_H name="s16_arm_prepare_to_grasp" />
           <Arm_H name="s17_arm_grasp_box" />
-          <Arm_H name="s18_arm_pickup_box" />
-          <MoveWaist_H name="s19_waist_bend_up_and_turn_around" />
-          <!-- <MoveWaist_H name="s20_waist_turn_around" /> -->
+          <Parallel name="parallel_action_7">
+            <Arm_H name="s18_arm_pickup_box" />
+            <MoveWaist_H name="s19_waist_bend_up" />
+          </Parallel>
           <MoveWheel_H name="s21_wheel_move_to_putdown_position" />
           <Arm_H name="s22_arm_putdown_box" />
           <Arm_H name="s23_arm_release_box" />
           <Arm_H name="s23_arm_raise_up" />
+          <Arm_H name="s23_arm_move_to_snapshot_pose" />
+          <Arm_H name="s23_arm_move_origin_position" />
           <MoveWheel_H name="s24_wheel_move_to_origin_pickup_position" />
-          <!-- <Arm_H name="s25_arm_move_origin_position" /> -->
+          <Arm_H name="s25_arm_move_origin_position" />
         </Sequence>
       </RetryUntilSuccessful>      
     </Sequence>

src/brain/config/bt_vision_grasp.params.yaml

@@ -1,32 +0,0 @@
-- name: root
-  params: '{}
-
-    '
-- name: retry_vision_hand
-  params: '{}
-
-    '
-- name: VisionObjectRecognition_H
-  params: '{}
-
-    '
-- name: Arm_H
-  params: 'body_id: 0
-
-    data_type: 0
-
-    data_length: 0
-
-    command_id: 0
-
-    frame_time_stamp: 0
-
-    data_array: []
-
-    '
-- name: HandControl_H
-  params: 'mode: 0
-
-    effort: 0.0
-
-    '

src/brain/config/bt_vision_grasp.xml

@@ -1,15 +0,0 @@
-<root BTCPP_format="4">
-  <BehaviorTree ID="MainTree">
-    <Sequence name="root">
-      <!-- Retry vision + hand until HandControl_H succeeds -->
-      <RetryUntilSuccessful name="retry_vision_hand" num_attempts="5">
-        <Sequence>
-          <VisionObjectRecognition_H name="VisionObjectRecognition_H" />
-          <Arm_H name="Arm_H" />
-        </Sequence>
-      </RetryUntilSuccessful>
-      <!-- After Arm_H eventually succeeds, run the hand -->
-      <HandControl_H name="HandControl_H" />
-    </Sequence>
-  </BehaviorTree>
-</root>

src/brain/config/bt_vision_grasp_dual_arm.params.yaml

@@ -0,0 +1,250 @@
+- name: root
+  params: '{}
+
+    '
+- name: s1_right_hand_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_left_hand_gripper_open
+  params: *gripper_open
+- name: s1_right_arm_initial
+  params: &arm_inittial_right "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [80, -5, -112, -12, -62, 175]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_left_arm_initial
+  params: &arm_inittial_left "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [100, 5, 112, 12, 62, 5]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_right_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_left_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_right_arm_cam_take_photo
+  params: &right_arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n  motion_type: 100\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_left_arm_cam_take_photo
+  params: &left_arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n  motion_type: 100\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_right_camera_vision_recg
+  params: 'camera_position: right
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_left_camera_vision_recg
+  params: 'camera_position: left
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_right_arm_vision_pre_grasp
+  params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n  motion_type: 101\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s3_left_arm_vision_pre_grasp
+  params: &pre_grasp_left "arm_motion_params:\n- arm_id: 0\n  motion_type: 101\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_right_arm_vision_grasp
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 102\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_left_arm_vision_grasp
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 102\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_right_hand_gripper_close
+  params: &gripper_close 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_left_hand_gripper_close
+  params: *gripper_close
+- name: s4_right_arm_take_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 110\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_left_arm_take_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 110\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_right_arm_take_box_off
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_left_arm_take_box_off
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_grasp_box
+  params: &grasp_box_s7_s9_right "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_left_arm_grasp_box
+  params: &grasp_box_s7_s9_left "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85.28, -103.86, 19.46, 40.34, -7.37, 49.94]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_left_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s8_right_hand_gripper_open
+  params: *gripper_open
+- name: s8_left_hand_gripper_open
+  params: *gripper_open
+- name: s9_right_arm_grasp_box
+  params: *grasp_box_s7_s9_right
+- name: s9_left_arm_grasp_box
+  params: *grasp_box_s7_s9_left
+- name: s11_right_arm_initial
+  params: *arm_inittial_right
+- name: s11_left_arm_initial
+  params: *arm_inittial_left
+- name: s12_right_hand_gripper_open
+  params: *gripper_open
+- name: s12_left_hand_gripper_open
+  params: *gripper_open
+- name: s13_right_arm_initial
+  params: *arm_inittial_right
+- name: s13_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_overall
+  params: '{}
+
+    '
+- name: s14_right_arm_initial
+  params: *arm_inittial_right
+- name: s14_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_right_grasp
+  params: '{}
+
+    '
+- name: clear_done_instances_left_grasp
+  params: '{}
+
+    '
+- name: s15_right_hand_gripper_open
+  params: *gripper_open
+- name: s15_left_hand_gripper_open
+  params: *gripper_open
+- name: s15_right_arm_pre_grasp
+  params: *pre_grasp_right
+- name: s15_left_arm_pre_grasp
+  params: *pre_grasp_left
+- name: s16_right_arm_initial
+  params: *arm_inittial_right
+- name: s16_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_arm_vision
+  params: '{}
+
+    '
+- name: s17_right_arm_initial
+  params: *arm_inittial_right
+- name: s17_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_left_arm_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_right_arm_vision
+  params: '{}
+
+    '
+- name: s0_right_hand_gripper_open
+  params: *gripper_open
+- name: s0_left_hand_gripper_open
+  params: *gripper_open
+- name: s1_wheel_move_to_origin_pickup_position
+  params: &origin_pickup_position 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: -0.5
+
+    move_angle: 0.0
+
+    '
+- name: s6_move_waist_turn_left_90
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s10_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s10_wheel_move_to_origin_pickup_position
+  params: *origin_pickup_position
+- name: s15_left_arm_cam_take_photo
+  params: *left_arm_cam_take_photo
+- name: s15_right_arm_cam_take_photo
+  params: *right_arm_cam_take_photo
+- name: s15_right_arm_initial_vision
+  params: *arm_inittial_right
+- name: s15_left_arm_initial_vision
+  params: *arm_inittial_left
+- name: s15_right_arm_initial_grasp
+  params: *arm_inittial_right
+- name: s15_left_arm_initial_grasp
+  params: *arm_inittial_left
+- name: s15_right_arm_initial_cam
+  params: *arm_inittial_right
+- name: s15_left_arm_initial_cam
+  params: *arm_inittial_left

src/brain/config/bt_vision_grasp_dual_arm.xml

@@ -0,0 +1,363 @@
+<!--
+  功能描述: 双臂协同视觉抓取流程（不含底盘；顶视+双手眼；分阶段恢复与完成标记清理）。
+  运行概览:
+    1) 变量初始化：通过 Script 初始化右/左臂拍照状态、视觉成功标记与完成标记。
+    2) 顶视视觉阶段：双爪打开、双臂回初始 -> 顶视相机识别（最多5次）；失败则进入视觉恢复子树。
+    3) 双臂拍照位同步：左右臂并行到手眼拍照位，失败则回初始并允许单臂成功，确保至少一臂可用。
+    4) 串行手眼识别：右臂手眼视觉（最多5次）-> 左臂手眼视觉（最多5次）；
+       仅对尚未完成且拍照成功的手臂执行识别。
+    5) 异步抓取：左右臂并行抓取，各自包含“抓取动作 + 抓取恢复”分支；任一臂成功即可继续。
+    6) 放置阶段：左右臂并行放置，各自独立执行放置子树；任一臂成功即可继续。
+    7) 结束归位：双臂并行回初始位。
+  恢复机制:
+    - 视觉恢复：双臂回初始并清理视觉完成标记。
+    - 抓取恢复：左右臂各自执行“松爪/预抓取/拍照位/回初始”，并清理对应完成标记（match=left/right）。
+    - 手眼视觉恢复：左右臂各自回初始并清理对应视觉标记。
+    - 总体恢复：双爪打开、双臂回初始并清理整体完成标记。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <Script code="right_arm_cam_take_photo:=false; left_arm_cam_take_photo:=false; right_arm_done:=false; left_arm_done:=false; right_vision_ok:=false; left_vision_ok:=false" />
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Phase 1: Shared Vision Setup (Top Camera) -->
+            <Fallback name="shared_vision_with_recovery">
+              <SubTree ID="vision_setup_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+
+            <!-- Move both arms to positions where their hand cameras can see potential objects -->
+            <SubTree ID="dual_arm_cam_subtree" right_cam_status="{right_arm_cam_take_photo}" left_cam_status="{left_arm_cam_take_photo}"/>
+
+            <!-- Phase 2: Sequential Hand Vision -->
+            <SubTree ID="sequential_hand_vision_subtree" right_done="{right_arm_done}" left_done="{left_arm_done}" right_cam_ok="{right_arm_cam_take_photo}" left_cam_ok="{left_arm_cam_take_photo}" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}"/>
+
+            <!-- Phase 3: Independent Dual Arm Operation -->
+            <SubTree ID="dual_arm_grasp_subtree_wrapper" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
+
+            <!-- Phase 4: Dual Arm Putdown -->
+            <SubTree ID="dual_arm_putdown_subtree_wrapper" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
+
+            <!-- Phase 5: Dual Arm Hand Vision -->
+            <Parallel name="dual_arm_init_action">
+              <DualArm_H name="s11_right_arm_initial" />
+              <DualArm_H name="s11_left_arm_initial" />
+            </Parallel>
+
+          </Sequence>
+          
+          <Sequence>
+            <!-- Overall fallback recovery -->
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence>
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Shared setup and head-camera vision -->
+  <BehaviorTree ID="vision_setup_subtree"> 
+    <Sequence name="vision_setup_root">
+      <Parallel name="parallel_arm_init_action">
+        <GripperCmd1_H name="s0_right_hand_gripper_open" />
+        <GripperCmd0_H name="s0_left_hand_gripper_open" />
+        <DualArm_H name="s1_right_arm_initial" />
+        <DualArm_H name="s1_left_arm_initial" />
+      </Parallel>
+      <Parallel name="parallel_init_action">
+        <GripperCmd1_H name="s1_right_hand_gripper_open" />
+        <GripperCmd0_H name="s1_left_hand_gripper_open" />
+        <!-- <DualArm_H name="s1_right_arm_pre_cam_take_photo" />
+        <DualArm_H name="s1_left_arm_pre_cam_take_photo" /> -->
+      </Parallel>
+      <RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Right Arm Specific Subtrees -->
+  <BehaviorTree ID="right_DualArm_Hand_vision_subtree">
+    <Fallback name="right_hand_vision_fallback">
+      <RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
+      </RetryUntilSuccessful>
+      <Sequence>
+        <DualArm_H name="s15_right_arm_initial_vision" />
+        <AlwaysFailure />
+      </Sequence>
+    </Fallback>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_grasp_subtree"> 
+    <Sequence name="right_arm_grasp_root">
+      <RetryUntilSuccessful name="retry_right_arm_grasp_action" num_attempts="1">
+        <Sequence> 
+          <DualArm_H name="s3_right_arm_vision_pre_grasp" />
+          <DualArm_H name="s4_right_arm_vision_grasp" />
+          <GripperCmd1_H name="s4_right_hand_gripper_close" />
+          <DualArm_H name="s4_right_arm_take_box" />
+          <DualArm_H name="s5_right_arm_take_box_off" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_putdown_subtree">
+    <Sequence name="right_arm_putdown_root">
+      <RetryUntilSuccessful name="retry_right_arm_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <DualArm_H name="s7_right_arm_grasp_box" /> -->
+          <DualArm_H name="s7_right_arm_put_down_box" />
+          <GripperCmd1_H name="s8_right_hand_gripper_open" />
+          <DualArm_H name="s9_right_arm_grasp_box" />
+          <!-- <DualArm_H name="s11_right_arm_initial" /> -->
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Left Arm Specific Subtrees -->
+  <BehaviorTree ID="left_DualArm_Hand_vision_subtree">
+    <Fallback name="left_hand_vision_fallback">
+      <RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
+      </RetryUntilSuccessful>
+      <Sequence>
+        <DualArm_H name="s15_left_arm_initial_vision" />
+        <AlwaysFailure />
+      </Sequence>
+    </Fallback>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_grasp_subtree"> 
+    <Sequence name="left_arm_grasp_root">
+      <RetryUntilSuccessful name="retry_left_arm_grasp_action" num_attempts="1">
+        <Sequence> 
+          <DualArm_H name="s3_left_arm_vision_pre_grasp" />
+          <DualArm_H name="s4_left_arm_vision_grasp" />
+          <GripperCmd0_H name="s4_left_hand_gripper_close" />
+          <DualArm_H name="s4_left_arm_take_box" />
+          <DualArm_H name="s5_left_arm_take_box_off" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_putdown_subtree">
+    <Sequence name="left_arm_putdown_root">
+      <RetryUntilSuccessful name="retry_left_arm_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <DualArm_H name="s7_left_arm_grasp_box" /> -->
+          <DualArm_H name="s7_left_arm_put_down_box" />
+          <GripperCmd0_H name="s8_left_hand_gripper_open" />
+          <DualArm_H name="s9_left_arm_grasp_box" />
+          <!-- <DualArm_H name="s11_left_arm_initial" /> -->
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Recovery Subtrees -->
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <Parallel name="overall_recovery_parallel">
+        <GripperCmd1_H name="s12_right_hand_gripper_open" />
+        <GripperCmd0_H name="s12_left_hand_gripper_open" />
+        <DualArm_H name="s13_right_arm_initial" />
+        <DualArm_H name="s13_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_overall" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <Parallel name="arm_initial2">
+        <DualArm_H name="s14_right_arm_initial" />
+        <DualArm_H name="s14_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_vision" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_grasp_recovery_subtree">
+    <Sequence name="right_grasp_recovery_root">
+      <GripperCmd1_H name="s15_right_hand_gripper_open" />
+      <DualArm_H name="s15_right_arm_pre_grasp" />
+      <DualArm_H name="s15_right_arm_cam_take_photo" />
+      <DualArm_H name="s15_right_arm_initial_grasp" />
+      <ClearDoneInstances_H name="clear_done_instances_right_grasp" match="right" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_grasp_recovery_subtree">
+    <Sequence name="left_grasp_recovery_root">
+      <GripperCmd0_H name="s15_left_hand_gripper_open" />
+      <DualArm_H name="s15_left_arm_pre_grasp" />
+      <DualArm_H name="s15_left_arm_cam_take_photo" />
+      <DualArm_H name="s15_left_arm_initial_grasp" />
+      <ClearDoneInstances_H name="clear_done_instances_left_grasp" match="left" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="arm_vision_recovery_subtree">
+    <Sequence name="arm_vision_recovery_root">
+      <Parallel name="arm_initial3">
+        <DualArm_H name="s16_right_arm_initial" />
+        <DualArm_H name="s16_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_arm_vision" />
+    </Sequence>
+  </BehaviorTree>
+  
+  <BehaviorTree ID="left_arm_vision_recovery_subtree">
+    <Sequence name="left_arm_vision_recovery_root">
+      <Parallel name="left_arm_initial">
+        <DualArm_H name="s17_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_left_arm_vision" match="left" />
+    </Sequence>
+  </BehaviorTree>
+  
+  <BehaviorTree ID="right_arm_vision_recovery_subtree">
+    <Sequence name="right_arm_vision_recovery_root">
+      <Parallel name="right_arm_initial">
+        <DualArm_H name="s17_right_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_right_arm_vision" match="right" />
+    </Sequence>
+  </BehaviorTree>
+
+
+<!-- 子树：双臂相机同步拍照位 -->
+<BehaviorTree ID="dual_arm_cam_subtree">
+  <Sequence name="cam_sync_root">
+    <Script code="right_cam_status:=false; left_cam_status:=false" />
+    <Parallel name="dual_arm_cam_take_photo" success_count="2" failure_count="2">
+      <!-- 右臂 -->
+      <Fallback>
+        <Sequence>
+          <DualArm_H name="s2_right_arm_cam_take_photo" />
+          <Script code="right_cam_status:=true" />
+        </Sequence>
+        <DualArm_H name="s15_right_arm_initial_cam" />
+        <AlwaysSuccess/>
+      </Fallback>
+      <!-- 左臂 -->
+      <Fallback>
+        <Sequence>
+          <DualArm_H name="s2_left_arm_cam_take_photo" />
+          <Script code="left_cam_status:=true" />
+        </Sequence>
+        <DualArm_H name="s15_left_arm_initial_cam" />
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="right_cam_status==true || left_cam_status==true" />
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：串行手眼视觉识别 -->
+<BehaviorTree ID="sequential_hand_vision_subtree">
+  <Sequence name="hand_vision_root">
+    <Script code="right_v_ok:=false; left_v_ok:=false" />
+    <!-- 右臂视觉 -->
+    <Fallback>
+      <Sequence>
+        <ScriptCondition code="right_done==false &amp;&amp; right_cam_ok==true" />
+        <Fallback>
+          <Sequence>
+            <SubTree ID="right_DualArm_Hand_vision_subtree"/>
+            <Script code="right_v_ok:=true" />
+          </Sequence>
+          <AlwaysFailure/>
+        </Fallback>
+      </Sequence>
+      <AlwaysSuccess/>
+    </Fallback>
+    <!-- 左臂视觉 -->
+    <Fallback>
+      <Sequence>
+        <ScriptCondition code="left_done==false &amp;&amp; left_cam_ok==true" />
+        <Fallback>
+          <Sequence>
+            <SubTree ID="left_DualArm_Hand_vision_subtree"/>
+            <Script code="left_v_ok:=true" />
+          </Sequence>
+          <AlwaysFailure/>
+        </Fallback>
+      </Sequence>
+      <AlwaysSuccess/>
+    </Fallback>
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：异步双臂抓取 -->
+<BehaviorTree ID="dual_arm_grasp_subtree_wrapper">
+  <Sequence name="grasp_sync_root">
+    <Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="2">
+      <!-- 右臂抓取 -->
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="right_v_ok==true" />
+          <SubTree ID="right_arm_grasp_subtree"/>
+          <Script code="right_done:=true" />
+        </Sequence>
+        <Sequence>
+          <ScriptCondition code="right_v_ok==true" />
+          <SubTree ID="right_arm_grasp_recovery_subtree"/>
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+      <!-- 左臂抓取 -->
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="left_v_ok==true" />
+          <SubTree ID="left_arm_grasp_subtree"/>
+          <Script code="left_done:=true" />
+        </Sequence>
+        <Sequence>
+          <ScriptCondition code="left_v_ok==true" />
+          <SubTree ID="left_arm_grasp_recovery_subtree"/>
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="right_done==true || left_done==true" />
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：双臂放置流程 -->
+<BehaviorTree ID="dual_arm_putdown_subtree_wrapper">
+  <Sequence name="putdown_sync_root">
+    <Script code="left_p_ok:=false; right_p_ok:=false" />
+    <Parallel name="dual_arm_putdown" success_count="2" failure_count="2">
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="left_done==true" />
+          <SubTree ID="left_arm_putdown_subtree"/>
+          <Script code="left_p_ok:=true" />
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="right_done==true" />
+          <SubTree ID="right_arm_putdown_subtree"/>
+          <Script code="right_p_ok:=true" />
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="left_p_ok==true || right_p_ok==true" />
+  </Sequence>
+</BehaviorTree>
+
+</root>

src/brain/config/bt_vision_grasp_dual_arm_demo1.params.yaml

@@ -0,0 +1,254 @@
+- name: root
+  params: '{}
+
+    '
+- name: s1_right_hand_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_left_hand_gripper_open
+  params: *gripper_open
+- name: s1_right_arm_initial
+  params: &arm_inittial_right "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [80, -5, -112, -12, -62, 175]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_left_arm_initial
+  params: &arm_inittial_left "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [100, 5, 112, 12, 62, 5]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_right_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_left_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_right_arm_cam_take_photo
+  params: &right_arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n  motion_type: 100\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_left_arm_cam_take_photo
+  params: &left_arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n  motion_type: 100\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_right_camera_vision_recg
+  params: 'camera_position: right
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_left_camera_vision_recg
+  params: 'camera_position: left
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_right_arm_vision_pre_grasp
+  params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n  motion_type: 101\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 50\nvelocity_scaling: 40\n"
+- name: s3_left_arm_vision_pre_grasp
+  params: &pre_grasp_left "arm_motion_params:\n- arm_id: 0\n  motion_type: 101\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 50\nvelocity_scaling: 40\n"
+- name: s4_right_arm_vision_grasp
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 102\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_left_arm_vision_grasp
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 102\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_right_hand_gripper_close
+  params: &gripper_close 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_left_hand_gripper_close
+  params: *gripper_close
+- name: s4_right_arm_take_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 110\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_left_arm_take_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 110\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_right_arm_take_box_off
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_left_arm_take_box_off
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_grasp_box
+  params: &grasp_box_s7_s9_right "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_left_arm_grasp_box
+  params: &grasp_box_s7_s9_left "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85.28, -103.86, 19.46, 40.34, -7.37, 49.94]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_left_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s8_right_hand_gripper_open
+  params: *gripper_open
+- name: s8_left_hand_gripper_open
+  params: *gripper_open
+- name: s9_right_arm_grasp_box
+  params: *grasp_box_s7_s9_right
+- name: s9_left_arm_grasp_box
+  params: *grasp_box_s7_s9_left
+- name: s11_right_arm_initial
+  params: *arm_inittial_right
+- name: s11_left_arm_initial
+  params: *arm_inittial_left
+- name: s12_right_hand_gripper_open
+  params: *gripper_open
+- name: s12_left_hand_gripper_open
+  params: *gripper_open
+- name: s13_right_arm_initial
+  params: *arm_inittial_right
+- name: s13_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_overall
+  params: '{}
+
+    '
+- name: s14_right_arm_initial
+  params: *arm_inittial_right
+- name: s14_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_right_grasp
+  params: '{}
+
+    '
+- name: clear_done_instances_left_grasp
+  params: '{}
+
+    '
+- name: s15_right_hand_gripper_open
+  params: *gripper_open
+- name: s15_left_hand_gripper_open
+  params: *gripper_open
+- name: s15_right_arm_pre_grasp
+  params: *pre_grasp_right
+- name: s15_left_arm_pre_grasp
+  params: *pre_grasp_left
+- name: s16_right_arm_initial
+  params: *arm_inittial_right
+- name: s16_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_arm_vision
+  params: '{}
+
+    '
+- name: s17_right_arm_initial
+  params: *arm_inittial_right
+- name: s17_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_left_arm_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_right_arm_vision
+  params: '{}
+
+    '
+- name: s0_right_hand_gripper_open
+  params: *gripper_open
+- name: s0_left_hand_gripper_open
+  params: *gripper_open
+- name: s1_wheel_move_to_origin_pickup_position
+  params: &origin_pickup_position 'move_distance: 0.5
+
+    move_angle: 0.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s6_move_waist_turn_left_90
+  params: &waist_turn_left_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s10_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s10_wheel_move_to_origin_pickup_position
+  params: *origin_pickup_position
+- name: s15_left_arm_cam_take_photo
+  params: *left_arm_cam_take_photo
+- name: s15_right_arm_cam_take_photo
+  params: *right_arm_cam_take_photo
+- name: s15_right_arm_initial_vision
+  params: *arm_inittial_right
+- name: s15_left_arm_initial_vision
+  params: *arm_inittial_left
+- name: s15_right_arm_initial_grasp
+  params: *arm_inittial_right
+- name: s15_left_arm_initial_grasp
+  params: *arm_inittial_left
+- name: s15_right_arm_initial_cam
+  params: *arm_inittial_right
+- name: s15_left_arm_initial_cam
+  params: *arm_inittial_left
+- name: s6_move_waist_turn_right_90
+  params: *waist_turn_right_90
+- name: s10_move_waist_turn_left_90
+  params: *waist_turn_left_90

src/brain/config/bt_vision_grasp_dual_arm_demo1.xml

@@ -0,0 +1,370 @@
+<!--
+  功能描述: 双臂协同视觉抓取演示流程1（全机身协作；顶视+双手眼；含恢复与重试）。
+  运行概览:
+    1) 起始移动：底盘移动到抓取工位。
+    2) 顶视视觉阶段：双爪打开、双臂回初始 -> 顶视相机识别（最多5次）；失败则进入视觉恢复。
+    3) 双臂拍照位同步：左右臂并行到手眼拍照位，允许单臂成功。
+    4) 串行手眼识别：右臂手眼 -> 左臂手眼（各最多5次），仅对可用且未完成的手臂执行。
+    5) 异步抓取：左右臂并行抓取，各自含抓取恢复；任一臂成功即可继续。
+    6) 放置移动：底盘到放置工位 + 腰部右转并行。
+    7) 放置阶段：左右臂并行放置，各自独立完成。
+    8) 复位：腰部左转复位；底盘回抓取位；双臂回初始位。
+  恢复机制: 视觉/抓取/手眼各自有恢复子树，失败时触发重试；外层总流程最多重试3次。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <Script code="right_arm_cam_take_photo:=false; left_arm_cam_take_photo:=false; right_arm_done:=false; left_arm_done:=false; right_vision_ok:=false; left_vision_ok:=false" />
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Phase 0: Move Wheel to Origin Pickup Position -->
+            <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
+
+            <!-- Phase 1: Shared Vision Setup (Top Camera) -->
+            <Fallback name="shared_vision_with_recovery">
+              <SubTree ID="vision_setup_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+
+            <!-- Move both arms to positions where their hand cameras can see potential objects -->
+            <SubTree ID="dual_arm_cam_subtree" right_cam_status="{right_arm_cam_take_photo}" left_cam_status="{left_arm_cam_take_photo}"/>
+
+            <!-- Phase 2: Sequential Hand Vision -->
+            <SubTree ID="sequential_hand_vision_subtree" right_done="{right_arm_done}" left_done="{left_arm_done}" right_cam_ok="{right_arm_cam_take_photo}" left_cam_ok="{left_arm_cam_take_photo}" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}"/>
+
+            <!-- Phase 3: Independent Dual Arm Operation -->
+            <SubTree ID="dual_arm_grasp_subtree_wrapper" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
+
+            <Parallel name="parallel_action_2">
+              <MoveWheel_H name="s6_wheel_move_to_putdown_position" />
+              <MoveWaist_H name="s6_move_waist_turn_right_90" />
+            </Parallel>
+
+            <!-- Phase 4: Dual Arm Putdown -->
+            <SubTree ID="dual_arm_putdown_subtree_wrapper" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
+
+            <MoveWaist_H name="s10_move_waist_turn_left_90" />
+
+            <!-- Phase 5: Dual Arm Initialization -->
+            <Parallel name="dual_arm_init_action">
+              <!-- <MoveWheel_H name="s10_wheel_move_to_origin_pickup_position" /> -->
+              <DualArm_H name="s11_right_arm_initial" />
+              <DualArm_H name="s11_left_arm_initial" />
+            </Parallel>
+
+          </Sequence>
+          
+          <Sequence>
+            <!-- Overall fallback recovery -->
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence>
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Shared setup and head-camera vision -->
+  <BehaviorTree ID="vision_setup_subtree"> 
+    <Sequence name="vision_setup_root">
+      <Parallel name="parallel_arm_init_action">
+        <GripperCmd1_H name="s0_right_hand_gripper_open" />
+        <GripperCmd0_H name="s0_left_hand_gripper_open" />
+        <DualArm_H name="s1_right_arm_initial" />
+        <DualArm_H name="s1_left_arm_initial" />
+      </Parallel>
+      <Parallel name="parallel_init_action">
+        <GripperCmd1_H name="s1_right_hand_gripper_open" />
+        <GripperCmd0_H name="s1_left_hand_gripper_open" />
+        <!-- <DualArm_H name="s1_right_arm_pre_cam_take_photo" />
+        <DualArm_H name="s1_left_arm_pre_cam_take_photo" /> -->
+      </Parallel>
+      <RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Right Arm Specific Subtrees -->
+  <BehaviorTree ID="right_DualArm_Hand_vision_subtree">
+    <Fallback name="right_hand_vision_fallback">
+      <RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
+      </RetryUntilSuccessful>
+      <Sequence>
+        <DualArm_H name="s15_right_arm_initial_vision" />
+        <AlwaysFailure />
+      </Sequence>
+    </Fallback>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_grasp_subtree"> 
+    <Sequence name="right_arm_grasp_root">
+      <RetryUntilSuccessful name="retry_right_arm_grasp_action" num_attempts="1">
+        <Sequence> 
+          <DualArm_H name="s3_right_arm_vision_pre_grasp" />
+          <DualArm_H name="s4_right_arm_vision_grasp" />
+          <GripperCmd1_H name="s4_right_hand_gripper_close" />
+          <DualArm_H name="s4_right_arm_take_box" />
+          <DualArm_H name="s5_right_arm_take_box_off" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_putdown_subtree">
+    <Sequence name="right_arm_putdown_root">
+      <RetryUntilSuccessful name="retry_right_arm_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <DualArm_H name="s7_right_arm_grasp_box" /> -->
+          <DualArm_H name="s7_right_arm_put_down_box" />
+          <GripperCmd1_H name="s8_right_hand_gripper_open" />
+          <DualArm_H name="s9_right_arm_grasp_box" />
+          <!-- <DualArm_H name="s11_right_arm_initial" /> -->
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Left Arm Specific Subtrees -->
+  <BehaviorTree ID="left_DualArm_Hand_vision_subtree">
+    <Fallback name="left_hand_vision_fallback">
+      <RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
+      </RetryUntilSuccessful>
+      <Sequence>
+        <DualArm_H name="s15_left_arm_initial_vision" />
+        <AlwaysFailure />
+      </Sequence>
+    </Fallback>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_grasp_subtree"> 
+    <Sequence name="left_arm_grasp_root">
+      <RetryUntilSuccessful name="retry_left_arm_grasp_action" num_attempts="1">
+        <Sequence> 
+          <DualArm_H name="s3_left_arm_vision_pre_grasp" />
+          <DualArm_H name="s4_left_arm_vision_grasp" />
+          <GripperCmd0_H name="s4_left_hand_gripper_close" />
+          <DualArm_H name="s4_left_arm_take_box" />
+          <DualArm_H name="s5_left_arm_take_box_off" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_putdown_subtree">
+    <Sequence name="left_arm_putdown_root">
+      <RetryUntilSuccessful name="retry_left_arm_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <DualArm_H name="s7_left_arm_grasp_box" /> -->
+          <DualArm_H name="s7_left_arm_put_down_box" />
+          <GripperCmd0_H name="s8_left_hand_gripper_open" />
+          <DualArm_H name="s9_left_arm_grasp_box" />
+          <!-- <DualArm_H name="s11_left_arm_initial" /> -->
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Recovery Subtrees -->
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <Parallel name="overall_recovery_parallel">
+        <GripperCmd1_H name="s12_right_hand_gripper_open" />
+        <GripperCmd0_H name="s12_left_hand_gripper_open" />
+        <DualArm_H name="s13_right_arm_initial" />
+        <DualArm_H name="s13_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_overall" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <Parallel name="arm_initial2">
+        <DualArm_H name="s14_right_arm_initial" />
+        <DualArm_H name="s14_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_vision" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_grasp_recovery_subtree">
+    <Sequence name="right_grasp_recovery_root">
+      <GripperCmd1_H name="s15_right_hand_gripper_open" />
+      <DualArm_H name="s15_right_arm_pre_grasp" />
+      <DualArm_H name="s15_right_arm_cam_take_photo" />
+      <DualArm_H name="s15_right_arm_initial_grasp" />
+      <ClearDoneInstances_H name="clear_done_instances_right_grasp" match="right" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_grasp_recovery_subtree">
+    <Sequence name="left_grasp_recovery_root">
+      <GripperCmd0_H name="s15_left_hand_gripper_open" />
+      <DualArm_H name="s15_left_arm_pre_grasp" />
+      <DualArm_H name="s15_left_arm_cam_take_photo" />
+      <DualArm_H name="s15_left_arm_initial_grasp" />
+      <ClearDoneInstances_H name="clear_done_instances_left_grasp" match="left" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="arm_vision_recovery_subtree">
+    <Sequence name="arm_vision_recovery_root">
+      <Parallel name="arm_initial3">
+        <DualArm_H name="s16_right_arm_initial" />
+        <DualArm_H name="s16_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_arm_vision" />
+    </Sequence>
+  </BehaviorTree>
+  
+  <BehaviorTree ID="left_arm_vision_recovery_subtree">
+    <Sequence name="left_arm_vision_recovery_root">
+      <Parallel name="left_arm_initial">
+        <DualArm_H name="s17_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_left_arm_vision" match="left" />
+    </Sequence>
+  </BehaviorTree>
+  
+  <BehaviorTree ID="right_arm_vision_recovery_subtree">
+    <Sequence name="right_arm_vision_recovery_root">
+      <Parallel name="right_arm_initial">
+        <DualArm_H name="s17_right_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_right_arm_vision" match="right" />
+    </Sequence>
+  </BehaviorTree>
+
+
+<!-- 子树：双臂相机同步拍照位 -->
+<BehaviorTree ID="dual_arm_cam_subtree">
+  <Sequence name="cam_sync_root">
+    <Script code="right_cam_status:=false; left_cam_status:=false" />
+    <Parallel name="dual_arm_cam_take_photo" success_count="2" failure_count="2">
+      <!-- 右臂 -->
+      <Fallback>
+        <Sequence>
+          <DualArm_H name="s2_right_arm_cam_take_photo" />
+          <Script code="right_cam_status:=true" />
+        </Sequence>
+        <DualArm_H name="s15_right_arm_initial_cam" />
+        <AlwaysSuccess/>
+      </Fallback>
+      <!-- 左臂 -->
+      <Fallback>
+        <Sequence>
+          <DualArm_H name="s2_left_arm_cam_take_photo" />
+          <Script code="left_cam_status:=true" />
+        </Sequence>
+        <DualArm_H name="s15_left_arm_initial_cam" />
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="right_cam_status==true || left_cam_status==true" />
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：串行手眼视觉识别 -->
+<BehaviorTree ID="sequential_hand_vision_subtree">
+  <Sequence name="hand_vision_root">
+    <Script code="right_v_ok:=false; left_v_ok:=false" />
+    <!-- 右臂视觉 -->
+    <Fallback>
+      <Sequence>
+        <ScriptCondition code="right_done==false &amp;&amp; right_cam_ok==true" />
+        <Fallback>
+          <Sequence>
+            <SubTree ID="right_DualArm_Hand_vision_subtree"/>
+            <Script code="right_v_ok:=true" />
+          </Sequence>
+          <AlwaysFailure/>
+        </Fallback>
+      </Sequence>
+      <AlwaysSuccess/>
+    </Fallback>
+    <!-- 左臂视觉 -->
+    <Fallback>
+      <Sequence>
+        <ScriptCondition code="left_done==false &amp;&amp; left_cam_ok==true" />
+        <Fallback>
+          <Sequence>
+            <SubTree ID="left_DualArm_Hand_vision_subtree"/>
+            <Script code="left_v_ok:=true" />
+          </Sequence>
+          <AlwaysFailure/>
+        </Fallback>
+      </Sequence>
+      <AlwaysSuccess/>
+    </Fallback>
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：异步双臂抓取 -->
+<BehaviorTree ID="dual_arm_grasp_subtree_wrapper">
+  <Sequence name="grasp_sync_root">
+    <Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="2">
+      <!-- 右臂抓取 -->
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="right_v_ok==true" />
+          <SubTree ID="right_arm_grasp_subtree"/>
+          <Script code="right_done:=true" />
+        </Sequence>
+        <Sequence>
+          <ScriptCondition code="right_v_ok==true" />
+          <SubTree ID="right_arm_grasp_recovery_subtree"/>
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+      <!-- 左臂抓取 -->
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="left_v_ok==true" />
+          <SubTree ID="left_arm_grasp_subtree"/>
+          <Script code="left_done:=true" />
+        </Sequence>
+        <Sequence>
+          <ScriptCondition code="left_v_ok==true" />
+          <SubTree ID="left_arm_grasp_recovery_subtree"/>
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="right_done==true || left_done==true" />
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：双臂放置流程 -->
+<BehaviorTree ID="dual_arm_putdown_subtree_wrapper">
+  <Sequence name="putdown_sync_root">
+    <Script code="left_p_ok:=false; right_p_ok:=false" />
+    <Parallel name="dual_arm_putdown" success_count="2" failure_count="2">
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="left_done==true" />
+          <SubTree ID="left_arm_putdown_subtree"/>
+          <Script code="left_p_ok:=true" />
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="right_done==true" />
+          <SubTree ID="right_arm_putdown_subtree"/>
+          <Script code="right_p_ok:=true" />
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="left_p_ok==true || right_p_ok==true" />
+  </Sequence>
+</BehaviorTree>
+
+</root>

src/brain/config/bt_vision_grasp_left_arm.params.yaml

@@ -0,0 +1,192 @@
+- name: root
+  params: '{}
+
+    '
+- name: retry_vision_hand
+  params: '{}
+
+    '
+- name: s1_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s1_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_left_arm_initial
+  params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [100, 5, 112, 12, 62, 5]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_right_arm_initial
+  params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [80, -5, -112, -12, -62, 175]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_wheel_move_to_origin_pickup_position
+  params: 'move_distance: 0.5
+
+    move_angle: 0.0
+
+    '
+- name: s2_left_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_right_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    classes: ["medicine_box"]
+
+    '
+- name: s2_left_arm_cam_take_photo
+  params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n  motion_type: 100\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s3_left_camera_vision_recg
+  params: 'camera_position: left
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_left_arm_vision_pre_grasp
+  params: &pre_grasp "arm_motion_params:\n- arm_id: 0\n  motion_type: 101\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_left_arm_vision_grasp
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 102\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_gripper_close
+  params: 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_left_arm_take_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 110\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_left_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [84, 88, 45, 22, 16, 68]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_right_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [95, -25, -90, 0, -68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s6_move_waist_turn_left_90
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s7_left_arm_grasp_box
+  params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85, 88, -14, 22, 16, 68]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [95, -25, -90, 0, -68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_left_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [95, 88, -14, -22, 16, 112]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [95, -25, -90, 0, -68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s8_gripper_open
+  params: *gripper_open
+- name: s9_left_arm_grasp_box
+  params: *grasp_box_s7_s9
+- name: s10_move_waist_turn_right_90
+  params: *waist_turn_right_90
+- name: s11_left_arm_initial
+  params: *left_arm_initial
+- name: s11_right_arm_initial
+  params: *right_arm_initial
+- name: s12_gripper_open
+  params: *gripper_open
+- name: s13_left_arm_initial
+  params: *left_arm_initial
+- name: s13_right_arm_initial
+  params: *right_arm_initial
+- name: s14_left_arm_initial
+  params: *left_arm_initial
+- name: s14_right_arm_initial
+  params: *right_arm_initial
+- name: s15_left_arm_initial
+  params: *left_arm_initial
+- name: s15_right_arm_initial
+  params: *right_arm_initial
+- name: s16_left_arm_initial
+  params: *left_arm_initial
+- name: s16_right_arm_initial
+  params: *right_arm_initial
+- name: s14_gripper_open
+  params: *gripper_open
+- name: s15_gripper_open
+  params: *gripper_open
+- name: s16_gripper_open
+  params: *gripper_open
+- name: s15_arm_pre_grasp
+  params: *pre_grasp
+- name: s16_arm_pre_grasp
+  params: *pre_grasp
+- name: s15_left_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: s16_left_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: clear_done_instances_overall
+  params: '{}
+
+    '
+- name: clear_done_instances_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_grasp
+  params: '{}
+
+    '
+- name: clear_done_instances_putdown
+  params: '{}
+
+    '

src/brain/config/bt_vision_grasp_left_arm.xml

@@ -0,0 +1,199 @@
+<!--
+  功能描述: 左臂视觉抓取流程（顶视+手眼识别；抓取/放置含恢复；外层3次重试）。
+  运行概览:
+   1) 视觉阶段（vision_subtree）：双臂回初始并打开左爪；进行顶视相机识别（最多10次）；
+     左臂到手眼拍照位后进行左手眼识别（最多10次）。
+   2) 抓取阶段（grasp_subtree）：左臂预抓取 -> 左臂抓取 -> 关闭左爪 -> 左臂抬起；
+     随后左右臂并行保持抓箱位（parallel_action_grasp_box）。
+   3) 放置阶段（putdown_subtree）：左右臂并行抓箱位/放箱位 -> 左爪松开 -> 左臂回抓箱位；
+     双臂并行回初始位。
+   4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
+     并通过 AlwaysFailure 触发阶段重试；外层总流程最多重试3次。
+  说明: 底盘与腰部动作在此版本中被注释，流程主要为视觉+双臂姿态与左爪控制。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Vision with recovery -->
+            <Fallback name="vision_with_recovery">
+              <SubTree ID="vision_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Grasp with recovery -->
+            <Fallback name="grasp_with_recovery">
+              <SubTree ID="grasp_subtree"/>
+              <Sequence>
+                <SubTree ID="grasp_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Putdown with recovery -->
+            <Fallback name="putdown_with_recovery">
+              <SubTree ID="putdown_subtree"/>
+              <Sequence>
+                <SubTree ID="putdown_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+          </Sequence>
+          
+          <!-- Overall recovery if the entire sequence fails -->
+          <!-- <Sequence>
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence> -->
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_subtree"> 
+    <Sequence name="vision_root">
+      <RetryUntilSuccessful name="retry_vision" num_attempts="1">
+        <Sequence> 
+          <Parallel name="parallel_action_1">
+            <DualArm_H name="s1_left_arm_initial" />
+            <DualArm_H name="s1_right_arm_initial" />
+            <GripperCmd0_H name="s1_gripper_open" />
+          </Parallel>
+          <!-- <Parallel name="parallel_action_2"> -->
+            <!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
+            <!-- <GripperCmd0_H name="s1_gripper_open" /> -->
+            <!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
+            <DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
+          <!-- </Parallel> -->
+          <SubTree ID="vision_top_cam_subtree"/>
+
+          <DualArm_H name="s2_left_arm_cam_take_photo" />
+          <SubTree ID="vision_hand_cam_subtree"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_top_cam_subtree"> 
+    <Sequence name="vision_top_cam_root">
+      <RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_hand_cam_subtree"> 
+    <Sequence name="vision_hand_cam_root">
+      <RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_subtree"> 
+    <Sequence name="grasp_root">
+      <RetryUntilSuccessful name="retry_grasp" num_attempts="1">
+        <Sequence> 
+          <DualArm_H name="s3_left_arm_vision_pre_grasp" />
+          <DualArm_H name="s4_left_arm_vision_grasp" />
+          <GripperCmd0_H name="s4_gripper_close" />
+          <DualArm_H name="s4_left_arm_take_box" />
+          <Parallel name="parallel_action_grasp_box">
+            <DualArm_H name="s5_left_arm_grasp_box" />
+            <DualArm_H name="s5_right_arm_grasp_box" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_subtree">
+    <Sequence name="putdown_root">
+      <RetryUntilSuccessful name="retry_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <Parallel name="parallel_action_2">
+            <MoveWheel_H name="s6_wheel_move_to_putdown_position" />
+            <MoveWaist_H name="s6_move_waist_turn_left_90" />
+          </Parallel> -->
+          <Parallel name="parallel_action_s7_grasp_box">
+            <DualArm_H name="s7_left_arm_grasp_box" />
+            <DualArm_H name="s7_right_arm_grasp_box" />
+          </Parallel>
+          <Parallel name="parallel_action_s7_put_down_box">
+            <DualArm_H name="s7_left_arm_put_down_box" />
+            <DualArm_H name="s7_right_arm_put_down_box" />
+          </Parallel>
+          <GripperCmd0_H name="s8_gripper_open" />
+          <DualArm_H name="s9_left_arm_grasp_box" />
+
+          <Parallel name="parallel_action_3">
+            <!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
+            <DualArm_H name="s11_left_arm_initial" />
+            <DualArm_H name="s11_right_arm_initial" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <RetryUntilSuccessful name="retry_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd0_H name="s12_gripper_open" />
+          <DualArm_H name="s13_left_arm_initial" />
+          <DualArm_H name="s13_right_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_overall" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
+        <Sequence> 
+          <!-- <GripperCmd0_H name="s14_gripper_open" /> -->
+          <DualArm_H name="s14_left_arm_initial" />
+          <DualArm_H name="s14_right_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_vision" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_recovery_subtree">
+    <Sequence name="grasp_recovery_root">
+      <RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd0_H name="s15_gripper_open" />
+          <DualArm_H name="s15_arm_pre_grasp" />
+          <DualArm_H name="s15_left_arm_cam_take_photo" />
+          <DualArm_H name="s15_left_arm_initial" />
+          <DualArm_H name="s15_right_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_grasp" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_recovery_subtree">
+    <Sequence name="putdown_recovery_root">
+      <RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd0_H name="s16_gripper_open" />
+          <DualArm_H name="s16_arm_pre_grasp" />
+          <DualArm_H name="s16_left_arm_cam_take_photo" />
+          <DualArm_H name="s16_left_arm_initial" />
+          <DualArm_H name="s16_right_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_putdown" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>

src/brain/config/bt_vision_grasp_left_arm_demo1.params.yaml

@@ -0,0 +1,196 @@
+- name: root
+  params: '{}
+
+    '
+- name: retry_vision_hand
+  params: '{}
+
+    '
+- name: s1_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s1_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_left_arm_initial
+  params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [100, 5, 112, 12, 62, 5]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_right_arm_initial
+  params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [80, -5, -112, -12, -62, 175]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_wheel_move_to_origin_pickup_position
+  params: 'move_distance: 0.5
+
+    move_angle: 0.0
+
+    '
+- name: s2_left_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_right_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    classes: ["medicine_box"]
+
+    '
+- name: s2_left_arm_cam_take_photo
+  params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n  motion_type: 100\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s3_left_camera_vision_recg
+  params: 'camera_position: left
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_left_arm_vision_pre_grasp
+  params: &pre_grasp "arm_motion_params:\n- arm_id: 0\n  motion_type: 101\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_left_arm_vision_grasp
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 102\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_gripper_close
+  params: 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_left_arm_take_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 110\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_left_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [84, 88, 45, 22, 16, 68]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_right_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [95, -25, -90, 0, -68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s6_move_waist_turn_left_90
+  params: &waist_turn_left_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s7_left_arm_grasp_box
+  params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85, 88, -14, 22, 16, 68]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [95, -25, -90, 0, -68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_left_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [95, 88, -14, -22, 16, 112]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [95, -25, -90, 0, -68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s8_gripper_open
+  params: *gripper_open
+- name: s9_left_arm_grasp_box
+  params: *grasp_box_s7_s9
+- name: s10_move_waist_turn_right_90
+  params: *waist_turn_right_90
+- name: s11_left_arm_initial
+  params: *left_arm_initial
+- name: s11_right_arm_initial
+  params: *right_arm_initial
+- name: s12_gripper_open
+  params: *gripper_open
+- name: s13_left_arm_initial
+  params: *left_arm_initial
+- name: s13_right_arm_initial
+  params: *right_arm_initial
+- name: s14_left_arm_initial
+  params: *left_arm_initial
+- name: s14_right_arm_initial
+  params: *right_arm_initial
+- name: s15_left_arm_initial
+  params: *left_arm_initial
+- name: s15_right_arm_initial
+  params: *right_arm_initial
+- name: s16_left_arm_initial
+  params: *left_arm_initial
+- name: s16_right_arm_initial
+  params: *right_arm_initial
+- name: s14_gripper_open
+  params: *gripper_open
+- name: s15_gripper_open
+  params: *gripper_open
+- name: s16_gripper_open
+  params: *gripper_open
+- name: s15_arm_pre_grasp
+  params: *pre_grasp
+- name: s16_arm_pre_grasp
+  params: *pre_grasp
+- name: s15_left_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: s16_left_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: s6_move_waist_turn_right_90
+  params: *waist_turn_right_90
+- name: s10_move_waist_turn_left_90
+  params: *waist_turn_left_90
+- name: clear_done_instances_overall
+  params: '{}
+
+    '
+- name: clear_done_instances_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_grasp
+  params: '{}
+
+    '
+- name: clear_done_instances_putdown
+  params: '{}
+
+    '

src/brain/config/bt_vision_grasp_left_arm_demo1.xml

@@ -0,0 +1,196 @@
+<!--
+  功能描述: 左臂视觉抓取演示流程1（顶视 + 左手眼识别；含底盘与腰部转向；外层3次重试）。
+  运行概览:
+   1) 视觉阶段：双臂回初始并打开左爪（GripperCmd0）；底盘移动到取物位；顶视相机识别（最多10次）；
+     左臂到手眼拍照位后进行左手眼识别（最多10次）。
+   2) 抓取阶段：左臂预抓取 -> 左臂抓取 -> 关闭左爪 -> 左臂抬起 -> 左右臂并行保持抓箱位。
+   3) 放置阶段：底盘移动到放置位并腰部右转；左右臂并行抓箱位/放箱位 -> 左爪松开 -> 左臂回抓箱位；
+     腰部左转复位，同时双臂回初始位。
+   4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
+     并通过 AlwaysFailure 触发阶段重试。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Vision with recovery -->
+            <Fallback name="vision_with_recovery">
+              <SubTree ID="vision_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Grasp with recovery -->
+            <Fallback name="grasp_with_recovery">
+              <SubTree ID="grasp_subtree"/>
+              <Sequence>
+                <SubTree ID="grasp_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Putdown with recovery -->
+            <Fallback name="putdown_with_recovery">
+              <SubTree ID="putdown_subtree"/>
+              <Sequence>
+                <SubTree ID="putdown_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+          </Sequence>
+          
+          <!-- Overall recovery if the entire sequence fails -->
+          <!-- <Sequence>
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence> -->
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_subtree"> 
+    <Sequence name="vision_root">
+      <RetryUntilSuccessful name="retry_vision" num_attempts="1">
+        <Sequence> 
+          <Parallel name="parallel_action_1">
+            <DualArm_H name="s1_left_arm_initial" />
+            <DualArm_H name="s1_right_arm_initial" />
+            <GripperCmd0_H name="s1_gripper_open" />
+          </Parallel>
+          <Parallel name="parallel_action_2">
+            <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
+            <!-- <GripperCmd0_H name="s1_gripper_open" /> -->
+            <!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
+            <DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
+          </Parallel>
+          <SubTree ID="vision_top_cam_subtree"/>
+
+          <DualArm_H name="s2_left_arm_cam_take_photo" />
+          <SubTree ID="vision_hand_cam_subtree"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_top_cam_subtree"> 
+    <Sequence name="vision_top_cam_root">
+      <RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_hand_cam_subtree"> 
+    <Sequence name="vision_hand_cam_root">
+      <RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_subtree"> 
+    <Sequence name="grasp_root">
+      <RetryUntilSuccessful name="retry_grasp" num_attempts="1">
+        <Sequence> 
+          <DualArm_H name="s3_left_arm_vision_pre_grasp" />
+          <DualArm_H name="s4_left_arm_vision_grasp" />
+          <GripperCmd0_H name="s4_gripper_close" />
+          <DualArm_H name="s4_left_arm_take_box" />
+          <Parallel name="parallel_action_grasp_box">
+            <DualArm_H name="s5_left_arm_grasp_box" />
+            <DualArm_H name="s5_right_arm_grasp_box" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_subtree">
+    <Sequence name="putdown_root">
+      <RetryUntilSuccessful name="retry_putdown" num_attempts="1">
+        <Sequence> 
+          <Parallel name="parallel_action_2">
+            <MoveWheel_H name="s6_wheel_move_to_putdown_position" />
+            <MoveWaist_H name="s6_move_waist_turn_right_90" />
+          </Parallel>
+          <Parallel name="parallel_action_s7_grasp_box">
+            <DualArm_H name="s7_left_arm_grasp_box" />
+            <DualArm_H name="s7_right_arm_grasp_box" />
+          </Parallel>
+          <Parallel name="parallel_action_s7_put_down_box">
+            <DualArm_H name="s7_left_arm_put_down_box" />
+            <DualArm_H name="s7_right_arm_put_down_box" />
+          </Parallel>
+          <GripperCmd0_H name="s8_gripper_open" />
+          <DualArm_H name="s9_left_arm_grasp_box" />
+          <MoveWaist_H name="s10_move_waist_turn_left_90" />
+          <Parallel name="parallel_action_3">
+            <DualArm_H name="s11_left_arm_initial" />
+            <DualArm_H name="s11_right_arm_initial" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <RetryUntilSuccessful name="retry_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd0_H name="s12_gripper_open" />
+          <DualArm_H name="s13_left_arm_initial" />
+          <DualArm_H name="s13_right_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_overall" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
+        <Sequence> 
+          <!-- <GripperCmd0_H name="s14_gripper_open" /> -->
+          <DualArm_H name="s14_left_arm_initial" />
+          <DualArm_H name="s14_right_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_vision" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_recovery_subtree">
+    <Sequence name="grasp_recovery_root">
+      <RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd0_H name="s15_gripper_open" />
+          <DualArm_H name="s15_arm_pre_grasp" />
+          <DualArm_H name="s15_left_arm_cam_take_photo" />
+          <DualArm_H name="s15_left_arm_initial" />
+          <DualArm_H name="s15_right_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_grasp" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_recovery_subtree">
+    <Sequence name="putdown_recovery_root">
+      <RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd0_H name="s16_gripper_open" />
+          <DualArm_H name="s16_arm_pre_grasp" />
+          <DualArm_H name="s16_left_arm_cam_take_photo" />
+          <DualArm_H name="s16_left_arm_initial" />
+          <DualArm_H name="s16_right_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_putdown" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>

src/brain/config/bt_vision_grasp_right_arm.params.yaml

@@ -0,0 +1,192 @@
+- name: root
+  params: '{}
+
+    '
+- name: retry_vision_hand
+  params: '{}
+
+    '
+- name: s1_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s1_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_left_arm_initial
+  params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [100, 5, 112, 12, 62, 5]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_right_arm_initial
+  params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [80, -5, -112, -12, -62, 175]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_wheel_move_to_origin_pickup_position
+  params: 'move_distance: 0.5
+
+    move_angle: 0.0
+
+    '
+- name: s2_left_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_right_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    classes: ["medicine_box"]
+
+    '
+- name: s2_right_arm_cam_take_photo
+  params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n  motion_type: 100\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s3_right_camera_vision_recg
+  params: 'camera_position: right
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_right_arm_vision_pre_grasp
+  params: &pre_grasp "arm_motion_params:\n- arm_id: 1\n  motion_type: 101\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_right_arm_vision_grasp
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 102\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_gripper_close
+  params: 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_right_arm_take_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 110\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_left_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85, 25, 90, 0, 68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_right_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [96.30, -88.95, -45, -22.88, -16.30, 112.10]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s6_move_waist_turn_left_90
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s7_left_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85, 25, 90, 0, 68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_grasp_box
+  params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [96.30, -88.95, 13.98, -22.88, -16.30, 112.10]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_left_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85, 25, 90, 0, 68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85.78, -88.95, 13.97, 22.88, -16.29, 67.99]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s8_gripper_open
+  params: *gripper_open
+- name: s9_right_arm_grasp_box
+  params: *grasp_box_s7_s9
+- name: s10_move_waist_turn_right_90
+  params: *waist_turn_right_90
+- name: s11_left_arm_initial
+  params: *left_arm_initial
+- name: s11_right_arm_initial
+  params: *right_arm_initial
+- name: s12_gripper_open
+  params: *gripper_open
+- name: s13_left_arm_initial
+  params: *left_arm_initial
+- name: s13_right_arm_initial
+  params: *right_arm_initial
+- name: s14_left_arm_initial
+  params: *left_arm_initial
+- name: s14_right_arm_initial
+  params: *right_arm_initial
+- name: s15_left_arm_initial
+  params: *left_arm_initial
+- name: s15_right_arm_initial
+  params: *right_arm_initial
+- name: s16_left_arm_initial
+  params: *left_arm_initial
+- name: s16_right_arm_initial
+  params: *right_arm_initial
+- name: s14_gripper_open
+  params: *gripper_open
+- name: s15_gripper_open
+  params: *gripper_open
+- name: s16_gripper_open
+  params: *gripper_open
+- name: s15_arm_pre_grasp
+  params: *pre_grasp
+- name: s16_arm_pre_grasp
+  params: *pre_grasp
+- name: s15_right_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: s16_right_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: clear_done_instances_overall
+  params: '{}
+
+    '
+- name: clear_done_instances_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_grasp
+  params: '{}
+
+    '
+- name: clear_done_instances_putdown
+  params: '{}
+
+    '

src/brain/config/bt_vision_grasp_right_arm.xml

@@ -0,0 +1,199 @@
+<!--
+  功能描述: 右臂视觉抓取流程（顶视+手眼识别；抓取/放置含恢复；外层3次重试）。
+  运行概览:
+   1) 视觉阶段（vision_subtree）：双臂回初始并打开右爪；进行顶视相机识别（最多10次）；
+     右臂到手眼拍照位后进行右手眼识别（最多10次）。
+   2) 抓取阶段（grasp_subtree）：右臂预抓取 -> 右臂抓取 -> 关闭右爪 -> 右臂抬起；
+     随后左右臂并行保持抓箱位（parallel_action_grasp_box）。
+   3) 放置阶段（putdown_subtree）：左右臂并行抓箱位/放箱位 -> 右爪松开 -> 右臂回抓箱位；
+     双臂并行回初始位。
+   4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
+     并通过 AlwaysFailure 触发阶段重试；外层总流程最多重试3次。
+  说明: 底盘与腰部动作在此版本中被注释，流程主要为视觉+双臂姿态与右爪控制。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Vision with recovery -->
+            <Fallback name="vision_with_recovery">
+              <SubTree ID="vision_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Grasp with recovery -->
+            <Fallback name="grasp_with_recovery">
+              <SubTree ID="grasp_subtree"/>
+              <Sequence>
+                <SubTree ID="grasp_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Putdown with recovery -->
+            <Fallback name="putdown_with_recovery">
+              <SubTree ID="putdown_subtree"/>
+              <Sequence>
+                <SubTree ID="putdown_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+          </Sequence>
+          
+          <!-- Overall recovery if the entire sequence fails -->
+          <!-- <Sequence>
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence> -->
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_subtree"> 
+    <Sequence name="vision_root">
+      <RetryUntilSuccessful name="retry_vision" num_attempts="1">
+        <Sequence> 
+          <Parallel name="parallel_action_1">
+            <DualArm_H name="s1_left_arm_initial" />
+            <DualArm_H name="s1_right_arm_initial" />
+            <GripperCmd1_H name="s1_gripper_open" />
+          </Parallel>
+          <!-- <Parallel name="parallel_action_2"> -->
+            <!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
+            <!-- <GripperCmd1_H name="s1_gripper_open" /> -->
+            <!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
+            <DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
+          <!-- </Parallel> -->
+          <SubTree ID="vision_top_cam_subtree"/>
+
+          <DualArm_H name="s2_right_arm_cam_take_photo" />
+          <SubTree ID="vision_hand_cam_subtree"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_top_cam_subtree"> 
+    <Sequence name="vision_top_cam_root">
+      <RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_hand_cam_subtree"> 
+    <Sequence name="vision_hand_cam_root">
+      <RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_subtree"> 
+    <Sequence name="grasp_root">
+      <RetryUntilSuccessful name="retry_grasp" num_attempts="1">
+        <Sequence> 
+          <DualArm_H name="s3_right_arm_vision_pre_grasp" />
+          <DualArm_H name="s4_right_arm_vision_grasp" />
+          <GripperCmd1_H name="s4_gripper_close" />
+          <DualArm_H name="s4_right_arm_take_box" />
+          <Parallel name="parallel_action_grasp_box">
+            <DualArm_H name="s5_left_arm_grasp_box" />
+            <DualArm_H name="s5_right_arm_grasp_box" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_subtree">
+    <Sequence name="putdown_root">
+      <RetryUntilSuccessful name="retry_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <Parallel name="parallel_action_2">
+            <MoveWheel_H name="s6_wheel_move_to_putdown_position" />
+            <MoveWaist_H name="s6_move_waist_turn_left_90" />
+          </Parallel> -->
+          <Parallel name="parallel_action_s7_grasp_box">
+            <DualArm_H name="s7_left_arm_grasp_box" />
+            <DualArm_H name="s7_right_arm_grasp_box" />
+          </Parallel>
+          <Parallel name="parallel_action_s7_put_down_box">
+            <DualArm_H name="s7_left_arm_put_down_box" />
+            <DualArm_H name="s7_right_arm_put_down_box" />
+          </Parallel>
+          <GripperCmd1_H name="s8_gripper_open" />
+          <DualArm_H name="s9_right_arm_grasp_box" />
+
+          <Parallel name="parallel_action_3">
+            <!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
+            <DualArm_H name="s11_right_arm_initial" />
+            <DualArm_H name="s11_left_arm_initial" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <RetryUntilSuccessful name="retry_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd1_H name="s12_gripper_open" />
+          <DualArm_H name="s13_right_arm_initial" />
+          <DualArm_H name="s13_left_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_overall" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
+        <Sequence> 
+          <!-- <GripperCmd1_H name="s14_gripper_open" /> -->
+          <DualArm_H name="s14_right_arm_initial" />
+          <DualArm_H name="s14_left_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_vision" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_recovery_subtree">
+    <Sequence name="grasp_recovery_root">
+      <RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd1_H name="s15_gripper_open" />
+          <DualArm_H name="s15_arm_pre_grasp" />
+          <DualArm_H name="s15_right_arm_cam_take_photo" />
+          <DualArm_H name="s15_right_arm_initial" />
+          <DualArm_H name="s15_left_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_grasp" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_recovery_subtree">
+    <Sequence name="putdown_recovery_root">
+      <RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd1_H name="s16_gripper_open" />
+          <DualArm_H name="s16_arm_pre_grasp" />
+          <DualArm_H name="s16_right_arm_cam_take_photo" />
+          <DualArm_H name="s16_right_arm_initial" />
+          <DualArm_H name="s16_left_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_putdown" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>

src/brain/config/bt_vision_grasp_right_arm_demo1.params.yaml

@@ -0,0 +1,196 @@
+- name: root
+  params: '{}
+
+    '
+- name: retry_vision_hand
+  params: '{}
+
+    '
+- name: s1_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s1_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_left_arm_initial
+  params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [100, 5, 112, 12, 62, 5]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_right_arm_initial
+  params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [80, -5, -112, -12, -62, 175]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s1_wheel_move_to_origin_pickup_position
+  params: 'move_distance: 0.5
+
+    move_angle: 0.0
+
+    '
+- name: s2_left_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_right_arm_pre_cam_take_photo
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    classes: ["medicine_box"]
+
+    '
+- name: s2_right_arm_cam_take_photo
+  params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n  motion_type: 100\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s3_right_camera_vision_recg
+  params: 'camera_position: right
+
+    classes: ["medicine_box"]
+
+    '
+- name: s3_right_arm_vision_pre_grasp
+  params: &pre_grasp "arm_motion_params:\n- arm_id: 1\n  motion_type: 101\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_right_arm_vision_grasp
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 102\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s4_gripper_close
+  params: 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_right_arm_take_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 110\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [360, 360, 360, 360, 360, 360]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_left_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85, 25, 90, 0, 68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s5_right_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [96.30, -88.95, -45, -22.88, -16.30, 112.10]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s6_move_waist_turn_left_90
+  params: &waist_turn_left_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s7_left_arm_grasp_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85, 25, 90, 0, 68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_grasp_box
+  params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [96.30, -88.95, 13.98, -22.88, -16.30, 112.10]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_left_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 0\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85, 25, 90, 0, 68, 90]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s7_right_arm_put_down_box
+  params: "arm_motion_params:\n- arm_id: 1\n  motion_type: 0\n  target_pose:\n   \
+    \ position:\n      x: 0.0\n      y: 0.0\n      z: 0.0\n    orientation:\n    \
+    \  x: 0.0\n      y: 0.0\n      z: 0.0\n      w: 1.0\n  target_joints: [85.78, -88.95, 13.97, 22.88, -16.29, 67.99]\n  blend_radius:\
+    \ 0\nvelocity_scaling: 40\n"
+- name: s8_gripper_open
+  params: *gripper_open
+- name: s9_right_arm_grasp_box
+  params: *grasp_box_s7_s9
+- name: s10_move_waist_turn_right_90
+  params: *waist_turn_right_90
+- name: s11_left_arm_initial
+  params: *left_arm_initial
+- name: s11_right_arm_initial
+  params: *right_arm_initial
+- name: s12_gripper_open
+  params: *gripper_open
+- name: s13_left_arm_initial
+  params: *left_arm_initial
+- name: s13_right_arm_initial
+  params: *right_arm_initial
+- name: s14_left_arm_initial
+  params: *left_arm_initial
+- name: s14_right_arm_initial
+  params: *right_arm_initial
+- name: s15_left_arm_initial
+  params: *left_arm_initial
+- name: s15_right_arm_initial
+  params: *right_arm_initial
+- name: s16_left_arm_initial
+  params: *left_arm_initial
+- name: s16_right_arm_initial
+  params: *right_arm_initial
+- name: s14_gripper_open
+  params: *gripper_open
+- name: s15_gripper_open
+  params: *gripper_open
+- name: s16_gripper_open
+  params: *gripper_open
+- name: s15_arm_pre_grasp
+  params: *pre_grasp
+- name: s16_arm_pre_grasp
+  params: *pre_grasp
+- name: s15_right_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: s16_right_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: s6_move_waist_turn_right_90
+  params: *waist_turn_right_90
+- name: s10_move_waist_turn_left_90
+  params: *waist_turn_left_90
+- name: clear_done_instances_overall
+  params: '{}
+
+    '
+- name: clear_done_instances_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_grasp
+  params: '{}
+
+    '
+- name: clear_done_instances_putdown
+  params: '{}
+
+    '

src/brain/config/bt_vision_grasp_right_arm_demo1.xml

@@ -0,0 +1,196 @@
+<!--
+  功能描述: 右臂视觉抓取演示流程1（顶视 + 右手眼识别；含底盘与腰部转向；外层3次重试）。
+  运行概览:
+   1) 视觉阶段：双臂回初始并打开右爪（GripperCmd1）；底盘移动到取物位；顶视相机识别（最多10次）；
+     右臂到手眼拍照位后进行右手眼识别（最多10次）。
+   2) 抓取阶段：右臂预抓取 -> 右臂抓取 -> 关闭右爪 -> 右臂抬起 -> 左右臂并行保持抓箱位。
+   3) 放置阶段：底盘移动到放置位并腰部右转；左右臂并行抓箱位/放箱位 -> 右爪松开 -> 右臂回抓箱位；
+     腰部左转复位，同时双臂回初始位。
+   4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
+     并通过 AlwaysFailure 触发阶段重试。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Vision with recovery -->
+            <Fallback name="vision_with_recovery">
+              <SubTree ID="vision_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Grasp with recovery -->
+            <Fallback name="grasp_with_recovery">
+              <SubTree ID="grasp_subtree"/>
+              <Sequence>
+                <SubTree ID="grasp_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Putdown with recovery -->
+            <Fallback name="putdown_with_recovery">
+              <SubTree ID="putdown_subtree"/>
+              <Sequence>
+                <SubTree ID="putdown_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+          </Sequence>
+          
+          <!-- Overall recovery if the entire sequence fails -->
+          <!-- <Sequence>
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence> -->
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_subtree"> 
+    <Sequence name="vision_root">
+      <RetryUntilSuccessful name="retry_vision" num_attempts="1">
+        <Sequence> 
+          <Parallel name="parallel_action_1">
+            <DualArm_H name="s1_left_arm_initial" />
+            <DualArm_H name="s1_right_arm_initial" />
+            <GripperCmd1_H name="s1_gripper_open" />
+          </Parallel>
+          <Parallel name="parallel_action_2">
+            <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
+            <!-- <GripperCmd1_H name="s1_gripper_open" /> -->
+            <!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
+            <DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
+          </Parallel>
+          <SubTree ID="vision_top_cam_subtree"/>
+
+          <DualArm_H name="s2_right_arm_cam_take_photo" />
+          <SubTree ID="vision_hand_cam_subtree"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_top_cam_subtree"> 
+    <Sequence name="vision_top_cam_root">
+      <RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_hand_cam_subtree"> 
+    <Sequence name="vision_hand_cam_root">
+      <RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_subtree"> 
+    <Sequence name="grasp_root">
+      <RetryUntilSuccessful name="retry_grasp" num_attempts="1">
+        <Sequence> 
+          <DualArm_H name="s3_right_arm_vision_pre_grasp" />
+          <DualArm_H name="s4_right_arm_vision_grasp" />
+          <GripperCmd1_H name="s4_gripper_close" />
+          <DualArm_H name="s4_right_arm_take_box" />
+          <Parallel name="parallel_action_grasp_box">
+            <DualArm_H name="s5_left_arm_grasp_box" />
+            <DualArm_H name="s5_right_arm_grasp_box" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_subtree">
+    <Sequence name="putdown_root">
+      <RetryUntilSuccessful name="retry_putdown" num_attempts="1">
+        <Sequence> 
+          <Parallel name="parallel_action_2">
+            <MoveWheel_H name="s6_wheel_move_to_putdown_position" />
+            <MoveWaist_H name="s6_move_waist_turn_right_90" />
+          </Parallel>
+          <Parallel name="parallel_action_s7_grasp_box">
+            <DualArm_H name="s7_left_arm_grasp_box" />
+            <DualArm_H name="s7_right_arm_grasp_box" />
+          </Parallel>
+          <Parallel name="parallel_action_s7_put_down_box">
+            <DualArm_H name="s7_left_arm_put_down_box" />
+            <DualArm_H name="s7_right_arm_put_down_box" />
+          </Parallel>
+          <GripperCmd1_H name="s8_gripper_open" />
+          <DualArm_H name="s9_right_arm_grasp_box" />
+          <MoveWaist_H name="s10_move_waist_turn_left_90" />
+          <Parallel name="parallel_action_3">
+            <DualArm_H name="s11_right_arm_initial" />
+            <DualArm_H name="s11_left_arm_initial" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <RetryUntilSuccessful name="retry_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd1_H name="s12_gripper_open" />
+          <DualArm_H name="s13_right_arm_initial" />
+          <DualArm_H name="s13_left_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_overall" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
+        <Sequence> 
+          <!-- <GripperCmd1_H name="s14_gripper_open" /> -->
+          <DualArm_H name="s14_right_arm_initial" />
+          <DualArm_H name="s14_left_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_vision" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_recovery_subtree">
+    <Sequence name="grasp_recovery_root">
+      <RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd1_H name="s15_gripper_open" />
+          <DualArm_H name="s15_arm_pre_grasp" />
+          <DualArm_H name="s15_right_arm_cam_take_photo" />
+          <DualArm_H name="s15_right_arm_initial" />
+          <DualArm_H name="s15_left_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_grasp" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_recovery_subtree">
+    <Sequence name="putdown_recovery_root">
+      <RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd1_H name="s16_gripper_open" />
+          <DualArm_H name="s16_arm_pre_grasp" />
+          <DualArm_H name="s16_right_arm_cam_take_photo" />
+          <DualArm_H name="s16_right_arm_initial" />
+          <DualArm_H name="s16_left_arm_initial" />
+          <ClearDoneInstances_H name="clear_done_instances_putdown" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>

src/brain/config/robot_config.yaml

@@ -6,12 +6,24 @@
   version: 1.0.0
   config_params_path: [
     {config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
-    {config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml}
+    {config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
+    {config: /config/bt_vision_grasp_left_arm.xml, param: /config/bt_vision_grasp_left_arm.params.yaml},
+    {config: /config/bt_vision_grasp_right_arm.xml, param: /config/bt_vision_grasp_right_arm.params.yaml},
+    {config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml},
+    {config: /config/bt_vision_grasp_left_arm_demo1.xml, param: /config/bt_vision_grasp_left_arm_demo1.params.yaml},
+    {config: /config/bt_vision_grasp_right_arm_demo1.xml, param: /config/bt_vision_grasp_right_arm_demo1.params.yaml},
+    {config: /config/bt_vision_grasp_dual_arm_demo1.xml, param: /config/bt_vision_grasp_dual_arm_demo1.params.yaml}
   ]
 
 - name: cerebellum_node
   version: 1.0.0
   config_params_path: [
     {config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
-    {config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml}
+    {config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
+    {config: /config/bt_vision_grasp_left_arm.xml, param: /config/bt_vision_grasp_left_arm.params.yaml},
+    {config: /config/bt_vision_grasp_right_arm.xml, param: /config/bt_vision_grasp_right_arm.params.yaml},
+    {config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml},
+    {config: /config/bt_vision_grasp_left_arm_demo1.xml, param: /config/bt_vision_grasp_left_arm_demo1.params.yaml},
+    {config: /config/bt_vision_grasp_right_arm_demo1.xml, param: /config/bt_vision_grasp_right_arm_demo1.params.yaml},
+    {config: /config/bt_vision_grasp_dual_arm_demo1.xml, param: /config/bt_vision_grasp_dual_arm_demo1.params.yaml}
   ]

src/brain/config/robot_skills.yaml

@@ -2,19 +2,22 @@
   version: 1.0.0
   description: "视觉识别指定物体"
   interfaces:
-    - VisionObjectRecognition.srv
+    - name: VisionObjectRecognition.srv
+      default_topic: "/vision_object_recognition"
+    - name: VisionObjectRecognition.action
+      default_topic: "/vision_object_recognition"
 
-- name: MapLoad
-  version: 1.0.0
-  description: "加载地图"
-  interfaces:
-    - MapLoad.srv
+# - name: MapLoad
+#   version: 1.0.0
+#   description: "加载地图"
+#   interfaces:
+#     - MapLoad.srv
 
-- name: MapSave
-  version: 1.0.0
-  description: "保存地图"
-  interfaces:
-    - MapSave.srv
+# - name: MapSave
+#   version: 1.0.0
+#   description: "保存地图"
+#   interfaces:
+#     - MapSave.srv
 
 - name: Arm
   version: 1.0.0
@@ -23,17 +26,24 @@
     - name: Arm.action
       default_topic: "ArmAction"
 
+- name: DualArm
+  version: 1.0.0
+  description: "双臂控制"
+  interfaces:
+    - name: DualArm.action
+      default_topic: "DualArm"
+
 - name: CameraTakePhoto
   version: 1.0.0
   description: "相机拍照"
   interfaces:
     - CameraTakePhoto.action
 
-- name: HandControl
-  version: 1.0.0
-  description: "手部控制"
-  interfaces:
-    - HandControl.action
+# - name: HandControl
+#   version: 1.0.0
+#   description: "手部控制"
+#   interfaces:
+#     - HandControl.action
 
 - name: MoveWaist
   version: 1.0.0
@@ -61,4 +71,18 @@
   description: "回到原点位姿"
   interfaces:
     - name: MoveHome.action
-      default_topic: "MoveHome"
+      default_topic: "MoveHome"
+
+- name: GripperCmd0
+  version: 1.0.0
+  description: "夹爪0控制"
+  interfaces:
+    - name: GripperCmd.action
+      default_topic: "gripper_cmd0"
+
+- name: GripperCmd1
+  version: 1.0.0
+  description: "夹爪1控制"
+  interfaces:
+    - name: GripperCmd.action
+      default_topic: "gripper_cmd1"

src/brain/include/brain/action_registry.hpp

@@ -11,6 +11,7 @@
 #include <optional>
 
 #include <rclcpp/rclcpp.hpp>
+#include <hivecore_logger/logger.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 
 /**
@@ -144,11 +145,11 @@ public:
   {
     virtual ~EntryBase() = default;
     /** @brief Send a goal using the entry's callbacks. */
-    virtual void send(rclcpp::Logger logger) = 0;
+    virtual void send() = 0;
     /** @brief Check if server is available within timeout. */
     virtual bool available(std::chrono::nanoseconds timeout) const = 0;
     /** @brief Attempt to cancel last goal if present. */
-    virtual void cancel(rclcpp::Logger logger) = 0;
+    virtual void cancel() = 0;
   };
 
   template<typename ActionT>
@@ -178,16 +179,16 @@ public:
       client = rclcpp_action::create_client<ActionT>(node, name);
     }
 
-    void send(rclcpp::Logger logger) override
+    void send() override
     {
       if (!client) {return;}
-      RCLCPP_INFO(logger, "[ActionClientRegistry] waiting for server '%s'", name.c_str());
+      LOG_INFO("[ActionClientRegistry] waiting for server '{}'", name.c_str());
       if (!client->wait_for_action_server(std::chrono::seconds(10))) {
-        RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
+        LOG_ERROR("Action server '{}' not available", name.c_str());
         return;
       }
       auto goal_msg = make_goal ? make_goal() : Goal{};
-      RCLCPP_INFO(logger, "[ActionClientRegistry] sending goal on '%s'", name.c_str());
+      LOG_INFO("[ActionClientRegistry] sending goal on '{}'", name.c_str());
 
       auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
       if (on_goal_response) {opts.goal_response_callback = on_goal_response;}
@@ -195,7 +196,7 @@ public:
       if (on_result) {opts.result_callback = on_result;}
       auto future_handle = client->async_send_goal(goal_msg, opts);
       std::thread(
-        [future_handle, this, logger]() mutable {
+        [future_handle, this]() mutable {
           if (future_handle.wait_for(std::chrono::seconds(0)) != std::future_status::ready) {
             future_handle.wait();
           }
@@ -203,11 +204,10 @@ public:
             auto gh = future_handle.get();
             if (gh) {
               last_goal_handle = gh;
-              RCLCPP_INFO(logger, "Stored goal handle for '%s'", name.c_str());
+              LOG_INFO("Stored goal handle for '{}'", name.c_str());
             }
           } catch (const std::exception & e) {
-            RCLCPP_WARN(
-              logger, "Failed to obtain goal handle for '%s': %s", name.c_str(),
+            LOG_WARN("Failed to obtain goal handle for '{}': {}", name.c_str(),
               e.what());
           }
         }).detach();
@@ -219,14 +219,14 @@ public:
       return client->wait_for_action_server(timeout);
     }
 
-    void cancel(rclcpp::Logger logger) override
+    void cancel() override
     {
       auto gh = last_goal_handle.lock();
       if (!gh) {
-        RCLCPP_WARN(logger, "No active goal handle to cancel for '%s'", name.c_str());
+        LOG_WARN("No active goal handle to cancel for '{}'", name.c_str());
         return;
       }
-      RCLCPP_INFO(logger, "Cancelling goal on '%s'", name.c_str());
+      LOG_INFO("Cancelling goal on '{}'", name.c_str());
       client->async_cancel_goal(gh);
     }
   };
@@ -249,6 +249,9 @@ public:
     std::function<void(const typename GoalHandle<ActionT>::WrappedResult &)> on_result,
     std::function<void(const std::shared_ptr<GoalHandle<ActionT>> &)> on_goal_response = nullptr)
   {
+    if (entries_.find(name) != entries_.end()) {
+      LOG_WARN("[ActionClientRegistry] Overwriting existing client for '{}'", name.c_str());
+    }
     entries_[name] =
       std::make_unique<Entry<ActionT>>(
       node_, name, std::move(make_goal), std::move(
@@ -260,15 +263,15 @@ public:
    * @param name Action client key.
    * @param logger Logger instance.
    */
-  void send(const std::string & name, rclcpp::Logger logger)
+  void send(const std::string & name)
   {
     auto it = entries_.find(name);
     if (it == entries_.end()) {
-      RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
+      LOG_ERROR("No action client registered for '{}'", name.c_str());
       return;
     }
-    RCLCPP_INFO(logger, "Sending action '%s'", name.c_str());
-    it->second->send(logger);
+    LOG_INFO("Sending action '{}'", name.c_str());
+    it->second->send();
   }
 
   /**
@@ -276,14 +279,14 @@ public:
    * @param name Action client key.
    * @param logger Logger instance.
    */
-  void cancel(const std::string & name, rclcpp::Logger logger)
+  void cancel(const std::string & name)
   {
     auto it = entries_.find(name);
     if (it == entries_.end()) {
-      RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
+      LOG_ERROR("No action client registered for '{}'", name.c_str());
       return;
     }
-    it->second->cancel(logger);
+    it->second->cancel();
   }
 
   /**
@@ -330,22 +333,21 @@ public:
   template<typename ActionT>
   std::optional<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult> send_and_wait(
     const std::string & name,
-    rclcpp::Logger logger,
     std::chrono::nanoseconds timeout = std::chrono::seconds(30))
   {
     auto it = entries_.find(name);
     if (it == entries_.end()) {
-      RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
+      LOG_ERROR("No action client registered for '{}'", name.c_str());
       return std::nullopt;
     }
     auto * e = dynamic_cast<Entry<ActionT> *>(it->second.get());
     if (!e) {
-      RCLCPP_ERROR(logger, "Action client type mismatch for '%s'", name.c_str());
+      LOG_ERROR("Action client type mismatch for '{}'", name.c_str());
       return std::nullopt;
     }
     if (!e->client) {return std::nullopt;}
     if (!e->client->wait_for_action_server(std::chrono::seconds(10))) {
-      RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
+      LOG_ERROR("Action server '{}' not available", name.c_str());
       return std::nullopt;
     }
     auto goal = e->make_goal ? e->make_goal() : typename ActionT::Goal{};
@@ -354,7 +356,7 @@ public:
     auto goal_future = e->client->async_send_goal(goal, opts);
     auto goal_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
     if (goal_future.wait_until(goal_deadline) != std::future_status::ready) {
-      RCLCPP_ERROR(logger, "Timed out waiting for goal response on '%s'", name.c_str());
+      LOG_ERROR("Timed out waiting for goal response on '{}'", name.c_str());
       return std::nullopt;
     }
 
@@ -362,11 +364,11 @@ public:
     try {
       goal_handle = goal_future.get();
     } catch (const std::exception & ex) {
-      RCLCPP_ERROR(logger, "Failed to get goal handle for '%s': %s", name.c_str(), ex.what());
+      LOG_ERROR("Failed to get goal handle for '{}': {}", name.c_str(), ex.what());
       return std::nullopt;
     }
     if (!goal_handle) {
-      RCLCPP_WARN(logger, "Goal rejected for '%s'", name.c_str());
+      LOG_WARN("Goal rejected for '{}'", name.c_str());
       return std::nullopt;
     }
     e->last_goal_handle = goal_handle;
@@ -374,18 +376,18 @@ public:
       try {
         e->on_goal_response(goal_handle);
       } catch (const std::exception & ex) {
-        RCLCPP_WARN(logger, "Goal response callback threw for '%s': %s", name.c_str(), ex.what());
+        LOG_WARN("Goal response callback threw for '{}': {}", name.c_str(), ex.what());
       }
     }
 
     auto result_future = e->client->async_get_result(goal_handle);
     auto deadline = std::chrono::steady_clock::now() + timeout;
     if (result_future.wait_until(deadline) != std::future_status::ready) {
-      RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
+      LOG_ERROR("Timed out waiting for result on '{}', sending cancel", name.c_str());
       try {
         e->client->async_cancel_goal(goal_handle);
       } catch (const std::exception & ex) {
-        RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
+        LOG_WARN("Failed cancelling goal for '{}': {}", name.c_str(), ex.what());
       }
       return std::nullopt;
     }
@@ -393,14 +395,94 @@ public:
     try {
       wrapped_result = result_future.get();
     } catch (const std::exception & ex) {
-      RCLCPP_ERROR(logger, "Failed to get result for '%s': %s", name.c_str(), ex.what());
+      LOG_ERROR("Failed to get result for '{}': {}", name.c_str(), ex.what());
       return std::nullopt;
     }
     if (e->on_result) {
       try {
         e->on_result(wrapped_result);
       } catch (const std::exception & ex) {
-        RCLCPP_WARN(logger, "Result callback threw for '%s': %s", name.c_str(), ex.what());
+        LOG_WARN("Result callback threw for '{}': {}", name.c_str(), ex.what());
+      }
+    }
+    return wrapped_result;
+  }
+
+  template<typename ActionT>
+  std::optional<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult> send_and_wait_with_goal(
+    const std::string & name,
+    const typename ActionT::Goal & goal,
+    std::chrono::nanoseconds timeout = std::chrono::seconds(30))
+  {
+    auto it = entries_.find(name);
+    if (it == entries_.end()) {
+      LOG_ERROR("No action client registered for '{}'", name.c_str());
+      return std::nullopt;
+    }
+    auto * e = dynamic_cast<Entry<ActionT> *>(it->second.get());
+    if (!e) {
+      LOG_ERROR("Action client type mismatch for '{}'", name.c_str());
+      return std::nullopt;
+    }
+    if (!e->client) {return std::nullopt;}
+    if (!e->client->wait_for_action_server(std::chrono::seconds(10))) {
+      LOG_ERROR("Action server '{}' not available", name.c_str());
+      return std::nullopt;
+    }
+    auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
+    if (e->on_feedback) {opts.feedback_callback = e->on_feedback;}
+    if (e->on_result) {opts.result_callback = e->on_result;}
+    if (e->on_goal_response) {opts.goal_response_callback = e->on_goal_response;}
+    auto goal_future = e->client->async_send_goal(goal, opts);
+    auto goal_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
+    if (goal_future.wait_until(goal_deadline) != std::future_status::ready) {
+      LOG_ERROR("Timed out waiting for goal response on '{}'", name.c_str());
+      return std::nullopt;
+    }
+
+    typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle;
+    try {
+      goal_handle = goal_future.get();
+    } catch (const std::exception & ex) {
+      LOG_ERROR("Failed to get goal handle for '{}': {}", name.c_str(), ex.what());
+      return std::nullopt;
+    }
+    if (!goal_handle) {
+      LOG_WARN("Goal rejected for '{}'", name.c_str());
+      return std::nullopt;
+    }
+    e->last_goal_handle = goal_handle;
+    if (e->on_goal_response) {
+      try {
+        e->on_goal_response(goal_handle);
+      } catch (const std::exception & ex) {
+        LOG_WARN("Goal response callback threw for '{}': {}", name.c_str(), ex.what());
+      }
+    }
+
+    auto result_future = e->client->async_get_result(goal_handle);
+    auto deadline = std::chrono::steady_clock::now() + timeout;
+    if (result_future.wait_until(deadline) != std::future_status::ready) {
+      LOG_ERROR("Timed out waiting for result on '{}', sending cancel", name.c_str());
+      try {
+        e->client->async_cancel_goal(goal_handle);
+      } catch (const std::exception & ex) {
+        LOG_WARN("Failed cancelling goal for '{}': {}", name.c_str(), ex.what());
+      }
+      return std::nullopt;
+    }
+    typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult wrapped_result;
+    try {
+      wrapped_result = result_future.get();
+    } catch (const std::exception & ex) {
+      LOG_ERROR("Failed to get result for '{}': {}", name.c_str(), ex.what());
+      return std::nullopt;
+    }
+    if (e->on_result) {
+      try {
+        e->on_result(wrapped_result);
+      } catch (const std::exception & ex) {
+        LOG_WARN("Result callback threw for '{}': {}", name.c_str(), ex.what());
       }
     }
     return wrapped_result;

src/brain/include/brain/bt_registry.hpp

@@ -25,6 +25,7 @@
 #include <behaviortree_cpp/bt_factory.h>
 #include <behaviortree_cpp/action_node.h>
 #include <rclcpp/rclcpp.hpp>
+#include <hivecore_logger/logger.hpp>
 #include <functional>
 #include <map>
 #include <string>
@@ -86,9 +87,12 @@ public:
       node) {}
 
   /**
-   * @brief Declare static ports. Currently none (empty list) simplifying dynamic XML generation.
+   * @brief Declare static ports. Added "match" for selective clearing logic in ClearDoneInstances_H.
    */
-  static BT::PortsList providedPorts() {return {};}
+  static BT::PortsList providedPorts()
+  {
+    return {BT::InputPort<std::string>("match")};
+  }
 
   /**
    * @brief First tick transition handler.
@@ -200,12 +204,12 @@ public:
     auto it = bt_sequences_.find(seq_name);
     if (it == bt_sequences_.end()) {
       if (node_) {
-        RCLCPP_ERROR(node_->get_logger(), "BT sequence '%s' not found", seq_name.c_str());
+        LOG_ERROR("BT sequence '{}' not found", seq_name.c_str());
       }
       return false;
     }
     const std::string xml_text = build_xml_from_sequence(it->second);
-    RCLCPP_INFO(node_->get_logger(), "Building BT sequence '%s'", xml_text.c_str());
+    LOG_INFO("Building BT sequence '{}'", xml_text.c_str());
     out_tree = factory_.createTreeFromText(xml_text);
     return true;
   }
@@ -223,7 +227,7 @@ public:
       return true;
     } catch (const std::exception & e) {
       if (node_) {
-        RCLCPP_ERROR(node_->get_logger(), "Failed to build BT from XML text: %s", e.what());
+        LOG_ERROR("Failed to build BT from XML text: {}", e.what());
       }
       return false;
     }
@@ -245,7 +249,7 @@ public:
     std::ifstream ifs(file_path);
     if (!ifs.is_open()) {
       if (node_) {
-        RCLCPP_ERROR(node_->get_logger(), "Could not open BT XML file: %s", file_path.c_str());
+        LOG_ERROR("Could not open BT XML file: {}", file_path.c_str());
       }
       return false;
     }
@@ -253,12 +257,12 @@ public:
     buffer << ifs.rdbuf();
     if (buffer.str().empty()) {
       if (node_) {
-        RCLCPP_ERROR(node_->get_logger(), "BT XML file empty: %s", file_path.c_str());
+        LOG_ERROR("BT XML file empty: {}", file_path.c_str());
       }
       return false;
     }
     if (node_) {
-      RCLCPP_INFO(node_->get_logger(), "Building BT from XML file: %s \n%s", file_path.c_str(), buffer.str().c_str());
+      LOG_INFO("Building BT from XML file: {} \n{}", file_path.c_str(), buffer.str().c_str());
     }
     return build_tree_from_xml_text(buffer.str(), out_tree);
   }
@@ -274,8 +278,11 @@ private:
     std::ostringstream oss;
     oss <<
       "\n    <root BTCPP_format=\"4\" >\n\n        <BehaviorTree ID=\"MainTree\">\n            <Sequence name=\"root_sequence\">\n";
-    for (const auto & act : seq) {
-      oss << "                <" << act.type << " name=\"" << act.name << "\"";
+    auto timestamp = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+    for (size_t i = 0; i < seq.size(); ++i) {
+      const auto & act = seq[i];
+      // Append timestamp and sequence index to ensure global uniqueness of the instance name in the tree
+      oss << "                <" << act.type << " name=\"" << act.name << "_" << timestamp << "_" << i << "\"";
       for (const auto & kv : act.ports) {
         oss << " " << kv.first << "=\"" << kv.second << "\"";
       }

src/brain/include/brain/calc_grasp_pose.hpp

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Transform.h>
+#include <string>
+#include <vector>
+#include <functional>
+
+namespace brain {
+
+struct Pose {
+    tf2::Vector3 position;
+    tf2::Quaternion orientation;
+};
+
+struct GraspResult {
+    std::string name;
+    Pose pre_grasp_pose;
+    Pose grasp_pose;
+};
+
+class GraspPoseCalculator {
+public:
+    GraspPoseCalculator();
+    static GraspResult calculate(
+        const tf2::Vector3& target_pos,
+        const tf2::Quaternion& target_quat,
+        const std::string& grasp_type = "side",
+        double angle_deg = 0.0,
+        const std::string& palm_facing = "down",
+        double safety_dist = 0.06,
+        double finger_length = 0.0,
+        const std::string& arm = "right"
+    );
+    static double evaluate_grasp_quality(
+        const tf2::Vector3& target_pos,
+        double angle_deg,
+        const std::string& arm = "right"
+    );
+    static std::vector<double> find_optimal_grasp_angles(
+        const tf2::Vector3& target_pos,
+        const std::string& arm = "right",
+        int top_k = 3
+    );
+
+private:
+    static bool rm_arm_algo_init_done_;
+
+    static GraspResult create_grasp_matrix(
+        const tf2::Vector3& target_pos,
+        const tf2::Vector3& approach_vec,
+        const tf2::Vector3& palm_normal_vec,
+        double safety_dist,
+        double finger_length
+    );
+
+};
+
+} // namespace brain

src/brain/include/brain/cerebellum_hooks.hpp

@@ -0,0 +1,36 @@
+#ifndef BRAIN_CEREBELLUM_HOOKS_HPP_
+#define BRAIN_CEREBELLUM_HOOKS_HPP_
+
+namespace brain
+{
+
+class CerebellumNode;
+
+class CerebellumHooks
+{
+public:
+  static void ConfigureActionHooks(CerebellumNode * node);
+  static void ConfigureArmSpaceControlHooks(CerebellumNode * node);
+  static void ConfigureArmHooks(CerebellumNode * node);
+  static void ConfigureDualArmHooks(CerebellumNode * node);
+  static void ConfigureHandControlHooks(CerebellumNode * node);
+  static void ConfigureCameraTakePhotoHooks(CerebellumNode * node);
+  static void ConfigureLegControlHooks(CerebellumNode * node);
+  static void ConfigureVisionGraspObjectHooks(CerebellumNode * node);
+  static void ConfigureSlamModeHooks(CerebellumNode * node);
+  static void ConfigureNavigateToPoseHooks(CerebellumNode * node);
+  static void ConfigureMoveWaistHooks(CerebellumNode * node);
+  static void ConfigureMoveLegHooks(CerebellumNode * node);
+  static void ConfigureMoveWheelHooks(CerebellumNode * node);
+  static void ConfigureMoveHomeHooks(CerebellumNode * node);
+  static void ConfigureGripperCmdControlHooks(CerebellumNode * node);
+
+  static void ConfigureServiceHooks(CerebellumNode * node);
+  static void ConfigureVisionObjectRecognitionHooks(CerebellumNode * node);
+  static void ConfigureMapLoadHooks(CerebellumNode * node);
+  static void ConfigureMapSaveHooks(CerebellumNode * node);
+};
+
+}  // namespace brain
+
+#endif  // BRAIN_CEREBELLUM_HOOKS_HPP_

src/brain/include/brain/cerebellum_node.hpp

@@ -4,7 +4,10 @@
 #include <atomic>
 #include <limits>
 #include <memory>
+#include <vector>
+#include <unordered_map>
 #include <rclcpp/rclcpp.hpp>
+#include <hivecore_logger/logger.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <std_msgs/msg/string.hpp>
@@ -18,8 +21,11 @@
 #include <smacc2/smacc_signal_detector.hpp>
 #include "brain/sm_cerebellum.hpp"  // for CerebellumData::ExecResult
 // For from_yaml<T>(YAML::Node)
+#include <sensor_msgs/msg/joint_state.hpp>
 #include "brain/payload_converters.hpp"
 #include "brain/robot_config.hpp"
+#include "brain/calc_grasp_pose.hpp"
+#include "interfaces/srv/inverse_kinematics.hpp"
 
 namespace brain
 {
@@ -41,7 +47,34 @@ public:
    */
   explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
+  std::unordered_map<std::string, std::vector<geometry_msgs::msg::Pose>> arm_poses_;
+  std::unordered_map<std::string, std::vector<std::vector<double>>> arm_angles_;
+
+  std::vector<double> left_arm_joint_angles_;
+  std::vector<double> right_arm_joint_angles_;
+  std::mutex joint_state_mutex_;
+
+  std::string camera_position_;
+
+  std::unordered_map<std::string, std::string> gripper_info_;
+  
+  /**
+   * @brief Call Inverse Kinematics service
+   * 
+   * @param pos 
+   * @param quat 
+   * @param angles 
+   * @param arm_id 
+   * @return int 
+   */
+  int CallInverseKinematics(const tf2::Vector3& pos, const tf2::Quaternion& quat, std::vector<double>& angles, int arm_id);
+
 private:
+  struct GoalExecutionContext { std::atomic<bool> cancel{false}; };
+  std::unordered_map<std::uintptr_t, std::shared_ptr<GoalExecutionContext>> goal_ctxs_;
+  std::mutex goal_ctxs_mutex_;
+  static thread_local const std::vector<interfaces::msg::SkillCall>* tls_current_calls_;
+  static thread_local int tls_arm_body_id;
   // Generic action registry to support multiple actions via registration
   std::unique_ptr<ActionServerRegistry> action_registry_;
   std::unique_ptr<ActionClientRegistry> action_clients_;
@@ -53,19 +86,65 @@ private:
 
   // SMACC2 execution context
   std::unique_ptr<smacc2::SmExecution> sm_exec_;
+  
+  // IK Client
+  rclcpp::Client<interfaces::srv::InverseKinematics>::SharedPtr ik_client_;
 
-  // Parameters: abort policy, default action timeout, specific grasp timeout
-  bool abort_on_failure_ {true};
-  double default_action_timeout_sec_ {45.0};
-  double vision_grasp_object_timeout_sec_ {120.0};
-  std::string map_save_url_ {"/tmp/humanoid_map"};
-  std::string map_save_image_format_ {"pgm"};
-  double map_save_free_thresh_ {0.25};
-  double map_save_occupied_thresh_ {0.65};
-  std::string map_load_url_ {};
-  double nav_goal_yaw_offset_ {0.0};
-  double nav_goal_distance_tolerance_ {0.5};
-  bool slam_enable_mapping_default_ {true};
+  // ROS parameters (replacing hardcoded defaults)
+  bool abort_on_failure_{true};
+  double default_action_timeout_sec_{45.0};
+  double vision_grasp_object_timeout_sec_{120.0};
+  std::string map_save_url_{"/tmp/humanoid_map"};
+  std::string map_save_image_format_{"pgm"};
+  double map_save_free_thresh_{0.25};
+  double map_save_occupied_thresh_{0.65};
+  std::string map_load_url_{};
+  double nav_goal_yaw_offset_{0.0};
+  double nav_goal_distance_tolerance_{0.5};
+  bool slam_enable_mapping_default_{true};
+  
+  // Grasp pose calculation parameters
+  double top_cam_right_arm_z_threshold_{0.05};
+  double top_cam_right_arm_x_offset_{0.10};
+  double top_cam_right_arm_y_{0.33};
+  std::vector<double> top_cam_right_arm_orientation_{-0.7071, 0.0, 0.0, 0.7071};
+  double top_cam_left_arm_z_threshold_{-0.10};
+  double top_cam_left_arm_x_offset_{0.10};
+  double top_cam_left_arm_y_{-0.33};
+  std::vector<double> top_cam_left_arm_orientation_{0.7071, 0.0, 0.0, 0.7071};
+  double side_cam_right_arm_y_offset_{0.02};
+  double side_cam_right_arm_z_offset_{0.02};
+  double side_cam_left_arm_y_offset_{-0.02};
+  double side_cam_left_arm_z_offset_{0.01};
+  double take_object_arm_x_{-0.05};
+  double vision_object_recognition_wait_timeout_sec_{2.0};
+  double vision_object_recognition_call_timeout_sec_{5.0};
+  double map_load_wait_timeout_sec_{2.0};
+  double map_load_call_timeout_sec_{5.0};
+  
+  std::string left_camera_frame_{"LeftLink6"};
+  std::string right_camera_frame_{"RightLink6"};
+  std::vector<std::string> target_frames_{"apple", "bottle", "medicine_box"};
+  std::string target_frame_{"medicine_box"};
+  std::vector<std::string> grasp_palm_facings_{"down"};
+  std::vector<double> grasp_best_angles_{0.0};
+  std::string grasp_type_{"side"};
+  double grasp_safety_dist_{0.05};
+  double grasp_finger_length_{0.20};
+  
+  int hand_control_default_mode_{1};
+  int gripper_default_loc_{30};
+  int gripper_default_speed_{0};
+  int gripper_default_torque_{0};
+  int gripper_default_mode_{0};
+
+  int arm_cmd_type_take_photo_{100};
+  int arm_cmd_type_pre_grasp_{101};
+  int arm_cmd_type_grasp_{102};
+  int arm_cmd_type_take_object_{110};
+  int arm_cmd_type_custom_min_{100};
+  int arm_cmd_type_custom_max_{120};
+  std::string dual_arm_motion_type_{"MOVEJ"};
 
   // Optional per-skill override timeout (skill_name -> seconds)
   std::unordered_map<std::string, double> skill_timeouts_;
@@ -73,19 +152,31 @@ private:
   // Statistics: per-skill success/failure counters & sequence totals
   std::unordered_map<std::string, size_t> skill_success_counts_;
   std::unordered_map<std::string, size_t> skill_failure_counts_;
-  size_t total_sequence_success_ {0};
-  size_t total_sequence_failure_ {0};
+  size_t total_sequence_success_{0};
+  size_t total_sequence_failure_{0};
+
+  // ROS parameters (new additions)
+  bool enable_stats_publisher_{true};
+  int stats_publish_interval_ms_{5000};
+  std::string robot_config_path_{""};
+
   rclcpp::TimerBase::SharedPtr stats_timer_;
   double stats_log_interval_sec_ {30.0};
   std::mutex stats_mutex_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_pub_;
   std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
-  std::string target_frame_{"bottle"};
   int64_t command_id_{0};
+  int64_t command_id_left_arm_ {0};
+  int64_t command_id_right_arm_{0};
+  std::unordered_map<std::string, geometry_msgs::msg::Pose> final_arm_pose_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
-  std::string arm_target_frame_{"base_link_leftarm"};
-  double arm_transform_timeout_sec_{0.2};
+  std::string arm_target_frame_{"base_link_rightarm"};
+  double arm_transform_timeout_sec_{1};
+  
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
+  void JointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
+  std::vector<double> right_hand_grab_width_;
 
   // Current accepted sequence epoch (from client) used to filter/reject stale goals
   std::atomic<uint32_t> current_epoch_{0};
@@ -103,6 +194,8 @@ private:
   std::atomic<double> nav2_last_distance_remaining_{std::numeric_limits<double>::quiet_NaN()};
   std::atomic<bool> nav2_last_goal_succeeded_{false};
 
+  friend class CerebellumHooks;
+
   // ---- helpers ----
   /**
    * @brief Try to parse the current goal's per-call YAML payload into a typed object.
@@ -114,16 +207,17 @@ private:
   template<typename T>
   bool TryParseCallPayload(const std::string & skill_name, T & out) const
   {
-    if (last_calls_.empty()) {return false;}
-    for (const auto & c : last_calls_) {
+    const auto * calls = tls_current_calls_ ? tls_current_calls_ : &last_calls_;
+    if (!calls || calls->empty()) {return false;}
+    for (const auto & c : *calls) {
       if (c.name == skill_name && !c.payload_yaml.empty()) {
         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());
+          LOG_INFO("[{}] payload_yaml parse success", skill_name.c_str());
           return true;
         } catch (const std::exception & e) {
-          RCLCPP_ERROR(rclcpp::Node::get_logger(), "[%s] payload_yaml parse failed: %s",
+          LOG_ERROR("[{}] payload_yaml parse failed: {}",
             skill_name.c_str(), e.what());
         }
         break;
@@ -131,26 +225,6 @@ private:
     }
     return false;
   }
-  /** @brief Install application-specific action hook overrides for skill manager. */
-  void ConfigureActionHooks();
-  void ConfigureArmSpaceControlHooks();
-  void ConfigureArmHooks();
-  void ConfigureHandControlHooks();
-  void ConfigureCameraTakePhotoHooks();
-  void ConfigureLegControlHooks();
-  void ConfigureVisionGraspObjectHooks();
-  void ConfigureSlamModeHooks();
-  void ConfigureNavigateToPoseHooks();
-  void ConfigureMoveWaistHooks();
-  void ConfigureMoveLegHooks();
-  void ConfigureMoveWheelHooks();
-  void ConfigureMoveHomeHooks();
-
-  /** @brief Install application-specific service hook overrides. */
-  void ConfigureServiceHooks();
-  void ConfigureVisionObjectRecognitionHooks();
-  void ConfigureMapLoadHooks();
-  void ConfigureMapSaveHooks();
 
   /**
    * @brief Parse a raw comma / semicolon / whitespace separated list of skill names.
@@ -258,6 +332,7 @@ private:
    * @brief Internal helper: run the action worker thread and emit RUN feedback until completion.
    * Extracted to reduce cyclomatic complexity of ExecuteActionSkill.
    * @param skill Skill name.
+   * @param interface_name Interface base name for dispatching.
    * @param topic Action topic.
    * @param timeout_ns Timeout duration (nanoseconds).
    * @param index Skill index.
@@ -268,8 +343,11 @@ private:
    */
   bool RunActionSkillWithProgress(
     const std::string & skill,
+    const std::string & interface_name,
     const std::string & topic,
     std::chrono::nanoseconds timeout_ns,
+    const std::string & instance_name,
+    uint32_t timeout_ms_override,
     int index,
     int total_skills,
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
@@ -279,6 +357,7 @@ private:
    * @brief Internal helper: run bilateral arm actions concurrently.
    * Extracted to reduce cyclomatic complexity of ExecuteActionSkill.
    * @param skill Skill name.
+   * @param interface_name Interface base name for dispatching.
    * @param topic Action topic.
    * @param timeout Action timeout.
    * @param index Skill index.
@@ -289,8 +368,11 @@ private:
    */
   bool ExecuteBilateralArmAction(
     const std::string & skill,
+    const std::string & interface_name,
     const std::string & topic,
     std::chrono::nanoseconds timeout,
+    const std::string & instance_name,
+    uint32_t timeout_ms_override,
     int index,
     int total_skills,
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
@@ -303,7 +385,7 @@ private:
   * @param detail [out] Detail / diagnostics.
   * @param start_progress Sequence progress prior to executing the service.
   * @param goal_handle Goal handle (used for feedback publishing).
-   * @return true if successful.
+  * @return true if successful.
    */
   bool ExecuteServiceSkill(
     const std::string & skill,
@@ -355,6 +437,9 @@ private:
    * @return Topic / action client key.
    */
   std::string ResolveTopicForSkill(const std::string & skill) const;
+  std::string ResolveTopicForSkillFromCalls(
+    const std::vector<interfaces::msg::SkillCall> & calls,
+    const std::string & skill) const;
   /**
    * @brief Finalize goal with summary result (success or abort) and log message.
    * @param goal_handle Goal handle.
@@ -379,9 +464,17 @@ private:
   /** @brief Register ExecuteBtAction server with goal/execute handlers. */
   void SetupExecuteBtServer();
 
+  void RunExecuteBtGoalConcurrent(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
+
+  brain::CerebellumData::ExecResult ExecuteSequenceConcurrent(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::vector<std::string> & skills,
+    const std::shared_ptr<GoalExecutionContext> & ctx);
+
   /** @brief 将检测到的姿态转换到机械臂参考坐标系。 */
   bool TransformPoseToArmFrame(
-    const geometry_msgs::msg::PoseStamped & source,
+    const geometry_msgs::msg::PoseStamped & source, const std::string & arm_target_frame,
     geometry_msgs::msg::PoseStamped & transformed) const;
 
   // NOTE: Additional future helpers (cancellation handling, concurrency) may be added here.

src/brain/include/brain/cerebrum_node.hpp

@@ -118,6 +118,20 @@ public:
    */
   static std::vector<std::string> ParseListString(const std::string & raw);
 
+  /**
+   * @brief Split a string by comma, ignoring commas inside quotes (single or double).
+   * @param raw Input string.
+   * @return Vector of tokens.
+   */
+  static std::vector<std::string> SplitQuotedString(const std::string & raw);
+
+  /**
+   * @brief Split a string by comma, preserving order and duplicates.
+   * @param raw Input string.
+   * @return Vector of tokens.
+   */
+  static std::vector<std::string> SplitString(const std::string & raw);
+
   /**
    * @brief Classify textual result detail (prefix or code= token) into ExecResultCode.
    * @param detail Input detail string.
@@ -173,7 +187,10 @@ private:
   /** Declare per-skill instance parameter entries used to customize outgoing calls. */
   bool UpdateBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name, const std::string & instance_params);
   /** Declare per-instance parameter entries used to customize a specific BT node occurrence. */
-  bool DeclareBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name);
+  bool DeclareBtActionParamsForSkillInstance(
+    const std::string & skill_name,
+    const std::string & instance_name,
+    std::string * out_instance_params = nullptr);
   /** Build a SkillCall message for given skill by consulting SkillSpec and per-skill params. */
   interfaces::msg::SkillCall BuildSkillCallForSkill(const std::string & skill_name) const;
   /** Build a SkillCall with optional per-instance override (cerebrum.bt.<Skill>.<Instance>.*). */
@@ -203,13 +220,25 @@ private:
   rclcpp::Service<interfaces::srv::BtRebuild>::SharedPtr rebuild_service_;
   std::atomic<bool> bt_updating_{false};
   std::atomic<bool> bt_pending_run_{false};
+  rclcpp::Time bt_execution_start_time_;
   // Encapsulated epoch+skill filter
   EpochSkillFilter epoch_filter_;
   // Current sequence skills (kept in original order for logging / statistics).
   std::vector<std::string> current_sequence_skills_;
+  // Map of sequence index to override params for the current sequence.
+  std::unordered_map<size_t, std::string> active_sequence_param_overrides_;
   // Mutex that protects both per_node_exec_ and epoch_skills_ updates.
   std::mutex exec_mutex_;
 
+  // Tracking currently running BT leaf instances (for serial/parallel reporting).
+  std::unordered_set<std::string> running_leaf_keys_;
+  // Index map for active RobotWorkInfo.action entries (key -> vector index).
+  std::unordered_map<std::string, std::size_t> action_index_by_key_;
+  // Parallel vector of action keys aligned with robot_work_info_.action.
+  std::vector<std::string> action_keys_;
+  // Map from BT node instance name to execution mode based on BT structure.
+  std::unordered_map<std::string, std::string> action_exec_by_node_name_;
+
   rclcpp::Publisher<interfaces::msg::RobotWorkInfo>::SharedPtr robot_work_info_pub_;
   interfaces::msg::RobotWorkInfo robot_work_info_;
 
@@ -237,6 +266,18 @@ private:
     std::optional<std::string> current_instance;
   };
   std::unordered_map<std::string, PerNodeExecInfo> per_node_exec_;
+
+  struct PerInstanceExecInfo
+  {
+    std::atomic<bool> in_flight {false};
+    std::atomic<bool> finished {false};
+    bool success {false};
+    ExecResultCode result_code {ExecResultCode::UNKNOWN};
+    rclcpp::Time start_time;
+    double last_progress {0.0};
+  };
+  std::unordered_map<std::string, PerInstanceExecInfo> per_instance_exec_;
+  std::unordered_set<std::string> done_instances_epoch_;
   // 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).
@@ -253,11 +294,24 @@ private:
   std::vector<std::string> allowed_action_skills_;
   // Fixed skill sequence (if set, random selection is skipped entirely).
   std::optional<std::vector<std::string>> fixed_skill_sequence_;
+  
+  // ROS parameters
+  double tick_rate_hz_{10.0};
+  int rebuild_interval_ms_{5000};
+  int stats_publish_interval_ms_{10000};
+  std::string robot_config_path_{""};
+  std::string skill_file_path_{""};
+  double default_bt_timeout_{300.0};
+  bool enable_sequence_rebuild_{true};
+
   /**
    * @brief Cancel the currently active ExecuteBtAction goal (if any).
    * @thread_safety Calls into ActionClientRegistry; assumed thread-safe for cancellation.
    */
   void CancelActiveExecuteBtGoal();
+
+  void StopBehaviorTree(bool cancel_goals);
+
   /**
    * @brief (Legacy) Update per-epoch allowed skills. Currently a no-op (handled by EpochSkillFilter).
    * @param skills Ignored.
@@ -366,16 +420,33 @@ private:
   
   /** Handle Trigger type rebuild request */
   void HandleTriggerRebuild(
+    const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
     const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp);
   
-  /** Handle Remote type rebuild request */
-  void HandleRemoteRebuild(
+  /** Handle Generic (Remote/Sequence) type rebuild request */
+  void HandleGenericRebuild(
     const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
     const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp);
   
   /** Extract filename without extension from a path */
   std::string ExtractFilenameWithoutExtension(const std::string & path);
 
+  // Update robot_work_info_ leaf execution mode based on running_leaf_keys_.
+  void UpdateLeafParallelismStatusLocked();
+
+  // Manage RobotWorkInfo.action entries (guarded by exec_mutex_).
+  void AddActionInfoLocked(
+    const std::string & key,
+    const std::string & skill,
+    const std::string & action_name,
+    const std::string & params);
+  void UpdateActionInfoStateLocked(const std::string & key, const std::string & state);
+  void RemoveActionInfoLocked(const std::string & key);
+  void ClearActionInfoLocked();
+
+  // Build map of BT node instance name -> execution mode (serial/parallel).
+  void BuildActionExecutionMapLocked();
+
   // Temporary override used so the generic ExecuteBtAction goal builder can
   // send a single-skill goal when a per-skill BT node starts.
   std::optional<std::string> single_skill_goal_override_;

src/brain/include/brain/payload_converters.hpp

@@ -9,6 +9,9 @@
 #include "interfaces/action/arm.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 #include "interfaces/action/camera_take_photo.hpp"
+#include "interfaces/action/dual_arm.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/gripper_cmd.hpp"
 #include "interfaces/action/hand_control.hpp"
 #include "interfaces/action/leg_control.hpp"
 #include "interfaces/action/move_home.hpp"
@@ -18,8 +21,21 @@
 #include "interfaces/action/move_wheel.hpp"
 #include "interfaces/action/slam_mode.hpp"
 #include "interfaces/action/vision_grasp_object.hpp"
+#include "interfaces/action/vision_object_recognition.hpp"
+#include "interfaces/msg/arm_motion_params.hpp"
+#include "interfaces/msg/skill_call.hpp"
+#include "interfaces/srv/asr_recognize.hpp"
+#include "interfaces/srv/audio_data.hpp"
+#include "interfaces/srv/bt_rebuild.hpp"
+#include "interfaces/srv/clear_arm_error.hpp"
+#include "interfaces/srv/gripper_cmd.hpp"
+#include "interfaces/srv/inverse_kinematics.hpp"
 #include "interfaces/srv/map_load.hpp"
 #include "interfaces/srv/map_save.hpp"
+#include "interfaces/srv/motor_info.hpp"
+#include "interfaces/srv/motor_param.hpp"
+#include "interfaces/srv/tts_synthesize.hpp"
+#include "interfaces/srv/vad_event.hpp"
 #include "interfaces/srv/vision_object_recognition.hpp"
 
 
@@ -28,37 +44,34 @@ namespace brain {
 template<typename T>
 T from_yaml(const YAML::Node & n);
 
-// VisionObjectRecognition::Request: camera_position
+// VisionObjectRecognition::Request: camera_position, classes
 template<>
 inline interfaces::srv::VisionObjectRecognition::Request from_yaml<interfaces::srv::VisionObjectRecognition::Request>(const YAML::Node & n)
 {
   interfaces::srv::VisionObjectRecognition::Request r;
   if (n && n.IsMap()) {
     if (n["camera_position"]) r.camera_position = n["camera_position"].as<std::string>();
+    if (n["classes"]) {
+      auto vec = n["classes"].as<std::vector<std::string>>();
+      r.classes.assign(vec.begin(), vec.end());
+    }
   }
   return r;
 }
 
-// MapLoad::Request: map_url
+// VisionObjectRecognition::Goal: camera_position, classes
 template<>
-inline interfaces::srv::MapLoad::Request from_yaml<interfaces::srv::MapLoad::Request>(const YAML::Node & n)
+inline interfaces::action::VisionObjectRecognition::Goal from_yaml<interfaces::action::VisionObjectRecognition::Goal>(const YAML::Node & n)
 {
-  interfaces::srv::MapLoad::Request r;
+  interfaces::action::VisionObjectRecognition::Goal g;
   if (n && n.IsMap()) {
-    if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
+    if (n["camera_position"]) g.camera_position = n["camera_position"].as<std::string>();
+    if (n["classes"]) {
+      auto vec = n["classes"].as<std::vector<std::string>>();
+      g.classes.assign(vec.begin(), vec.end());
+    }
   }
-  return r;
-}
-
-// MapSave::Request: map_url
-template<>
-inline interfaces::srv::MapSave::Request from_yaml<interfaces::srv::MapSave::Request>(const YAML::Node & n)
-{
-  interfaces::srv::MapSave::Request r;
-  if (n && n.IsMap()) {
-    if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
-  }
-  return r;
+  return g;
 }
 
 // Arm::Goal: body_id, data_type, data_length, command_id, frame_time_stamp, data_array
@@ -84,6 +97,43 @@ inline interfaces::action::Arm::Goal from_yaml<interfaces::action::Arm::Goal>(co
   return g;
 }
 
+// DualArm::Goal: arm_motion_params, velocity_scaling
+template<>
+inline interfaces::action::DualArm::Goal from_yaml<interfaces::action::DualArm::Goal>(const YAML::Node & n)
+{
+  interfaces::action::DualArm::Goal g;
+  if (n && n.IsMap()) {
+    if (n["arm_motion_params"] && n["arm_motion_params"].IsSequence()) {
+      for (const auto & item : n["arm_motion_params"]) {
+        interfaces::msg::ArmMotionParams tmp;
+        if (item["arm_id"]) tmp.arm_id = item["arm_id"].as<uint8_t>();
+        if (item["motion_type"]) tmp.motion_type = item["motion_type"].as<uint8_t>();
+        if (auto node_1_target_pose = item["target_pose"]) {
+          if (auto node_2_position = node_1_target_pose["position"]) {
+            if (node_2_position["x"]) tmp.target_pose.position.x = node_2_position["x"].as<double>();
+            if (node_2_position["y"]) tmp.target_pose.position.y = node_2_position["y"].as<double>();
+            if (node_2_position["z"]) tmp.target_pose.position.z = node_2_position["z"].as<double>();
+          }
+          if (auto node_2_orientation = node_1_target_pose["orientation"]) {
+            if (node_2_orientation["x"]) tmp.target_pose.orientation.x = node_2_orientation["x"].as<double>();
+            if (node_2_orientation["y"]) tmp.target_pose.orientation.y = node_2_orientation["y"].as<double>();
+            if (node_2_orientation["z"]) tmp.target_pose.orientation.z = node_2_orientation["z"].as<double>();
+            if (node_2_orientation["w"]) tmp.target_pose.orientation.w = node_2_orientation["w"].as<double>();
+          }
+        }
+        if (item["target_joints"]) {
+          auto vec = item["target_joints"].as<std::vector<double>>();
+          tmp.target_joints.assign(vec.begin(), vec.end());
+        }
+        if (item["blend_radius"]) tmp.blend_radius = item["blend_radius"].as<int32_t>();
+        g.arm_motion_params.push_back(tmp);
+      }
+    }
+    if (n["velocity_scaling"]) g.velocity_scaling = n["velocity_scaling"].as<int32_t>();
+  }
+  return g;
+}
+
 // CameraTakePhoto::Goal: mode, effort
 template<>
 inline interfaces::action::CameraTakePhoto::Goal from_yaml<interfaces::action::CameraTakePhoto::Goal>(const YAML::Node & n)
@@ -96,18 +146,6 @@ inline interfaces::action::CameraTakePhoto::Goal from_yaml<interfaces::action::C
   return g;
 }
 
-// HandControl::Goal: mode, effort
-template<>
-inline interfaces::action::HandControl::Goal from_yaml<interfaces::action::HandControl::Goal>(const YAML::Node & n)
-{
-  interfaces::action::HandControl::Goal g;
-  if (n && n.IsMap()) {
-    if (n["mode"]) g.mode = n["mode"].as<int32_t>();
-    if (n["effort"]) g.effort = n["effort"].as<float>();
-  }
-  return g;
-}
-
 // MoveWaist::Goal: move_pitch_degree, move_yaw_degree
 template<>
 inline interfaces::action::MoveWaist::Goal from_yaml<interfaces::action::MoveWaist::Goal>(const YAML::Node & n)
@@ -153,14 +191,59 @@ inline interfaces::action::MoveHome::Goal from_yaml<interfaces::action::MoveHome
   return g;
 }
 
-// MoveToPosition::Goal: target_x, target_y
+// GripperCmd::Goal: loc, speed, torque, mode
 template<>
-inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
+inline interfaces::action::GripperCmd::Goal from_yaml<interfaces::action::GripperCmd::Goal>(const YAML::Node & n)
 {
-  interfaces::action::MoveToPosition::Goal g;
+  interfaces::action::GripperCmd::Goal g;
   if (n && n.IsMap()) {
-    if (n["target_x"]) g.target_x = n["target_x"].as<float>();
-    if (n["target_y"]) g.target_y = n["target_y"].as<float>();
+    if (n["loc"]) g.loc = n["loc"].as<uint8_t>();
+    if (n["speed"]) g.speed = n["speed"].as<uint8_t>();
+    if (n["torque"]) g.torque = n["torque"].as<uint8_t>();
+    if (n["mode"]) g.mode = n["mode"].as<uint8_t>();
+  }
+  return g;
+}
+
+// LegControl::Goal: target
+template<>
+inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(const YAML::Node & n)
+{
+  interfaces::action::LegControl::Goal g;
+  if (n && n.IsMap()) {
+    if (auto node_0_target = n["target"]) {
+      if (auto node_1_header = node_0_target["header"]) {
+        if (auto node_2_stamp = node_1_header["stamp"]) {
+          if (node_2_stamp["sec"]) g.target.header.stamp.sec = node_2_stamp["sec"].as<int32_t>();
+          if (node_2_stamp["nanosec"]) g.target.header.stamp.nanosec = node_2_stamp["nanosec"].as<uint32_t>();
+        }
+        if (node_1_header["frame_id"]) g.target.header.frame_id = node_1_header["frame_id"].as<std::string>();
+      }
+      if (auto node_1_twist = node_0_target["twist"]) {
+        if (auto node_2_linear = node_1_twist["linear"]) {
+          if (node_2_linear["x"]) g.target.twist.linear.x = node_2_linear["x"].as<double>();
+          if (node_2_linear["y"]) g.target.twist.linear.y = node_2_linear["y"].as<double>();
+          if (node_2_linear["z"]) g.target.twist.linear.z = node_2_linear["z"].as<double>();
+        }
+        if (auto node_2_angular = node_1_twist["angular"]) {
+          if (node_2_angular["x"]) g.target.twist.angular.x = node_2_angular["x"].as<double>();
+          if (node_2_angular["y"]) g.target.twist.angular.y = node_2_angular["y"].as<double>();
+          if (node_2_angular["z"]) g.target.twist.angular.z = node_2_angular["z"].as<double>();
+        }
+      }
+    }
+  }
+  return g;
+}
+
+// HandControl::Goal: mode, effort
+template<>
+inline interfaces::action::HandControl::Goal from_yaml<interfaces::action::HandControl::Goal>(const YAML::Node & n)
+{
+  interfaces::action::HandControl::Goal g;
+  if (n && n.IsMap()) {
+    if (n["mode"]) g.mode = n["mode"].as<int32_t>();
+    if (n["effort"]) g.effort = n["effort"].as<float>();
   }
   return g;
 }
@@ -176,53 +259,73 @@ inline interfaces::action::SlamMode::Goal from_yaml<interfaces::action::SlamMode
   return g;
 }
 
+// MoveToPosition::Goal: target_x, target_y, target_z, target_angle_pitch, target_angle_roll, target_angle_yaw
+template<>
+inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
+{
+  interfaces::action::MoveToPosition::Goal g;
+  if (n && n.IsMap()) {
+    if (n["target_x"]) g.target_x = n["target_x"].as<float>();
+    if (n["target_y"]) g.target_y = n["target_y"].as<float>();
+    if (n["target_z"]) g.target_z = n["target_z"].as<float>();
+    if (n["target_angle_pitch"]) g.target_angle_pitch = n["target_angle_pitch"].as<float>();
+    if (n["target_angle_roll"]) g.target_angle_roll = n["target_angle_roll"].as<float>();
+    if (n["target_angle_yaw"]) g.target_angle_yaw = n["target_angle_yaw"].as<float>();
+  }
+  return g;
+}
+
 // ArmSpaceControl::Goal: target
 template<>
 inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::ArmSpaceControl::Goal>(const YAML::Node & n)
 {
   interfaces::action::ArmSpaceControl::Goal g;
   if (n && n.IsMap()) {
-    // geometry_msgs::msg::PoseStamped
-    if (n) {
-      if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
-      if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
-      if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
-      if (auto p = n["position"]) {
-        if (p["x"]) g.target.pose.position.x = p["x"].as<double>();
-        if (p["y"]) g.target.pose.position.y = p["y"].as<double>();
-        if (p["z"]) g.target.pose.position.z = p["z"].as<double>();
+    if (auto node_0_target = n["target"]) {
+      if (auto node_1_header = node_0_target["header"]) {
+        if (auto node_2_stamp = node_1_header["stamp"]) {
+          if (node_2_stamp["sec"]) g.target.header.stamp.sec = node_2_stamp["sec"].as<int32_t>();
+          if (node_2_stamp["nanosec"]) g.target.header.stamp.nanosec = node_2_stamp["nanosec"].as<uint32_t>();
+        }
+        if (node_1_header["frame_id"]) g.target.header.frame_id = node_1_header["frame_id"].as<std::string>();
       }
-      if (auto q = n["orientation"]) {
-        if (q["x"]) g.target.pose.orientation.x = q["x"].as<double>();
-        if (q["y"]) g.target.pose.orientation.y = q["y"].as<double>();
-        if (q["z"]) g.target.pose.orientation.z = q["z"].as<double>();
-        if (q["w"]) g.target.pose.orientation.w = q["w"].as<double>();
+      if (auto node_1_pose = node_0_target["pose"]) {
+        if (auto node_2_position = node_1_pose["position"]) {
+          if (node_2_position["x"]) g.target.pose.position.x = node_2_position["x"].as<double>();
+          if (node_2_position["y"]) g.target.pose.position.y = node_2_position["y"].as<double>();
+          if (node_2_position["z"]) g.target.pose.position.z = node_2_position["z"].as<double>();
+        }
+        if (auto node_2_orientation = node_1_pose["orientation"]) {
+          if (node_2_orientation["x"]) g.target.pose.orientation.x = node_2_orientation["x"].as<double>();
+          if (node_2_orientation["y"]) g.target.pose.orientation.y = node_2_orientation["y"].as<double>();
+          if (node_2_orientation["z"]) g.target.pose.orientation.z = node_2_orientation["z"].as<double>();
+          if (node_2_orientation["w"]) g.target.pose.orientation.w = node_2_orientation["w"].as<double>();
+        }
       }
     }
   }
   return g;
 }
 
-// LegControl::Goal: target
+// ExecuteBtAction::Goal: epoch, action_name, calls
 template<>
-inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(const YAML::Node & n)
+inline interfaces::action::ExecuteBtAction::Goal from_yaml<interfaces::action::ExecuteBtAction::Goal>(const YAML::Node & n)
 {
-  interfaces::action::LegControl::Goal g;
+  interfaces::action::ExecuteBtAction::Goal g;
   if (n && n.IsMap()) {
-    // geometry_msgs::msg::TwistStamped
-    if (n) {
-      if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
-      if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
-      if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
-      if (auto lin = n["linear"]) {
-        if (lin["x"]) g.target.twist.linear.x = lin["x"].as<double>();
-        if (lin["y"]) g.target.twist.linear.y = lin["y"].as<double>();
-        if (lin["z"]) g.target.twist.linear.z = lin["z"].as<double>();
-      }
-      if (auto ang = n["angular"]) {
-        if (ang["x"]) g.target.twist.angular.x = ang["x"].as<double>();
-        if (ang["y"]) g.target.twist.angular.y = ang["y"].as<double>();
-        if (ang["z"]) g.target.twist.angular.z = ang["z"].as<double>();
+    if (n["epoch"]) g.epoch = n["epoch"].as<uint32_t>();
+    if (n["action_name"]) g.action_name = n["action_name"].as<std::string>();
+    if (n["calls"] && n["calls"].IsSequence()) {
+      for (const auto & item : n["calls"]) {
+        interfaces::msg::SkillCall tmp;
+        if (item["name"]) tmp.name = item["name"].as<std::string>();
+        if (item["interface_type"]) tmp.interface_type = item["interface_type"].as<std::string>();
+        if (item["call_kind"]) tmp.call_kind = item["call_kind"].as<std::string>();
+        if (item["topic"]) tmp.topic = item["topic"].as<std::string>();
+        if (item["payload_yaml"]) tmp.payload_yaml = item["payload_yaml"].as<std::string>();
+        if (item["instance_name"]) tmp.instance_name = item["instance_name"].as<std::string>();
+        if (item["timeout_ms"]) tmp.timeout_ms = item["timeout_ms"].as<int32_t>();
+        g.calls.push_back(tmp);
       }
     }
   }
@@ -240,4 +343,167 @@ inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action:
   return g;
 }
 
+// VADEvent::Request: command, timeout_ms
+template<>
+inline interfaces::srv::VADEvent::Request from_yaml<interfaces::srv::VADEvent::Request>(const YAML::Node & n)
+{
+  interfaces::srv::VADEvent::Request r;
+  if (n && n.IsMap()) {
+    if (n["command"]) r.command = n["command"].as<std::string>();
+    if (n["timeout_ms"]) r.timeout_ms = n["timeout_ms"].as<int32_t>();
+  }
+  return r;
+}
+
+// MotorInfo::Request: target, type, motor_ids
+template<>
+inline interfaces::srv::MotorInfo::Request from_yaml<interfaces::srv::MotorInfo::Request>(const YAML::Node & n)
+{
+  interfaces::srv::MotorInfo::Request r;
+  if (n && n.IsMap()) {
+    if (n["target"]) r.target = n["target"].as<std::string>();
+    if (n["type"]) r.type = n["type"].as<std::string>();
+    if (n["motor_ids"]) {
+      auto vec = n["motor_ids"].as<std::vector<uint8_t>>();
+      r.motor_ids.assign(vec.begin(), vec.end());
+    }
+  }
+  return r;
+}
+
+// BtRebuild::Request: type, config, param
+template<>
+inline interfaces::srv::BtRebuild::Request from_yaml<interfaces::srv::BtRebuild::Request>(const YAML::Node & n)
+{
+  interfaces::srv::BtRebuild::Request r;
+  if (n && n.IsMap()) {
+    if (n["type"]) r.type = n["type"].as<std::string>();
+    if (n["config"]) r.config = n["config"].as<std::string>();
+    if (n["param"]) r.param = n["param"].as<std::string>();
+  }
+  return r;
+}
+
+// MapLoad::Request: map_url
+template<>
+inline interfaces::srv::MapLoad::Request from_yaml<interfaces::srv::MapLoad::Request>(const YAML::Node & n)
+{
+  interfaces::srv::MapLoad::Request r;
+  if (n && n.IsMap()) {
+    if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
+  }
+  return r;
+}
+
+// AudioData::Request: command, duration_ms
+template<>
+inline interfaces::srv::AudioData::Request from_yaml<interfaces::srv::AudioData::Request>(const YAML::Node & n)
+{
+  interfaces::srv::AudioData::Request r;
+  if (n && n.IsMap()) {
+    if (n["command"]) r.command = n["command"].as<std::string>();
+    if (n["duration_ms"]) r.duration_ms = n["duration_ms"].as<int32_t>();
+  }
+  return r;
+}
+
+// ClearArmError::Request: arm_id, joint_num
+template<>
+inline interfaces::srv::ClearArmError::Request from_yaml<interfaces::srv::ClearArmError::Request>(const YAML::Node & n)
+{
+  interfaces::srv::ClearArmError::Request r;
+  if (n && n.IsMap()) {
+    if (n["arm_id"]) r.arm_id = n["arm_id"].as<int8_t>();
+    if (n["joint_num"]) r.joint_num = n["joint_num"].as<int8_t>();
+  }
+  return r;
+}
+
+// ASRRecognize::Request: command
+template<>
+inline interfaces::srv::ASRRecognize::Request from_yaml<interfaces::srv::ASRRecognize::Request>(const YAML::Node & n)
+{
+  interfaces::srv::ASRRecognize::Request r;
+  if (n && n.IsMap()) {
+    if (n["command"]) r.command = n["command"].as<std::string>();
+  }
+  return r;
+}
+
+// MotorParam::Request: motor_id, max_speed, add_acc, dec_acc
+template<>
+inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorParam::Request>(const YAML::Node & n)
+{
+  interfaces::srv::MotorParam::Request r;
+  if (n && n.IsMap()) {
+    if (n["motor_id"]) r.motor_id = n["motor_id"].as<uint16_t>();
+    if (n["max_speed"]) r.max_speed = n["max_speed"].as<uint16_t>();
+    if (n["add_acc"]) r.add_acc = n["add_acc"].as<uint16_t>();
+    if (n["dec_acc"]) r.dec_acc = n["dec_acc"].as<uint16_t>();
+  }
+  return r;
+}
+
+// GripperCmd::Request: devid, loc, speed, torque
+template<>
+inline interfaces::srv::GripperCmd::Request from_yaml<interfaces::srv::GripperCmd::Request>(const YAML::Node & n)
+{
+  interfaces::srv::GripperCmd::Request r;
+  if (n && n.IsMap()) {
+    if (n["devid"]) r.devid = n["devid"].as<uint8_t>();
+    if (n["loc"]) r.loc = n["loc"].as<uint8_t>();
+    if (n["speed"]) r.speed = n["speed"].as<uint8_t>();
+    if (n["torque"]) r.torque = n["torque"].as<uint8_t>();
+  }
+  return r;
+}
+
+// MapSave::Request: map_url
+template<>
+inline interfaces::srv::MapSave::Request from_yaml<interfaces::srv::MapSave::Request>(const YAML::Node & n)
+{
+  interfaces::srv::MapSave::Request r;
+  if (n && n.IsMap()) {
+    if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
+  }
+  return r;
+}
+
+// TTSSynthesize::Request: command, text, voice
+template<>
+inline interfaces::srv::TTSSynthesize::Request from_yaml<interfaces::srv::TTSSynthesize::Request>(const YAML::Node & n)
+{
+  interfaces::srv::TTSSynthesize::Request r;
+  if (n && n.IsMap()) {
+    if (n["command"]) r.command = n["command"].as<std::string>();
+    if (n["text"]) r.text = n["text"].as<std::string>();
+    if (n["voice"]) r.voice = n["voice"].as<std::string>();
+  }
+  return r;
+}
+
+// InverseKinematics::Request: arm_id, pose
+template<>
+inline interfaces::srv::InverseKinematics::Request from_yaml<interfaces::srv::InverseKinematics::Request>(const YAML::Node & n)
+{
+  interfaces::srv::InverseKinematics::Request r;
+  if (n && n.IsMap()) {
+    if (n["arm_id"]) r.arm_id = n["arm_id"].as<int32_t>();
+    if (auto node_0_pose = n["pose"]) {
+      if (auto node_1_position = node_0_pose["position"]) {
+        if (node_1_position["x"]) r.pose.position.x = node_1_position["x"].as<double>();
+        if (node_1_position["y"]) r.pose.position.y = node_1_position["y"].as<double>();
+        if (node_1_position["z"]) r.pose.position.z = node_1_position["z"].as<double>();
+      }
+      if (auto node_1_orientation = node_0_pose["orientation"]) {
+        if (node_1_orientation["x"]) r.pose.orientation.x = node_1_orientation["x"].as<double>();
+        if (node_1_orientation["y"]) r.pose.orientation.y = node_1_orientation["y"].as<double>();
+        if (node_1_orientation["z"]) r.pose.orientation.z = node_1_orientation["z"].as<double>();
+        if (node_1_orientation["w"]) r.pose.orientation.w = node_1_orientation["w"].as<double>();
+      }
+    }
+  }
+  return r;
+}
+
 } // namespace brain

src/brain/include/brain/skill_config.hpp

@@ -5,14 +5,33 @@
 #include <string>
 
 #include "interfaces/action/arm.hpp"
+#include "interfaces/action/arm_space_control.hpp"
 #include "interfaces/action/camera_take_photo.hpp"
+#include "interfaces/action/dual_arm.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/gripper_cmd.hpp"
 #include "interfaces/action/hand_control.hpp"
+#include "interfaces/action/leg_control.hpp"
 #include "interfaces/action/move_home.hpp"
 #include "interfaces/action/move_leg.hpp"
+#include "interfaces/action/move_to_position.hpp"
 #include "interfaces/action/move_waist.hpp"
 #include "interfaces/action/move_wheel.hpp"
+#include "interfaces/action/slam_mode.hpp"
+#include "interfaces/action/vision_grasp_object.hpp"
+#include "interfaces/action/vision_object_recognition.hpp"
+#include "interfaces/srv/asr_recognize.hpp"
+#include "interfaces/srv/audio_data.hpp"
+#include "interfaces/srv/bt_rebuild.hpp"
+#include "interfaces/srv/clear_arm_error.hpp"
+#include "interfaces/srv/gripper_cmd.hpp"
+#include "interfaces/srv/inverse_kinematics.hpp"
 #include "interfaces/srv/map_load.hpp"
 #include "interfaces/srv/map_save.hpp"
+#include "interfaces/srv/motor_info.hpp"
+#include "interfaces/srv/motor_param.hpp"
+#include "interfaces/srv/tts_synthesize.hpp"
+#include "interfaces/srv/vad_event.hpp"
 #include "interfaces/srv/vision_object_recognition.hpp"
 
 namespace brain {
@@ -24,54 +43,82 @@ template<typename ServiceT>
 struct SkillServiceTrait;
 
 using SkillActionTypes = std::tuple<
+  interfaces::action::VisionObjectRecognition,
   interfaces::action::Arm,
+  interfaces::action::DualArm,
   interfaces::action::CameraTakePhoto,
-  interfaces::action::HandControl,
   interfaces::action::MoveWaist,
   interfaces::action::MoveLeg,
   interfaces::action::MoveWheel,
-  interfaces::action::MoveHome
+  interfaces::action::MoveHome,
+  interfaces::action::GripperCmd,
+  interfaces::action::ArmSpaceControl,
+  interfaces::action::ExecuteBtAction,
+  interfaces::action::HandControl,
+  interfaces::action::LegControl,
+  interfaces::action::MoveToPosition,
+  interfaces::action::SlamMode,
+  interfaces::action::VisionGraspObject
 >;
 
 using SkillServiceTypes = std::tuple<
   interfaces::srv::VisionObjectRecognition,
+  interfaces::srv::ASRRecognize,
+  interfaces::srv::AudioData,
+  interfaces::srv::BtRebuild,
+  interfaces::srv::ClearArmError,
+  interfaces::srv::GripperCmd,
+  interfaces::srv::InverseKinematics,
   interfaces::srv::MapLoad,
-  interfaces::srv::MapSave
+  interfaces::srv::MapSave,
+  interfaces::srv::MotorInfo,
+  interfaces::srv::MotorParam,
+  interfaces::srv::TTSSynthesize,
+  interfaces::srv::VADEvent
 >;
 
+template<>
+struct SkillActionTrait<interfaces::action::VisionObjectRecognition>
+{
+  static constexpr const char * skill_name = "VisionObjectRecognition";
+  static constexpr const char * default_topic = "/vision_object_recognition";
+  static bool success(const interfaces::action::VisionObjectRecognition::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
+  static std::string message(const interfaces::action::VisionObjectRecognition::Result & r) {return r.info;}
+};
+
 template<>
 struct SkillActionTrait<interfaces::action::Arm>
 {
   static constexpr const char * skill_name = "Arm";
-  static constexpr const char * default_topic = "ArmAction";
-  static bool success(const interfaces::action::Arm::Result & r) {return (r.result == 0);}
+  static constexpr const char * default_topic = "Arm";
+  static bool success(const interfaces::action::Arm::Result & r, const std::string & instance_name) {(void)instance_name; return (r.result == 0);}
   static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
 };
 
+template<>
+struct SkillActionTrait<interfaces::action::DualArm>
+{
+  static constexpr const char * skill_name = "DualArm";
+  static constexpr const char * default_topic = "DualArm";
+  static bool success(const interfaces::action::DualArm::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
+  static std::string message(const interfaces::action::DualArm::Result & r) {return r.message;}
+};
+
 template<>
 struct SkillActionTrait<interfaces::action::CameraTakePhoto>
 {
   static constexpr const char * skill_name = "CameraTakePhoto";
   static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::CameraTakePhoto::Result & r) {return r.success;}
+  static bool success(const interfaces::action::CameraTakePhoto::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
   static std::string message(const interfaces::action::CameraTakePhoto::Result & r) {return r.message;}
 };
 
-template<>
-struct SkillActionTrait<interfaces::action::HandControl>
-{
-  static constexpr const char * skill_name = "HandControl";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::HandControl::Result & r) {return r.success;}
-  static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
-};
-
 template<>
 struct SkillActionTrait<interfaces::action::MoveWaist>
 {
   static constexpr const char * skill_name = "MoveWaist";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::MoveWaist::Result & r) {return r.success;}
+  static constexpr const char * default_topic = "MoveWaist";
+  static bool success(const interfaces::action::MoveWaist::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
   static std::string message(const interfaces::action::MoveWaist::Result & r) {return r.message;}
 };
 
@@ -79,8 +126,8 @@ template<>
 struct SkillActionTrait<interfaces::action::MoveLeg>
 {
   static constexpr const char * skill_name = "MoveLeg";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::MoveLeg::Result & r) {return r.success;}
+  static constexpr const char * default_topic = "MoveLeg";
+  static bool success(const interfaces::action::MoveLeg::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
   static std::string message(const interfaces::action::MoveLeg::Result & r) {return r.message;}
 };
 
@@ -88,8 +135,8 @@ template<>
 struct SkillActionTrait<interfaces::action::MoveWheel>
 {
   static constexpr const char * skill_name = "MoveWheel";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::MoveWheel::Result & r) {return r.success;}
+  static constexpr const char * default_topic = "MoveWheel";
+  static bool success(const interfaces::action::MoveWheel::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
   static std::string message(const interfaces::action::MoveWheel::Result & r) {return r.message;}
 };
 
@@ -97,15 +144,144 @@ template<>
 struct SkillActionTrait<interfaces::action::MoveHome>
 {
   static constexpr const char * skill_name = "MoveHome";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::MoveHome::Result & r) {return r.success;}
+  static constexpr const char * default_topic = "MoveHome";
+  static bool success(const interfaces::action::MoveHome::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
   static std::string message(const interfaces::action::MoveHome::Result & r) {return r.message;}
 };
 
+template<>
+struct SkillActionTrait<interfaces::action::GripperCmd>
+{
+  static constexpr const char * skill_name = "GripperCmd";
+  static constexpr const char * default_topic = "gripper_cmd0";
+  static bool success(const interfaces::action::GripperCmd::Result & r, const std::string & instance_name) {
+    (void)instance_name; 
+
+    if (r.result == 0) return false;
+
+    //如果instance_name中带有"open"，则表示是打开手指的命令
+    if (instance_name.find("open") != std::string::npos) {
+      //如果r.state中包含"手指已到达指定的位置"，则认为成功
+      return r.state.find("手指已到达指定的位置") != std::string::npos;
+    } else if (instance_name.find("close") != std::string::npos) {
+      //如果r.state中包含"手指在闭合检测到物体"，则认为成功
+      return (r.state.find("手指在闭合检测到物体") != std::string::npos);
+    } else {
+      return false;
+    }
+  }
+  static std::string message(const interfaces::action::GripperCmd::Result & r) {return r.state;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::ArmSpaceControl>
+{
+  static constexpr const char * skill_name = "ArmSpaceControl";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::ArmSpaceControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
+  static std::string message(const interfaces::action::ArmSpaceControl::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::ExecuteBtAction>
+{
+  static constexpr const char * skill_name = "ExecuteBtAction";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::ExecuteBtAction::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
+  static std::string message(const interfaces::action::ExecuteBtAction::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::HandControl>
+{
+  static constexpr const char * skill_name = "HandControl";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::HandControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
+  static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::LegControl>
+{
+  static constexpr const char * skill_name = "LegControl";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::LegControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
+  static std::string message(const interfaces::action::LegControl::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::MoveToPosition>
+{
+  static constexpr const char * skill_name = "MoveToPosition";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::MoveToPosition::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
+  static std::string message(const interfaces::action::MoveToPosition::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::SlamMode>
+{
+  static constexpr const char * skill_name = "SlamMode";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::SlamMode::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
+  static std::string message(const interfaces::action::SlamMode::Result & r) {return r.message;}
+};
+
+template<>
+struct SkillActionTrait<interfaces::action::VisionGraspObject>
+{
+  static constexpr const char * skill_name = "VisionGraspObject";
+  static constexpr const char * default_topic = "";
+  static bool success(const interfaces::action::VisionGraspObject::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
+  static std::string message(const interfaces::action::VisionGraspObject::Result & r) {return r.message;}
+};
+
 template<>
 struct SkillServiceTrait<interfaces::srv::VisionObjectRecognition>
 {
   static constexpr const char * skill_name = "VisionObjectRecognition";
+  static constexpr const char * default_topic = "/vision_object_recognition";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::ASRRecognize>
+{
+  static constexpr const char * skill_name = "ASRRecognize";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::AudioData>
+{
+  static constexpr const char * skill_name = "AudioData";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::BtRebuild>
+{
+  static constexpr const char * skill_name = "BtRebuild";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::ClearArmError>
+{
+  static constexpr const char * skill_name = "ClearArmError";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::GripperCmd>
+{
+  static constexpr const char * skill_name = "GripperCmd";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::InverseKinematics>
+{
+  static constexpr const char * skill_name = "InverseKinematics";
   static constexpr const char * default_topic = "";
 };
 
@@ -123,4 +299,32 @@ struct SkillServiceTrait<interfaces::srv::MapSave>
   static constexpr const char * default_topic = "";
 };
 
+template<>
+struct SkillServiceTrait<interfaces::srv::MotorInfo>
+{
+  static constexpr const char * skill_name = "MotorInfo";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::MotorParam>
+{
+  static constexpr const char * skill_name = "MotorParam";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::TTSSynthesize>
+{
+  static constexpr const char * skill_name = "TTSSynthesize";
+  static constexpr const char * default_topic = "";
+};
+
+template<>
+struct SkillServiceTrait<interfaces::srv::VADEvent>
+{
+  static constexpr const char * skill_name = "VADEvent";
+  static constexpr const char * default_topic = "";
+};
+
 } // namespace brain

src/brain/include/brain/skill_manager.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include <hivecore_logger/logger.hpp>
 #include <yaml-cpp/yaml.h>
 #include <chrono>
 #include <functional>
@@ -287,10 +288,17 @@ private:
     const std::string & topic,
     const std::string & internal_skill)
   {
-    auto it = action_override_registrars_.find(base);
+    // PRIORITY 1: Match exact skill name (e.g. "GripperCmd0") for highly specific overrides
+    auto it = action_override_registrars_.find(internal_skill);
+    // PRIORITY 2: Match interface base name (e.g. "GripperCmd") for generic overrides
+    if (it == action_override_registrars_.end()) {
+      it = action_override_registrars_.find(base);
+    }
+
     if (it != action_override_registrars_.end()) {
       it->second(topic, internal_skill);
-      RCLCPP_INFO(node_->get_logger(), "Overridden action client registration: %s", base.c_str());
+      LOG_INFO("Registered action client '{}' via Hook on '{}'", 
+        internal_skill.c_str(), it->first.c_str());
       return;
     }
     using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
@@ -324,10 +332,10 @@ private:
     if (!result_cb) {
       result_cb = [this, internal_skill](const typename GoalHandle::WrappedResult & res) {
           if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
-            SkillActionTrait<ActionT>::success(*res.result)) {
-            RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
+            SkillActionTrait<ActionT>::success(*res.result, "")) {
+            LOG_INFO("action succeeded: {}", internal_skill.c_str());
           } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
-            RCLCPP_WARN(node_->get_logger(), "action cancelled: %s", internal_skill.c_str());
+            LOG_WARN("action cancelled: {}", internal_skill.c_str());
           } else {
             std::string detail;
             if (res.result) {
@@ -335,8 +343,7 @@ private:
             } else {
               detail = "result unavailable";
             }
-            RCLCPP_ERROR(
-              node_->get_logger(), "action failed: %s (%s)", internal_skill.c_str(), detail.c_str());
+            LOG_ERROR("action failed: {} ({})", internal_skill.c_str(), detail.c_str());
           }
         };
     }
@@ -365,16 +372,16 @@ private:
 template<size_t I = 0, typename TupleT, typename RegistryT>
 bool dispatch_skill_action(
   const std::string & skill, const std::string & topic, RegistryT * reg,
-  rclcpp::Logger logger, std::chrono::nanoseconds timeout, std::string & detail)
+  std::chrono::nanoseconds timeout, const std::string & instance_name, std::string & detail)
 {
   if constexpr (I == std::tuple_size<TupleT>::value) {
     return false;
   } else {
     using ActionT = std::tuple_element_t<I, TupleT>;
     if (skill == SkillActionTrait<ActionT>::skill_name) {
-      auto opt = reg->template send_and_wait<ActionT>(topic, logger, timeout);
+      auto opt = reg->template send_and_wait<ActionT>(topic, timeout);
       if (opt && opt->code == rclcpp_action::ResultCode::SUCCEEDED && opt->result) {
-        bool ok = SkillActionTrait<ActionT>::success(*opt->result);
+        bool ok = SkillActionTrait<ActionT>::success(*opt->result, instance_name);
         detail = ok ? SkillActionTrait<ActionT>::message(*opt->result) : std::string(
           "result flag false");
         return ok;
@@ -382,7 +389,7 @@ bool dispatch_skill_action(
       detail = "action failed";
       return false;
     }
-    return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, detail);
+    return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, timeout, instance_name, detail);
   }
 }
 

src/brain/include/brain/sm_cerebellum.hpp

@@ -3,6 +3,7 @@
 #include <smacc2/smacc.hpp>
 #include <boost/mpl/list.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <hivecore_logger/logger.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <atomic>
 #include <string>
@@ -49,7 +50,7 @@ struct CerebellumData
    * @brief Bound by the action execute callback: performs the whole skill
    * sequence synchronously when invoked inside StExecuting supervision thread.
    */
-  std::function<ExecResult(rclcpp::Logger)> execute_fn;          // installed by action execute
+  std::function<ExecResult()> execute_fn;          // installed by action execute
   /**
    * @brief Completion callback invoked by terminal states (Completed/Failed/Emergency)
    * to produce the actual action result (succeed/abort) back to the client.
@@ -117,7 +118,7 @@ struct StIdle : smacc2::SmaccState<StIdle, SmCerebellum>
    */
   sc::result react(const EvGoalReceived &)
   {
-    RCLCPP_INFO(this->getLogger(), "[SM] Transition Idle -> Executing (goal received)");
+    LOG_INFO("[SM] Transition Idle -> Executing (goal received)");
     return transit<StExecuting>();
   }
 };
@@ -140,7 +141,7 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
   /** @brief State entry: spawns supervision thread and invokes execute_fn when ready. */
   void onEntry()
   {
-    RCLCPP_INFO(this->getLogger(), "[SM] Enter Executing");
+    LOG_INFO("[SM] Enter Executing");
     // Supervising thread waits until execute_fn is set, then runs it.
     std::thread{[this]() {
         auto & d = runtime();
@@ -149,13 +150,13 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
         while (!d.execute_fn) {
           std::this_thread::sleep_for(5ms);
           auto now = std::chrono::steady_clock::now();
-            if (now - last_warn > 5s) {
-              RCLCPP_WARN(this->getLogger(), "[SM] Still waiting for execute_fn to be set...");
-              last_warn = now;
-            }
+          if (now - last_warn > 10s) {
+            LOG_WARN("[SM] Still waiting for execute_fn to be set...");
+            last_warn = now;
+          }
         }
         try {
-          auto res = d.execute_fn(this->getLogger());
+          auto res = d.execute_fn();
           d.preempt.store(!res.success, std::memory_order_release);
           d.preempt_reason = res.message;
           d.last_success.store(res.success, std::memory_order_release);
@@ -180,19 +181,19 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
    * @return Transition to Completed state.
    */
   sc::result react(const EvExecutionFinished &)
-  { RCLCPP_INFO(this->getLogger(), "[SM] Transition Executing -> Completed"); return transit<StCompleted>(); }
+  { LOG_INFO("[SM] Transition Executing -> Completed"); return transit<StCompleted>(); }
   /**
    * @brief Reaction: functional failure -> StFailed.
    * @return Transition to Failed state.
    */
   sc::result react(const EvExecutionFailed &)
-  { RCLCPP_WARN(this->getLogger(), "[SM] Transition Executing -> Failed"); return transit<StFailed>(); }
+  { LOG_WARN("[SM] Transition Executing -> Failed"); return transit<StFailed>(); }
   /**
    * @brief Reaction: exception / emergency -> StEmergency.
    * @return Transition to Emergency state.
    */
   sc::result react(const EvEmergency &)
-  { RCLCPP_ERROR(this->getLogger(), "[SM] Transition Executing -> Emergency"); return transit<StEmergency>(); }
+  { LOG_ERROR("[SM] Transition Executing -> Emergency"); return transit<StEmergency>(); }
 };
 
 // ----------------------------------------------------------------------------
@@ -223,7 +224,7 @@ struct StCompleted : smacc2::SmaccState<StCompleted, SmCerebellum>
    * @return Transition to StExecuting.
    */
   sc::result react(const EvGoalReceived &)
-  { RCLCPP_INFO(this->getLogger(), "[SM] Transition Completed -> Executing (new goal)"); return transit<StExecuting>(); }
+  { LOG_INFO("[SM] Transition Completed -> Executing (new goal)"); return transit<StExecuting>(); }
 };
 
 // ----------------------------------------------------------------------------
@@ -249,7 +250,7 @@ struct StFailed : smacc2::SmaccState<StFailed, SmCerebellum>
    * @return Transition to StExecuting.
    */
   sc::result react(const EvGoalReceived &)
-  { RCLCPP_INFO(this->getLogger(), "[SM] Transition Failed -> Executing (new goal)"); return transit<StExecuting>(); }
+  { LOG_INFO("[SM] Transition Failed -> Executing (new goal)"); return transit<StExecuting>(); }
 };
 
 // ----------------------------------------------------------------------------
@@ -268,7 +269,7 @@ struct StEmergency : smacc2::SmaccState<StEmergency, SmCerebellum>
   {
     auto & d = runtime();
     if (d.complete_cb) d.complete_cb(false, std::string("EMERGENCY: ")+ d.preempt_reason);
-    RCLCPP_ERROR(this->getLogger(), "[SM] Emergency: %s", d.preempt_reason.c_str());
+    LOG_ERROR("[SM] Emergency: {}", d.preempt_reason.c_str());
     d.execute_fn = nullptr; d.complete_cb = nullptr; d.preempt.store(false);
   }
   /**
@@ -276,7 +277,7 @@ struct StEmergency : smacc2::SmaccState<StEmergency, SmCerebellum>
    * @return Transition to StExecuting.
    */
   sc::result react(const EvGoalReceived &)
-  { RCLCPP_INFO(this->getLogger(), "[SM] Transition Emergency -> Executing (new goal)"); return transit<StExecuting>(); }
+  { LOG_INFO("[SM] Transition Emergency -> Executing (new goal)"); return transit<StExecuting>(); }
 };
 
 } // namespace brain

src/brain/launch/brain.launch.py

@@ -0,0 +1,42 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare launch arguments
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=PathJoinSubstitution([
+            FindPackageShare('brain'),
+            'config',
+            'brain_node_params.yaml'
+        ]),
+        description='Full path to params file to load'
+    )
+
+    # Get the params file path
+    params_file = LaunchConfiguration('params_file')
+
+    # Create the brain node that runs both cerebrum and cerebellum
+    brain_node = Node(
+        package='brain',
+        executable='brain_node',
+        # name='brain_node',
+        parameters=[params_file],
+        output='screen',
+        emulate_tty=True
+    )
+
+    # Create the launch description
+    ld = LaunchDescription()
+
+    # Declare launch arguments
+    ld.add_action(declare_params_file_cmd)
+
+    # Add nodes
+    ld.add_action(brain_node)
+
+    return ld

src/brain/launch/brain_with_params.launch.py

@@ -0,0 +1,56 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+import os
+
+
+def launch_setup(context, *args, **kwargs):
+    # Get the params file path
+    params_file = LaunchConfiguration('params_file').perform(context)
+    
+    # Create the brain node that runs both cerebrum and cerebellum
+    brain_node = Node(
+        package='brain',
+        executable='brain_node',
+        name='brain_node',
+        parameters=[params_file],
+        output='screen',
+        emulate_tty=True,
+        # Override specific parameters if needed
+        arguments=[
+            # Example of overriding individual parameters via command line
+            # Uncomment and modify as needed
+            # "--ros-args", 
+            # "-p", "tick_rate_hz:=20.0",
+            # "-p", "default_action_timeout_sec:=60.0"
+        ]
+    )
+
+    return [brain_node]
+
+
+def generate_launch_description():
+    # Declare launch arguments
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=PathJoinSubstitution([
+            FindPackageShare('brain'),
+            'config',
+            'brain_node_params.yaml'
+        ]),
+        description='Full path to params file to load'
+    )
+
+    # Create the launch description
+    ld = LaunchDescription()
+
+    # Declare launch arguments
+    ld.add_action(declare_params_file_cmd)
+
+    # Add the opaque function to set up the nodes
+    ld.add_action(OpaqueFunction(function=launch_setup))
+
+    return ld

src/brain/package.xml

@@ -13,7 +13,6 @@
   <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/brain_node.cpp

@@ -1,5 +1,5 @@
 #include <rclcpp/rclcpp.hpp>
-#include <rclcpp/logger.hpp>
+#include <hivecore_logger/logger.hpp>
 
 #include "../include/brain/cerebellum_node.hpp"
 #include "../include/brain/cerebrum_node.hpp"
@@ -22,11 +22,26 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
 
-  rclcpp::Logger logger = rclcpp::get_logger("brain_node");
-  RCLCPP_INFO(logger, "Starting brain node...");
+  hivecore::log::LoggerOptions log_opts;
+  log_opts.default_level        = hivecore::log::Level::INFO;   // 默认日志等级
+  log_opts.log_dir              = "/var/log/robot";              // 主日志目录
+  log_opts.fallback_log_dir     = "/tmp/robot_logs";             // 备用目录（主目录不可写时使用）
+  log_opts.max_file_size_mb     = 50;                            // 单个日志文件最大 50 MB
+  log_opts.max_files            = 10;                            // 最多保留 10 个轮转文件
+  log_opts.queue_size           = 8192;                          // 异步队列容量
+  log_opts.worker_threads       = 1;                             // 后台写日志线程数
+  log_opts.enable_console       = true;                          // 同时输出到控制台
+  log_opts.enable_level_sync    = true;                          // 允许运行时动态调整日志等级
+  log_opts.level_sync_interval_ms = 100;                        // 动态等级同步检查间隔 (ms)
+  hivecore::log::Logger::init("brain_node", log_opts);
 
-  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  LOG_INFO("Starting brain node...");
+
+  // Create node options with parameter overrides if provided
   rclcpp::NodeOptions options;
+  // If command line arguments are provided, they will be handled by rclcpp::init
+  
+  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
 
   auto cerebellum_node = std::make_shared<brain::CerebellumNode>(options);
   auto cerebrum_node = std::make_shared<brain::CerebrumNode>(options);
@@ -36,6 +51,7 @@ int main(int argc, char ** argv)
 
   executor->spin();
 
+  hivecore::log::Logger::shutdown();
   rclcpp::shutdown();
   return 0;
 }

src/brain/src/calc_grasp_pose.cpp

@@ -0,0 +1,202 @@
+#include "brain/calc_grasp_pose.hpp"
+#include <cmath>
+#include <iostream>
+#include <algorithm>
+#include <vector>
+#include <utility>
+#include <string.h>
+#include <rclcpp/rclcpp.hpp>
+#include <hivecore_logger/logger.hpp>
+
+namespace brain {
+
+bool GraspPoseCalculator::rm_arm_algo_init_done_ = false;
+
+GraspPoseCalculator::GraspPoseCalculator()
+{
+}
+
+tf2::Vector3 normalize_vector(const tf2::Vector3& v) {
+    if (v.length() == 0) return v;
+    return v.normalized();
+}
+
+GraspResult GraspPoseCalculator::create_grasp_matrix(
+    const tf2::Vector3& target_pos,
+    const tf2::Vector3& approach_vec,
+    const tf2::Vector3& palm_normal_vec,
+    double safety_dist,
+    double finger_length
+) {
+    tf2::Vector3 z_axis = normalize_vector(approach_vec);
+    tf2::Vector3 x_axis = normalize_vector(palm_normal_vec);
+    tf2::Vector3 y_axis = z_axis.cross(x_axis);
+    x_axis = y_axis.cross(z_axis);
+    tf2::Matrix3x3 R_grasp(
+        x_axis.x(), y_axis.x(), z_axis.x(),
+        x_axis.y(), y_axis.y(), z_axis.y(),
+        x_axis.z(), y_axis.z(), z_axis.z()
+    );
+    
+    tf2::Transform T_grasp;
+    T_grasp.setIdentity();
+    T_grasp.setOrigin(target_pos);
+    T_grasp.setBasis(R_grasp);
+    T_grasp.setOrigin(T_grasp.getOrigin() - z_axis * finger_length);
+    tf2::Transform T_pre = T_grasp;
+    T_pre.setOrigin(T_pre.getOrigin() - z_axis * safety_dist);
+    
+    GraspResult result;
+    result.grasp_pose.position = T_grasp.getOrigin();
+    result.grasp_pose.orientation = T_grasp.getRotation();
+    result.pre_grasp_pose.position = T_pre.getOrigin();
+    result.pre_grasp_pose.orientation = T_pre.getRotation();
+    
+    return result;
+}
+
+GraspResult GraspPoseCalculator::calculate(
+    const tf2::Vector3& target_pos,
+    const tf2::Quaternion& target_quat,
+    const std::string& grasp_type,
+    double angle_deg,
+    const std::string& palm_facing,
+    double safety_dist,
+    double finger_length,
+    const std::string& arm
+) {
+    (void)target_quat;
+
+    bool is_left = (arm.find("left") != std::string::npos);
+
+    tf2::Vector3 V_world_up(-1.0, 0.0, 0.0);
+    tf2::Vector3 V_world_down = -V_world_up;
+    tf2::Vector3 P_shoulder(0, 0, 0);
+    tf2::Vector3 V_sb = target_pos - P_shoulder;
+    tf2::Vector3 V_horiz = V_sb - (V_sb.dot(V_world_up) * V_world_up);
+    if (V_horiz.length() < 1e-3) {
+        V_horiz = tf2::Vector3(0.0, 1.0, 0.0); // Default horizontal vector
+    } else {
+        V_horiz = normalize_vector(V_horiz);
+    }
+    tf2::Vector3 approach_vec;
+    tf2::Vector3 palm_normal;
+    std::string name_suffix;
+    
+    if (grasp_type == "side") {
+        // tf2::Vector3 V_horiz_sb(0, V_sb.y(), V_sb.z());
+        double base_angle = 0.0;
+        // if (V_horiz_sb.length() >= 1e-3) {
+        //     base_angle = std::atan2(V_horiz_sb.z(), V_horiz_sb.y());
+        // }
+        double rad = angle_deg * M_PI / 180.0;
+        double angle = base_angle + rad;
+        double dy = std::cos(angle);
+        double dz = std::sin(angle);
+        if (is_left) {
+            dy = -dy;
+        }
+        approach_vec = tf2::Vector3(0, dy, dz); // Horizontal approach
+
+        if (palm_facing == "up") {
+            palm_normal = V_world_up;
+            name_suffix = "Side_Angle_" + std::to_string((int)angle_deg) + "_Palm_Up";
+        } else {
+            palm_normal = V_world_down;
+            name_suffix = "Side_Angle_" + std::to_string((int)angle_deg) + "_Palm_Down";
+        }
+    } else if (grasp_type == "top") {
+        approach_vec = V_world_down;
+        palm_normal = V_horiz;
+        name_suffix = "Top_Grasp";
+    } else {
+        approach_vec = V_horiz;
+        palm_normal = V_world_down;
+        name_suffix = "Side_Default";
+    }
+    
+    GraspResult result = create_grasp_matrix(target_pos, approach_vec, palm_normal, safety_dist, finger_length);
+    result.name = name_suffix;
+
+    return result;
+}
+
+double GraspPoseCalculator::evaluate_grasp_quality(
+    const tf2::Vector3& target_pos,
+    double angle_deg,
+    const std::string& arm
+) {
+    double score = 0.0;
+    bool is_right = (arm.find("right") != std::string::npos);
+    // In this frame (Z is Right), angle > 0 moves towards Z (Right/Outward)
+    bool is_outward = (is_right && angle_deg > 0) || (!is_right && angle_deg < 0);
+    
+    if (is_outward) {
+        score += 50.0;
+    } else if (std::abs(angle_deg) < 1e-3) {
+        score += 10.0;
+    } else {
+        score -= 50.0;
+    }
+    
+    double abs_angle = std::abs(angle_deg);
+    if (abs_angle < 10.0) {
+        score -= 20.0;
+    } else if (abs_angle >= 20.0 && abs_angle <= 50.0) {
+        score += 30.0;
+    } else if (abs_angle > 60.0) {
+        score -= 20.0;
+    }
+    
+    tf2::Vector3 P_shoulder(0, 0, 0);
+    tf2::Vector3 V_sb = target_pos - P_shoulder;
+    tf2::Vector3 V_horiz_sb(0, V_sb.y(), V_sb.z());
+    
+    double base_angle = std::atan2(V_horiz_sb.z(), V_horiz_sb.y());
+    double rad = angle_deg * M_PI / 180.0;
+    double angle = base_angle + rad;
+    
+    tf2::Vector3 approach_vec(0, std::cos(angle), std::sin(angle));
+    tf2::Vector3 wrist_pos = target_pos - approach_vec * 0.2;
+    double dist_shoulder_wrist = wrist_pos.length();
+    
+    if (dist_shoulder_wrist > 0.8) {
+        score -= 100.0;
+    } else if (dist_shoulder_wrist < 0.2) {
+        score -= 100.0;
+    } else if (dist_shoulder_wrist >= 0.35 && dist_shoulder_wrist <= 0.65) {
+        score += 20.0;
+    }
+    return score;
+}
+
+std::vector<double> GraspPoseCalculator::find_optimal_grasp_angles(
+    const tf2::Vector3& target_pos,
+    const std::string& arm,
+    int top_k
+) {
+    std::vector<std::pair<double, double>> scored_angles;
+    std::vector<double> angles;
+    bool is_right = (arm.find("right") != std::string::npos);
+    if (is_right) {
+        for (double a = 0.0; a >= -60.0; a -= 5.0) angles.push_back(a);
+    } else {
+        for (double a = 0.0; a <= 60.0; a += 5.0) angles.push_back(a);
+    }
+    for (double angle : angles) {
+        double score = evaluate_grasp_quality(target_pos, angle, arm);
+        scored_angles.push_back({score, angle});
+    }
+    std::sort(scored_angles.begin(), scored_angles.end(), 
+        [](const std::pair<double, double>& a, const std::pair<double, double>& b) {
+            return a.first > b.first;
+        }
+    );
+    std::vector<double> result;
+    for (int i = 0; i < std::min((int)scored_angles.size(), top_k); ++i) {
+        result.push_back(scored_angles[i].second);
+    }
+    return result;
+}
+
+} // namespace brain

src/brain/src/skill_manager.cpp

@@ -14,6 +14,7 @@
 #include <cmath>
 #include <utility>
 #include <rclcpp_action/rclcpp_action.hpp>
+#include <hivecore_logger/logger.hpp>
 
 namespace brain
 {
@@ -87,7 +88,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
 {
   YAML::Node root = YAML::LoadFile(yaml_path);
   if (!root || !root.IsSequence()) {
-    RCLCPP_ERROR(node_->get_logger(), "Invalid skills YAML: expected sequence at root");
+    LOG_ERROR("Invalid skills YAML: expected sequence at root");
     return false;
   }
 
@@ -112,7 +113,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
           // Extended form: { name: BaseName.kind, default_topic: <str> }
           const auto name_node = v["name"];
           if (!name_node || !name_node.IsScalar()) {
-            RCLCPP_ERROR(node_->get_logger(), "Invalid interface entry (missing 'name'): %s", YAML::Dump(v).c_str());
+            LOG_ERROR("Invalid interface entry (missing 'name'): {}", YAML::Dump(v).c_str());
             continue;
           }
           const std::string token = name_node.as<std::string>("");
@@ -126,7 +127,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
             }
           }
         } else {
-          RCLCPP_ERROR(node_->get_logger(), "Unsupported interface entry type: %s", YAML::Dump(v).c_str());
+          LOG_ERROR("Unsupported interface entry type: {}", YAML::Dump(v).c_str());
         }
       }
     }
@@ -135,7 +136,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
     register_interfaces_(s);
   }
 
-  RCLCPP_INFO(node_->get_logger(), "Loaded %zu skills from %s", skills_.size(), yaml_path.c_str());
+  LOG_INFO("Loaded {} skills from {}", skills_.size(), yaml_path.c_str());
   return true;
 }
 
@@ -275,15 +276,15 @@ static std::string join_keys(const std::unordered_map<std::string, T> & m)
 }
 
 template<size_t I = 0>
-static void validate_action_registrars_complete(rclcpp::Logger logger)
+static void validate_action_registrars_complete()
 {
   if constexpr (I < std::tuple_size<SkillActionTypes>::value) {
     using ActionT = std::tuple_element_t<I, SkillActionTypes>;
     const char * name = SkillActionTrait<ActionT>::skill_name;
     if (get_action_registrars().find(name) == get_action_registrars().end()) {
-      RCLCPP_ERROR(logger, "Missing action registrar for '%s'", name);
+      LOG_ERROR("Missing action registrar for '{}'", name);
     }
-    validate_action_registrars_complete<I + 1>(logger);
+    validate_action_registrars_complete<I + 1>();
   }
 }
 
@@ -300,7 +301,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
   // One-time runtime validation to ensure all actions in SkillActionTypes have a registrar.
   static bool s_validated = false;
   if (!s_validated) {
-    validate_action_registrars_complete(node_->get_logger());
+    validate_action_registrars_complete();
     s_validated = true;
   }
 
@@ -311,10 +312,12 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
     return prefix + "/" + base;
   };
 
+  const std::string skill_snake = to_snake_case(s.name);
+
   for (const auto & iface : s.interfaces) {
     auto parsed = parse_interface_entry(iface);
     if (!parsed) {
-      RCLCPP_ERROR(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
+      LOG_ERROR("Skip invalid interface entry: {}", iface.c_str());
       continue;
     }
 
@@ -323,8 +326,8 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
     const std::string base_snake = to_snake_case(parsed->base);
 
     auto resolve_topic_name = [&](const std::string & kind_suffix) {
-      // Compute the prior default (prefix + base_snake), but allow per-trait default topic when provided.
-      const std::string prefix_default = join_topic(prefix_value, base_snake);
+      // Compute the prior default (prefix + skill_snake), but allow per-trait default topic when provided.
+      const std::string prefix_default = join_topic(prefix_value, skill_snake);
       const auto & action_defaults = get_action_default_topics();
       const auto & service_defaults = get_service_default_topics();
       std::string trait_default;
@@ -342,8 +345,10 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
         yaml_override = it_yaml->second;
       }
       const bool has_trait_default = !trait_default.empty();
-      const std::string computed_default = !yaml_override.empty() ? yaml_override : (has_trait_default ? trait_default : prefix_default);
-      const std::string param_name = base_snake + "." + kind_suffix + "_name";
+      // If skill name doesn't match interface base, it's a specific instance; prefer prefix_default (skill_snake) over generic trait_default.
+      const std::string computed_default = !yaml_override.empty() ? yaml_override : 
+                                           ((skill_snake == base_snake && has_trait_default) ? trait_default : prefix_default);
+      const std::string param_name = skill_snake + "." + kind_suffix + "_name";
       std::string topic_override;
       if (node_->get_parameter(param_name, topic_override)) {
         // If user explicitly sets an empty parameter, fall back to computed_default.
@@ -362,7 +367,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
       if (it != action_regs.end()) {
         it->second(this, topic_name, s.name);
       } else {
-        RCLCPP_ERROR(node_->get_logger(), "No action registrar for interface base '%s'. Available: [%s]",
+        LOG_ERROR("No action registrar for interface base '{}'. Available: [{}]",
           parsed->base.c_str(), join_keys(action_regs).c_str());
       }
     } else if (parsed->kind == "srv") {
@@ -372,7 +377,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
       if (it != service_regs.end()) {
         it->second(this, topic_name, s.name);
       } else {
-        RCLCPP_ERROR(node_->get_logger(), "No service registrar for interface base '%s'. Available: [%s]",
+        LOG_ERROR("No service registrar for interface base '{}'. Available: [{}]",
           parsed->base.c_str(), join_keys(service_regs).c_str());
       }
     }

src/brain_interfaces/CMakeLists.txt

@@ -1,56 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(brain_interfaces)
-
-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)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-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
-)
-set(srv_files
-  srv/VisionDemo.srv
-)
-
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-rosidl_generate_interfaces(${PROJECT_NAME}
-  ${msg_files}
-  ${action_files}
-  ${srv_files}
-  
-  DEPENDENCIES
-    std_msgs
-    geometry_msgs
-    builtin_interfaces
-    interfaces
-)
-
-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_export_dependencies(rosidl_default_runtime)
-ament_package()

src/brain_interfaces/msg/Movej.msg

@@ -1,5 +0,0 @@
-float32[] joint
-uint8 speed
-bool block
-uint8 trajectory_connect   #0 代表立即规划，1 代表和下一条轨迹一起规划，当为 1 时，轨迹不会立即执行
-uint8 dof

src/brain_interfaces/package.xml

@@ -1,23 +0,0 @@
-<?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>brain_interfaces</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>
-  <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>
-
-  <member_of_group>rosidl_interface_packages</member_of_group>
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/brain_interfaces/srv/VisionDemo.srv

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

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

@@ -44,64 +44,48 @@ private:
 
       // Fake two types of objects (example: cup / bottle), each type gives 1~2 Poses
       interfaces::msg::PoseClassAndID obj1;
-      obj1.class_name = "bottlex";
+      obj1.class_name = "medicine_box";
       obj1.class_id = 1;
 
-      //{-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
       geometry_msgs::msg::Pose pose1; // 基本位姿
-      // pose1.position.x = 0.9758;
-      // pose1.position.y = 0;
-      // pose1.position.z = 0.42;
-      // pose1.orientation.x = -0.5;
-      // pose1.orientation.y = 0.5;
-      // pose1.orientation.z = 0.5;
-      // pose1.orientation.w = -0.5;
 
-      //geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-0.2851924786746129, y=-0.056353812689333635, z=1.073772448259523), 
-      //orientation=geometry_msgs.msg.Quaternion(x=-0.16713377464857188, y=-0.42460237568763715, z=-0.26706232441180955, w=0.8487972895879773))
-      // pose1.position.x = -0.2851924786746129;
-      // pose1.position.y = -0.056353812689333635;
-      // pose1.position.z = 1.073772448259523;
-      // pose1.orientation.x = -0.16713377464857188;
-      // pose1.orientation.y = -0.42460237568763715;
-      // pose1.orientation.z = -0.26706232441180955;
-      // pose1.orientation.w = 0.8487972895879773;
+      if (req->camera_position == "top") {
+        //0.1647, 0.7436, -0.2081, 0.4809, 0.7398, 0.4006, 0.2469
+        pose1.position.x = 0.1647;
+        pose1.position.y = 0.7436;
+        pose1.position.z = -0.2081;
+        pose1.orientation.x = 0.4809;
+        pose1.orientation.y = 0.7398;
+        pose1.orientation.z = 0.4006;
+        pose1.orientation.w = 0.2469;
+      } else if (req->camera_position == "left") {
+        //0.2320, -0.6137, 0.2141, 0.3670, 0.7705, -0.4314, 0.2924
+        pose1.position.x = 0.2320;
+        pose1.position.y = 0.6137;
+        pose1.position.z = 0.2141;
+        pose1.orientation.x = -0.3670;
+        pose1.orientation.y = 0.7705;
+        pose1.orientation.z = 0.4314;
+        pose1.orientation.w = 0.2924;
+      } else if (req->camera_position == "right") {
+        //-0.0165, 0.0246, 0.4166, 0.2939, -0.2219, -0.6882, 0.6251
+        pose1.position.x = -0.0165;
+        pose1.position.y = 0.0246;
+        pose1.position.z = 0.4166;
+        pose1.orientation.x = 0.2939;
+        pose1.orientation.y = -0.2219;
+        pose1.orientation.z = -0.6882;
+        pose1.orientation.w = 0.6251;
+      }
 
-      //222.853, 514.124, 261.742
-      //-0.65115316, 0.05180144, -0.19539139, 0.73153153
-      //-0.63142093, 0.18186004, -0.12407289, 0.74353241
-      pose1.position.x = 222.853;
-      pose1.position.y = 514.124;
-      pose1.position.z = 261.742;
-      pose1.orientation.x = -0.65115316;
-      pose1.orientation.y = 0.05180144;
-      pose1.orientation.z = -0.19539139;
-      pose1.orientation.w = 0.73153153;
-
-      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 = "cup";
-      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);
+      obj1.pose = pose1;
+      obj1.grab_width = {0.046393, 0.065393, 0.046393}; // Example grab widths
 
       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()) << ")";
+      oss << "Recognized " << res->objects.size() << " object classes (total instances: " << res->objects.size() << ")";
       res->info = oss.str();
     } else {
       res->success = false;

src/scripts/build.sh

@@ -1,4 +0,0 @@
-#!/bin/bash
-
-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

src/scripts/euler_to_equa.py

@@ -1,46 +0,0 @@
-import numpy as np
-from scipy.spatial.transform import Rotation as R
-
-# # 欧拉角
-# rx, ry, rz = -1.433, 0.114, -0.430
-
-# # 尝试不同的旋转顺序
-# orders = ['xyz', 'zyx', 'xzy', 'yxz', 'yzx', 'zyx']
-
-# print("尝试不同旋转顺序的结果：")
-# for order in orders:
-#     try:
-#         rot = R.from_euler(order, [rx, ry, rz], degrees=False)
-#         quat = rot.as_quat()  # [x, y, z, w]
-#         print(f"顺序 {order}: [{quat[0]:.8f}, {quat[1]:.8f}, {quat[2]:.8f}, {quat[3]:.8f}]")
-#     except:
-#         continue
-
-# # 特别检查XYZ顺序
-# print("\nXYZ顺序的详细计算：")
-# rot_xyz = R.from_euler('xyz', [rx, ry, rz], degrees=False)
-# quat_xyz = rot_xyz.as_quat()
-# print(f"XYZ顺序: [{quat_xyz[0]:.8f}, {quat_xyz[1]:.8f}, {quat_xyz[2]:.8f}, {quat_xyz[3]:.8f}]")
-
-
-# 输入欧拉角 (rx, ry, rz)，单位弧度
-# euler_angles_rad = [-1.433, 0.114, -0.430]
-# euler_angles_rad = [-1.622, -0.016, -0.4]
-# euler_angles_rad = [-0.972, -0.557, -0.901]
-# euler_angles_rad = [-1.674, 0.223, -0.747]
-# euler_angles_rad = [-1.484, 0.152, -0.26]
-# euler_angles_rad = [-1.691, -0.443, -0.452]
-# euler_angles_rad = [-1.72, -0.189, -0.223]
-# euler_angles_rad = [-2.213, -0.202, -0.391]
-# euler_angles_rad = [-1.358, -0.284, -0.255]
-# euler_angles_rad = [-1.509, -0.234, -0.416]
-# euler_angles_rad = [-1.79, -0.447, -0.133]
-euler_angles_rad = [-1.468, 0.199, -0.361]
-
-# 创建旋转对象，指定欧拉角顺序为 'xyz'
-rot = R.from_euler('xyz', euler_angles_rad, degrees=False)
-
-# 转换为四元数，格式为 [x, y, z, w]
-quaternion = rot.as_quat()
-
-print(f"四元数 (x, y, z, w): [{quaternion[0]:.8f}, {quaternion[1]:.8f}, {quaternion[2]:.8f}, {quaternion[3]:.8f}]")

src/scripts/euler_to_quaternion.py

@@ -1,207 +0,0 @@
-#!/usr/bin/env python3
-"""
-Convert poses to quaternions (qx, qy, qz, qw) from either:
-- Euler angles (rx, ry, rz)
-- Rotation vector (Rodrigues) (rx, ry, rz) where |r| is angle in rad
-
-Supports:
-- CSV input/output with configurable column names
-- Unit options: radians/degrees for angles, mm->m for position
-- Euler order: xyz (default) or zyx
-
-Examples:
-  python src/scripts/euler_to_quaternion.py \
-        --in poses.csv --out poses_quat.csv --mode euler \
-    --rx rx --ry ry --rz rz --order xyz \
-    --x x --y y --z z --mm-to-m
-
-  # Single value
-    python src/scripts/euler_to_quaternion.py --mode euler --single 0.1 0.2 -0.3 --order zyx
-
-    # Rotation vector (e.g., from many robots):
-    python src/scripts/euler_to_quaternion.py --mode rotvec --single -1.433 0.114 -0.430
-"""
-from __future__ import annotations
-
-import argparse
-import csv
-import math
-from typing import Tuple, List
-
-
-def euler_to_quaternion(rx: float, ry: float, rz: float, order: str = "xyz", degrees: bool = False) -> Tuple[float, float, float, float]:
-    """Convert Euler angles to quaternion [x, y, z, w].
-
-    Args:
-        rx, ry, rz: Euler angles. If degrees=True, values are in degrees; else radians.
-        order: Rotation order. Supported: 'xyz' (default), 'zyx'. Intrinsic rotations.
-        degrees: Whether the input angles are in degrees.
-
-    Returns:
-        (qx, qy, qz, qw)
-    """
-    if degrees:
-        rx, ry, rz = math.radians(rx), math.radians(ry), math.radians(rz)
-
-    def q_from_axis_angle(ax: str, angle: float) -> Tuple[float, float, float, float]:
-        s = math.sin(angle / 2.0)
-        c = math.cos(angle / 2.0)
-        if ax == 'x':
-            return (s, 0.0, 0.0, c)
-        if ax == 'y':
-            return (0.0, s, 0.0, c)
-        if ax == 'z':
-            return (0.0, 0.0, s, c)
-        raise ValueError('Invalid axis')
-
-    def q_mul(q1: Tuple[float, float, float, float], q2: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]:
-        x1, y1, z1, w1 = q1
-        x2, y2, z2, w2 = q2
-        x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
-        y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
-        z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
-        w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
-        return (x, y, z, w)
-
-    if order == 'xyz':
-        qx = q_from_axis_angle('x', rx)
-        qy = q_from_axis_angle('y', ry)
-        qz = q_from_axis_angle('z', rz)
-        q = q_mul(q_mul(qx, qy), qz)
-    elif order == 'zyx':
-        qz = q_from_axis_angle('z', rz)
-        qy = q_from_axis_angle('y', ry)
-        qx = q_from_axis_angle('x', rx)
-        q = q_mul(q_mul(qz, qy), qx)
-    else:
-        raise ValueError("Unsupported Euler order. Use 'xyz' or 'zyx'.")
-
-    # Normalize
-    norm = math.sqrt(sum(v * v for v in q))
-    return tuple(v / (norm + 1e-12) for v in q)  # type: ignore
-
-
-def rotvec_to_quaternion(rx: float, ry: float, rz: float) -> Tuple[float, float, float, float]:
-    """Convert rotation vector (Rodrigues) to quaternion [x,y,z,w].
-    The vector direction is the rotation axis and its norm is the rotation angle in radians.
-    """
-    angle = math.sqrt(rx * rx + ry * ry + rz * rz)
-    if angle < 1e-12:
-        return (0.0, 0.0, 0.0, 1.0)
-    ax = rx / angle
-    ay = ry / angle
-    az = rz / angle
-    s = math.sin(angle / 2.0)
-    c = math.cos(angle / 2.0)
-    q = (ax * s, ay * s, az * s, c)
-    norm = math.sqrt(sum(v * v for v in q))
-    return tuple(v / (norm + 1e-12) for v in q)  # type: ignore
-
-
-def convert_csv(in_path: str, out_path: str,
-                rx_col: str = 'rx', ry_col: str = 'ry', rz_col: str = 'rz',
-                x_col: str | None = None, y_col: str | None = None, z_col: str | None = None,
-                order: str = 'xyz', degrees: bool = False, mm_to_m: bool = False,
-                mode: str = 'euler') -> None:
-    """Read CSV, append quaternion columns (qx,qy,qz,qw) and write to output.
-
-    If x/y/z columns are provided, optionally convert mm->m.
-    """
-    with open(in_path, 'r', newline='') as f_in:
-        reader = csv.DictReader(f_in)
-        fieldnames: List[str] = list(reader.fieldnames or [])
-        # Ensure quaternion columns at the end
-        for c in ['qx', 'qy', 'qz', 'qw']:
-            if c not in fieldnames:
-                fieldnames.append(c)
-        # If converting position units, overwrite or add x_m/y_m/z_m
-        if x_col and y_col and z_col and mm_to_m:
-            for c in ['x_m', 'y_m', 'z_m']:
-                if c not in fieldnames:
-                    fieldnames.append(c)
-
-        with open(out_path, 'w', newline='') as f_out:
-            writer = csv.DictWriter(f_out, fieldnames=fieldnames)
-            writer.writeheader()
-            for row in reader:
-                try:
-                    rx = float(row[rx_col])
-                    ry = float(row[ry_col])
-                    rz = float(row[rz_col])
-                except KeyError as e:
-                    raise KeyError(f"Missing rotation columns in CSV: {e}")
-                if mode == 'euler':
-                    qx, qy, qz, qw = euler_to_quaternion(rx, ry, rz, order=order, degrees=degrees)
-                elif mode == 'rotvec':
-                    qx, qy, qz, qw = rotvec_to_quaternion(rx, ry, rz)
-                else:
-                    raise ValueError("mode must be 'euler' or 'rotvec'")
-                row.update({'qx': f"{qx:.8f}", 'qy': f"{qy:.8f}", 'qz': f"{qz:.8f}", 'qw': f"{qw:.8f}"})
-
-                if x_col and y_col and z_col and (x_col in row and y_col in row and z_col in row):
-                    try:
-                        x = float(row[x_col])
-                        y = float(row[y_col])
-                        z = float(row[z_col])
-                        if mm_to_m:
-                            row['x_m'] = f"{x / 1000.0:.6f}"
-                            row['y_m'] = f"{y / 1000.0:.6f}"
-                            row['z_m'] = f"{z / 1000.0:.6f}"
-                    except ValueError:
-                        pass
-
-                writer.writerow(row)
-
-
-def main():
-    parser = argparse.ArgumentParser(description='Convert pose angles to quaternion (qx,qy,qz,qw) from Euler or rotation vector.')
-    parser.add_argument('--mode', type=str, default='euler', choices=['euler', 'rotvec'], help='Input angle format')
-    parser.add_argument('--in', dest='in_path', type=str, help='Input CSV file path')
-    parser.add_argument('--out', dest='out_path', type=str, help='Output CSV file path')
-    parser.add_argument('--rx', type=str, default='rx', help='Column name for rx')
-    parser.add_argument('--ry', type=str, default='ry', help='Column name for ry')
-    parser.add_argument('--rz', type=str, default='rz', help='Column name for rz')
-    parser.add_argument('--x', type=str, default=None, help='Column name for x (position)')
-    parser.add_argument('--y', type=str, default=None, help='Column name for y (position)')
-    parser.add_argument('--z', type=str, default=None, help='Column name for z (position)')
-    parser.add_argument('--order', type=str, default='xyz', choices=['xyz', 'zyx'], help='Euler order')
-    parser.add_argument('--degrees', action='store_true', help='Input angles are in degrees (default radians)')
-    parser.add_argument('--mm-to-m', action='store_true', help='Convert position units from mm to m when x/y/z provided')
-
-    # single conversion mode
-    parser.add_argument('--single', nargs=3, type=float, metavar=('RX', 'RY', 'RZ'),
-                        help='Convert a single Euler triplet to quaternion (prints to stdout)')
-
-    args = parser.parse_args()
-
-    if args.single is not None:
-        rx, ry, rz = args.single
-        if args.mode == 'euler':
-            q = euler_to_quaternion(rx, ry, rz, order=args.order, degrees=args.degrees)
-        else:
-            q = rotvec_to_quaternion(rx, ry, rz)
-        print(f"qx,qy,qz,qw = {q[0]:.8f}, {q[1]:.8f}, {q[2]:.8f}, {q[3]:.8f}")
-        return
-
-    if not args.in_path or not args.out_path:
-        parser.error('CSV mode requires --in and --out')
-
-    convert_csv(
-        in_path=args.in_path,
-        out_path=args.out_path,
-        rx_col=args.rx,
-        ry_col=args.ry,
-        rz_col=args.rz,
-        x_col=args.x,
-        y_col=args.y,
-        z_col=args.z,
-        order=args.order,
-        degrees=args.degrees,
-        mm_to_m=args.mm_to_m,
-        mode=args.mode,
-    )
-    print(f"Written: {args.out_path}")
-
-
-if __name__ == '__main__':
-    main()

src/scripts/gen_bt_params.py

@@ -31,6 +31,7 @@ Notes:
 from __future__ import annotations
 
 import argparse
+import glob
 import os
 import re
 import sys
@@ -94,6 +95,51 @@ def resolve_interface_subdir(root: str, sub: str) -> str:
     return candidates[0]
 
 
+def normalize_ros_type(ros_type: str) -> str:
+    bt = ros_type.replace('::', '/')
+    bt = bt.replace('/msg/', '/').replace('/Msg/', '/')
+    return bt
+
+
+def split_pkg_msg(ros_type: str):
+    if '/' not in ros_type:
+        return None, None
+    pkg, msg = ros_type.split('/', 1)
+    return pkg, msg
+
+
+def find_msg_file(interfaces_root: str, pkg: str, msg: str) -> Optional[str]:
+    if not pkg or not msg:
+        return None
+    if pkg == 'interfaces':
+        msg_dir = resolve_interface_subdir(interfaces_root, 'msg')
+        if not os.path.isdir(msg_dir):
+            return None
+        msg_file = os.path.join(msg_dir, msg + '.msg')
+        if os.path.exists(msg_file):
+            return msg_file
+        found = find_file_case_insensitive(msg_dir, msg + '.msg')
+        if found:
+            return found
+        snake_path = os.path.join(msg_dir, to_snake(msg) + '.msg')
+        if os.path.exists(snake_path):
+            return snake_path
+        return None
+
+    distro = os.environ.get('ROS_DISTRO')
+    if distro:
+        candidate = os.path.join('/opt/ros', distro, 'share', pkg, 'msg', msg + '.msg')
+        if os.path.exists(candidate):
+            return candidate
+    for candidate in glob.glob(os.path.join('/opt/ros', '*', 'share', pkg, 'msg', msg + '.msg')):
+        if os.path.exists(candidate):
+            return candidate
+    candidate = os.path.join('/usr/share', pkg, 'msg', msg + '.msg')
+    if os.path.exists(candidate):
+        return candidate
+    return None
+
+
 def parse_action_file(path: str) -> List[Tuple[str, str]]:
     """Parse a .action file, return list of (ros_type, field_name) only for Goal part."""
     try:
@@ -108,6 +154,11 @@ def parse_action_file(path: str) -> List[Tuple[str, str]]:
         line = line.strip()
         if not line or line.startswith('#'):
             continue
+        line = line.split('#', 1)[0].strip()
+        if not line:
+            continue
+        if '=' in line:
+            continue
         m = re.match(r"^([A-Za-z0-9_\[\]/]+)\s+([A-Za-z0-9_]+).*$", line)
         if m:
             ty = m.group(1)
@@ -116,32 +167,74 @@ def parse_action_file(path: str) -> List[Tuple[str, str]]:
     return fields
 
 
-def locate_action_file(interfaces_root: str, base: str) -> Optional[str]:
-    action_dir = resolve_interface_subdir(interfaces_root, 'action')
+def parse_msg_file(path: str) -> List[Tuple[str, str]]:
+    """Parse a .msg file, return list of (ros_type, field_name)."""
+    try:
+        with open(path, 'r', encoding='utf-8') as f:
+            content = f.read()
+    except FileNotFoundError:
+        return []
+    fields: List[Tuple[str, str]] = []
+    for line in content.splitlines():
+        line = line.strip()
+        if not line or line.startswith('#'):
+            continue
+        line = line.split('#', 1)[0].strip()
+        if not line:
+            continue
+        if '=' in line:
+            continue
+        m = re.match(r"^([A-Za-z0-9_\[\]/]+)\s+([A-Za-z0-9_]+).*$", line)
+        if m:
+            ty = m.group(1)
+            name = m.group(2)
+            fields.append((ty, name))
+    return fields
+
+
+_MSG_FIELDS_CACHE: Dict[Tuple[str, str], Optional[List[Tuple[str, str]]]] = {}
+
+
+def get_msg_fields(interfaces_root: str, pkg: str, msg: str) -> Optional[List[Tuple[str, str]]]:
+    key = (pkg, msg)
+    if key in _MSG_FIELDS_CACHE:
+        return _MSG_FIELDS_CACHE[key]
+    path = find_msg_file(interfaces_root, pkg, msg)
+    if path and os.path.exists(path):
+        fields = parse_msg_file(path)
+    else:
+        fields = None
+    _MSG_FIELDS_CACHE[key] = fields
+    return fields
+
+
+def locate_action_file(interfaces_root: str, base: str, type: str) -> Optional[str]:
+    action_dir = resolve_interface_subdir(interfaces_root, type)
     # 1) Try as-is (CamelCase)
-    candidate = os.path.join(action_dir, f'{base}.action')
+    candidate = os.path.join(action_dir, f'{base}.{type}')
     if os.path.exists(candidate):
         return candidate
     # 2) Try CamelCase of snake
     camel = snake_to_camel(base)
     if camel != base:
-        candidate2 = os.path.join(action_dir, f'{camel}.action')
+        candidate2 = os.path.join(action_dir, f'{camel}.{type}')
         if os.path.exists(candidate2):
             return candidate2
     # 3) Try snake_case
     snake = to_snake(base)
-    candidate3 = os.path.join(action_dir, f'{snake}.action')
+    candidate3 = os.path.join(action_dir, f'{snake}.{type}')
     if os.path.exists(candidate3):
         return candidate3
     # 4) Case-insensitive lookup
-    found = find_file_case_insensitive(action_dir, f'{base}.action')
+    found = find_file_case_insensitive(action_dir, f'{base}.{type}')
     if found:
         return found
     # Not found
     return None
 
 
-def make_placeholder_for_type(ros_type: str) -> object:
+def make_placeholder_for_type(ros_type: str, interfaces_root: str, current_pkg: Optional[str] = None,
+                              visited: Optional[set] = None) -> object:
     """Return a default placeholder Python value for a ROS field type.
 
     - Primitive -> default numeric/bool/string
@@ -150,42 +243,62 @@ def make_placeholder_for_type(ros_type: str) -> object:
     - geometry_msgs/TwistStamped -> nested dict with common keys
     - Otherwise -> None
     """
+    if visited is None:
+        visited = set()
     is_array = ros_type.endswith('[]')
     base = ros_type[:-2] if is_array else ros_type
-    # normalize namespace
-    base_norm = base.replace('::', '/')
-    base_norm = base_norm.replace('/msg/', '/').replace('/Msg/', '/')
+    base_norm = normalize_ros_type(base)
 
-    if is_array:
-        return []
     if '/' not in base_norm:
         # primitive
-        return PRIMITIVES_DEFAULTS.get(base_norm, None)
-    if base_norm in ("geometry_msgs/PoseStamped",):
-        return {
-            'frame_id': '',
-            'stamp_sec': 0,
-            'stamp_nanosec': 0,
+        val = PRIMITIVES_DEFAULTS.get(base_norm, None)
+        return [] if is_array else val
+
+    # known geometry messages fallback
+    if base_norm in ("geometry_msgs/Pose",):
+        val = {
             'position': {'x': 0.0, 'y': 0.0, 'z': 0.0},
             'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0},
         }
-    if base_norm in ("geometry_msgs/TwistStamped",):
-        return {
-            'frame_id': '',
-            'stamp_sec': 0,
-            'stamp_nanosec': 0,
-            'linear': {'x': 0.0, 'y': 0.0, 'z': 0.0},
-            'angular': {'x': 0.0, 'y': 0.0, 'z': 0.0},
-        }
-    # unknown complex -> None
-    return None
+        return [val] if is_array else val
+    if base_norm in ("geometry_msgs/Point",):
+        val = {'x': 0.0, 'y': 0.0, 'z': 0.0}
+        return [val] if is_array else val
+    if base_norm in ("geometry_msgs/Quaternion",):
+        val = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
+        return [val] if is_array else val
+
+    pkg, msg = split_pkg_msg(base_norm)
+    if not pkg:
+        pkg = current_pkg
+        msg = base_norm
+    if not pkg or not msg:
+        return [] if is_array else None
+
+    if (pkg, msg) in visited:
+        return [] if is_array else None
+    visited.add((pkg, msg))
+
+    fields = get_msg_fields(interfaces_root, pkg, msg)
+    if not fields:
+        visited.discard((pkg, msg))
+        return [] if is_array else None
+
+    mapping: Dict[str, object] = {}
+    for ftype, fname in fields:
+        mapping[fname] = make_placeholder_for_type(ftype, interfaces_root, pkg, visited)
+
+    visited.discard((pkg, msg))
+    if is_array:
+        return [mapping]
+    return mapping
 
 
-def build_yaml_params_from_fields(fields: Sequence[Tuple[str, str]]) -> str:
+def build_yaml_params_from_fields(fields: Sequence[Tuple[str, str]], interfaces_root: str) -> str:
     """Create a YAML snippet string from .action Goal fields."""
     mapping: Dict[str, object] = {}
     for ros_type, name in fields:
-        mapping[name] = make_placeholder_for_type(ros_type)
+        mapping[name] = make_placeholder_for_type(ros_type, interfaces_root)
     # Dump as YAML block string
     # Ensure multi-line by setting default_flow_style=False
     snippet = yaml.safe_dump(mapping, allow_unicode=True, sort_keys=False)
@@ -220,6 +333,7 @@ def main():
     ap = argparse.ArgumentParser(description='Generate YAML params for BT nodes.')
     ap.add_argument('--xml', default='src/brain/config/bt_carry_boxes.xml', help='Path to BehaviorTree XML')
     ap.add_argument('--interfaces', default='../hivecore_robot_interfaces', help='Path to hivecore_robot_interfaces root')
+    ap.add_argument('--skills', default='src/brain/config/robot_skills.yaml', help='Path to robot_skills.yaml')
     ap.add_argument('--out', default='src/brain/config/bt_carry_boxes.params.default.yaml', help='Output YAML path')
     args = ap.parse_args()
 
@@ -227,21 +341,51 @@ def main():
     repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
     xml_path = args.xml if os.path.isabs(args.xml) else os.path.join(repo_root, args.xml)
     interfaces_root = args.interfaces if os.path.isabs(args.interfaces) else os.path.join(repo_root, args.interfaces)
+    skills_path = args.skills if os.path.isabs(args.skills) else os.path.join(repo_root, args.skills)
     out_path = args.out if os.path.isabs(args.out) else os.path.join(repo_root, args.out)
 
     if not os.path.exists(xml_path):
         print(f'ERROR: XML not found: {xml_path}', file=sys.stderr)
         sys.exit(2)
 
+    # Load mapping from skills.yaml if available
+    skill_to_interface: Dict[str, str] = {}
+    if os.path.exists(skills_path):
+        try:
+            with open(skills_path, 'r', encoding='utf-8') as f:
+                skills_data = yaml.safe_load(f)
+                if isinstance(skills_data, list):
+                    for skill in skills_data:
+                        name = skill.get('name')
+                        if not name: continue
+                        ifaces = skill.get('interfaces', [])
+                        for iface in ifaces:
+                            token = None
+                            if isinstance(iface, str):
+                                token = iface
+                            elif isinstance(iface, dict):
+                                token = iface.get('name')
+                            if token and '.' in token:
+                                base = token.split('.', 1)[0]
+                                skill_to_interface[name] = base
+                                break # Use first action/srv found
+        except Exception as e:
+            print(f"WARN: Failed to parse skills YAML {skills_path}: {e}", file=sys.stderr)
+
     nodes = collect_bt_nodes(xml_path)
     results = []
 
     for instance_name, node_tag in nodes:
         # Map tag to interface base by stripping trailing '_H' if present
         base = node_tag[:-2] if node_tag.endswith('_H') else node_tag
-        action_file = locate_action_file(interfaces_root, base)
+        # If tag (like SkillName_H) refers to a skill in robot_skills.yaml, resolve to its interface
+        base = skill_to_interface.get(base, base)
+
+        action_file = locate_action_file(interfaces_root, base, "action")
+        if action_file == None:
+            action_file = locate_action_file(interfaces_root, base, "srv")
         fields: List[Tuple[str, str]] = parse_action_file(action_file) if action_file else []
-        yaml_params = build_yaml_params_from_fields(fields)
+        yaml_params = build_yaml_params_from_fields(fields, interfaces_root)
         results.append({'name': instance_name, 'params': yaml_params})
 
     # Write output as YAML list; ensure deterministic order as in XML

src/scripts/gen_payload_converters.py

@@ -19,6 +19,7 @@ and array primitives. For complex ROS message types it will emit a TODO comment.
 """
 
 import argparse
+import glob
 import os
 import re
 import sys
@@ -66,15 +67,10 @@ HEADER_POSTAMBLE = '\n} // namespace brain\n'
 
 
 def to_snake(s: str) -> str:
-    out = ''
-    for i,ch in enumerate(s):
-        if ch.isupper():
-            if i>0:
-                out += '_'
-            out += ch.lower()
-        else:
-            out += ch
-    return out
+    # Convert CamelCase (including initialisms like ASRRecognize) to snake_case
+    s1 = re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', s)
+    s2 = re.sub(r'([a-z0-9])([A-Z])', r'\1_\2', s1)
+    return s2.lower()
 
 
 def parse_action_file(path: str):
@@ -89,6 +85,13 @@ def parse_action_file(path: str):
         line = line.strip()
         if not line or line.startswith('#'):
             continue
+        # Strip comments before checking constants
+        line = line.split('#', 1)[0].strip()
+        if not line:
+            continue
+        # Skip constants like: uint8 STATUS_PLANNING=0
+        if '=' in line:
+            continue
         # lines like: float32 move_pitch_degree
         m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
         if m:
@@ -109,6 +112,37 @@ def parse_srv_file(path: str):
         line = line.strip()
         if not line or line.startswith('#'):
             continue
+        # Strip comments before checking constants
+        line = line.split('#', 1)[0].strip()
+        if not line:
+            continue
+        # Skip constants like: uint8 STATUS_PLANNING=0
+        if '=' in line:
+            continue
+        m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
+        if m:
+            ty = m.group(1)
+            name = m.group(2)
+            fields.append((ty, name))
+    return fields
+
+
+def parse_msg_file(path: str):
+    """Parse a .msg file, return list of (type,name) fields."""
+    with open(path, 'r', encoding='utf-8') as f:
+        content = f.read()
+    fields = []
+    for line in content.splitlines():
+        line = line.strip()
+        if not line or line.startswith('#'):
+            continue
+        # Strip comments before checking constants
+        line = line.split('#', 1)[0].strip()
+        if not line:
+            continue
+        # Skip constants like: uint8 STATUS_PLANNING=0
+        if '=' in line:
+            continue
         m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
         if m:
             ty = m.group(1)
@@ -160,7 +194,148 @@ def resolve_interface_subdir(root: str, sub: str):
     return candidates[0]
 
 
-def gen_converter_action(base: str, fields, include_path: str):
+def normalize_ros_type(ros_type: str) -> str:
+    bt = ros_type.replace('::', '/')
+    bt = bt.replace('/msg/', '/')
+    bt = bt.replace('/Msg/', '/')
+    return bt
+
+
+def split_pkg_msg(ros_type: str):
+    if '/' not in ros_type:
+        return None, None
+    pkg, msg = ros_type.split('/', 1)
+    return pkg, msg
+
+
+def find_msg_file(interfaces_root: str, pkg: str, msg: str):
+    if not pkg or not msg:
+        return None
+    if pkg == 'interfaces':
+        msg_dir = resolve_interface_subdir(interfaces_root, 'msg')
+        if not os.path.isdir(msg_dir):
+            return None
+        msg_file = os.path.join(msg_dir, msg + '.msg')
+        if os.path.exists(msg_file):
+            return msg_file
+        found = find_file_case_insensitive(msg_dir, msg + '.msg')
+        if found:
+            return found
+        snake_path = os.path.join(msg_dir, to_snake(msg) + '.msg')
+        if os.path.exists(snake_path):
+            return snake_path
+        return None
+
+    # Try common ROS share locations for other packages (e.g. geometry_msgs)
+    distro = os.environ.get('ROS_DISTRO')
+    if distro:
+        candidate = os.path.join('/opt/ros', distro, 'share', pkg, 'msg', msg + '.msg')
+        if os.path.exists(candidate):
+            return candidate
+    for candidate in glob.glob(os.path.join('/opt/ros', '*', 'share', pkg, 'msg', msg + '.msg')):
+        if os.path.exists(candidate):
+            return candidate
+    candidate = os.path.join('/usr/share', pkg, 'msg', msg + '.msg')
+    if os.path.exists(candidate):
+        return candidate
+    return None
+
+
+_MSG_FIELDS_CACHE = {}
+
+
+def get_msg_fields(interfaces_root: str, pkg: str, msg: str):
+    key = (pkg, msg)
+    if key in _MSG_FIELDS_CACHE:
+        return _MSG_FIELDS_CACHE[key]
+    path = find_msg_file(interfaces_root, pkg, msg)
+    if path and os.path.exists(path):
+        fields = parse_msg_file(path)
+    else:
+        fields = None
+    _MSG_FIELDS_CACHE[key] = fields
+    return fields
+
+
+def msg_cpp_type(pkg: str, msg: str):
+    return f'{pkg}::msg::{msg}'
+
+
+def emit_msg_fields(cpp, node_expr: str, target_expr: str, fields, indent: str,
+                    interfaces_root: str, visited: set, current_pkg: str, depth: int = 0):
+    for ty, fname in fields:
+        is_array, cpp_type, base_type = make_cpp_type(ty)
+        if is_array and cpp_type:
+            cpp.append(f'{indent}if ({node_expr}["{fname}"]) {{')
+            cpp.append(f'{indent}  auto vec = {node_expr}["{fname}"].as<std::vector<{cpp_type}>>();')
+            cpp.append(f'{indent}  {target_expr}.{fname}.assign(vec.begin(), vec.end());')
+            cpp.append(f'{indent}}}')
+        elif not is_array and cpp_type:
+            cpp.append(f'{indent}if ({node_expr}["{fname}"]) {target_expr}.{fname} = {node_expr}["{fname}"].as<{cpp_type}>();')
+        else:
+            bt = normalize_ros_type(base_type)
+            pkg, msg = split_pkg_msg(bt)
+            if not pkg:
+                pkg = current_pkg
+                msg = bt
+            if pkg and msg and (pkg, msg) not in visited:
+                nested_fields = get_msg_fields(interfaces_root, pkg, msg)
+                if nested_fields:
+                    visited.add((pkg, msg))
+                    if is_array:
+                        cpp.append(f'{indent}if ({node_expr}["{fname}"] && {node_expr}["{fname}"].IsSequence()) {{')
+                        cpp.append(f'{indent}  for (const auto & item : {node_expr}["{fname}"]) {{')
+                        cpp.append(f'{indent}    {msg_cpp_type(pkg, msg)} tmp;')
+                        emit_msg_fields(cpp, 'item', 'tmp', nested_fields, indent + '    ', interfaces_root, visited, pkg, depth + 1)
+                        cpp.append(f'{indent}    {target_expr}.{fname}.push_back(tmp);')
+                        cpp.append(f'{indent}  }}')
+                        cpp.append(f'{indent}}}')
+                    else:
+                        sub_var = f'node_{depth}_{fname}'
+                        cpp.append(f'{indent}if (auto {sub_var} = {node_expr}["{fname}"]) {{')
+                        emit_msg_fields(cpp, sub_var, f'{target_expr}.{fname}', nested_fields, indent + '  ', interfaces_root, visited, pkg, depth + 1)
+                        cpp.append(f'{indent}}}')
+                    visited.discard((pkg, msg))
+                    continue
+            cpp.append(f'{indent}// TODO: parse field `{fname}` of ROS type `{ty}` into {target_expr}.{fname} (complex type)')
+
+
+def emit_complex_field(cpp, name: str, base_type: str, is_array: bool, target_root: str,
+                       interfaces_root: str):
+    bt = normalize_ros_type(base_type)
+    pkg, msg = split_pkg_msg(bt)
+    if not pkg or not msg:
+        return False
+    fields = get_msg_fields(interfaces_root, pkg, msg)
+    if not fields:
+        return False
+    visited = {(pkg, msg)}
+    if is_array:
+        cpp.append(f'    if (n["{name}"] && n["{name}"].IsSequence()) {{')
+        cpp.append(f'      for (const auto & item : n["{name}"]) {{')
+        cpp.append(f'        {msg_cpp_type(pkg, msg)} tmp;')
+        emit_msg_fields(cpp, 'item', 'tmp', fields, '        ', interfaces_root, visited, pkg, 1)
+        cpp.append(f'        {target_root}.{name}.push_back(tmp);')
+        cpp.append('      }')
+        cpp.append('    }')
+    else:
+        sub_var = f'node_0_{name}'
+        cpp.append(f'    if (auto {sub_var} = n["{name}"]) {{')
+        emit_msg_fields(cpp, sub_var, f'{target_root}.{name}', fields, '      ', interfaces_root, visited, pkg, 1)
+        cpp.append('    }')
+    return True
+
+
+def add_msg_includes_for_fields(fields, includes):
+    for ty, _ in fields:
+        base_ty = ty[:-2] if ty.endswith('[]') else ty
+        base_ty = normalize_ros_type(base_ty)
+        pkg, msg = split_pkg_msg(base_ty)
+        if pkg == 'interfaces' and msg:
+            includes.add(f'"interfaces/msg/{to_snake(msg)}.hpp"')
+
+
+def gen_converter_action(base: str, fields, include_path: str, interfaces_root: str):
     # base: MoveWaist
     cpp = []
     cpp.append(f'// {base}::Goal: ' + ', '.join([n for _,n in fields]))
@@ -182,47 +357,7 @@ def gen_converter_action(base: str, fields, include_path: str):
             # map to as<type>()
             cpp.append(f'    if (n["{name}"]) g.{name} = n["{name}"].as<{cpp_type}>();')
         else:
-            # complex types: handle some known messages
-            # normalize geometry type names like geometry_msgs/msg/PoseStamped -> geometry_msgs/PoseStamped
-            bt = base_type.replace('::', '/')
-            bt = bt.replace('/msg/', '/')
-            bt = bt.replace('/Msg/', '/')
-            if bt in ("geometry_msgs/PoseStamped", "geometry_msgs/PoseStamped") and not is_array:
-                cpp.append('    // geometry_msgs::msg::PoseStamped')
-                cpp.append('    if (n) {')
-                cpp.append('      if (n["frame_id"]) g.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
-                cpp.append('      if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
-                cpp.append('      if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
-                cpp.append('      if (auto p = n["position"]) {')
-                cpp.append('        if (p["x"]) g.{0}.pose.position.x = p["x"].as<double>();'.format(name))
-                cpp.append('        if (p["y"]) g.{0}.pose.position.y = p["y"].as<double>();'.format(name))
-                cpp.append('        if (p["z"]) g.{0}.pose.position.z = p["z"].as<double>();'.format(name))
-                cpp.append('      }')
-                cpp.append('      if (auto q = n["orientation"]) {')
-                cpp.append('        if (q["x"]) g.{0}.pose.orientation.x = q["x"].as<double>();'.format(name))
-                cpp.append('        if (q["y"]) g.{0}.pose.orientation.y = q["y"].as<double>();'.format(name))
-                cpp.append('        if (q["z"]) g.{0}.pose.orientation.z = q["z"].as<double>();'.format(name))
-                cpp.append('        if (q["w"]) g.{0}.pose.orientation.w = q["w"].as<double>();'.format(name))
-                cpp.append('      }')
-                cpp.append('    }')
-            elif bt in ("geometry_msgs/TwistStamped", "geometry_msgs/TwistStamped") and not is_array:
-                cpp.append('    // geometry_msgs::msg::TwistStamped')
-                cpp.append('    if (n) {')
-                cpp.append('      if (n["frame_id"]) g.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
-                cpp.append('      if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
-                cpp.append('      if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
-                cpp.append('      if (auto lin = n["linear"]) {')
-                cpp.append('        if (lin["x"]) g.{0}.twist.linear.x = lin["x"].as<double>();'.format(name))
-                cpp.append('        if (lin["y"]) g.{0}.twist.linear.y = lin["y"].as<double>();'.format(name))
-                cpp.append('        if (lin["z"]) g.{0}.twist.linear.z = lin["z"].as<double>();'.format(name))
-                cpp.append('      }')
-                cpp.append('      if (auto ang = n["angular"]) {')
-                cpp.append('        if (ang["x"]) g.{0}.twist.angular.x = ang["x"].as<double>();'.format(name))
-                cpp.append('        if (ang["y"]) g.{0}.twist.angular.y = ang["y"].as<double>();'.format(name))
-                cpp.append('        if (ang["z"]) g.{0}.twist.angular.z = ang["z"].as<double>();'.format(name))
-                cpp.append('      }')
-                cpp.append('    }')
-            else:
+            if not emit_complex_field(cpp, name, base_type, is_array, 'g', interfaces_root):
                 cpp.append(f'    // TODO: parse field `{name}` of ROS type `{ty}` into g.{name} (complex type)')
     # post-processing example: if both data_array and data_length exist, set length from array size
     if has_data_array and has_data_length:
@@ -236,7 +371,7 @@ def gen_converter_action(base: str, fields, include_path: str):
     return '\n'.join(cpp)
 
 
-def gen_converter_srv(base: str, fields, include_path: str):
+def gen_converter_srv(base: str, fields, include_path: str, interfaces_root: str):
     cpp = []
     cpp.append(f'// {base}::Request: ' + ', '.join([n for _,n in fields]))
     cpp.append('template<>')
@@ -254,45 +389,7 @@ def gen_converter_srv(base: str, fields, include_path: str):
         elif not is_array and cpp_type:
             cpp.append(f'    if (n["{name}"]) r.{name} = n["{name}"].as<{cpp_type}>();')
         else:
-            bt = base_type.replace('::', '/')
-            bt = bt.replace('/msg/', '/')
-            bt = bt.replace('/Msg/', '/')
-            if bt in ("geometry_msgs/PoseStamped", "geometry_msgs/PoseStamped") and not is_array:
-                cpp.append('    // geometry_msgs::msg::PoseStamped')
-                cpp.append('    if (n) {')
-                cpp.append('      if (n["frame_id"]) r.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
-                cpp.append('      if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
-                cpp.append('      if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
-                cpp.append('      if (auto p = n["position"]) {')
-                cpp.append('        if (p["x"]) r.{0}.pose.position.x = p["x"].as<double>();'.format(name))
-                cpp.append('        if (p["y"]) r.{0}.pose.position.y = p["y"].as<double>();'.format(name))
-                cpp.append('        if (p["z"]) r.{0}.pose.position.z = p["z"].as<double>();'.format(name))
-                cpp.append('      }')
-                cpp.append('      if (auto q = n["orientation"]) {')
-                cpp.append('        if (q["x"]) r.{0}.pose.orientation.x = q["x"].as<double>();'.format(name))
-                cpp.append('        if (q["y"]) r.{0}.pose.orientation.y = q["y"].as<double>();'.format(name))
-                cpp.append('        if (q["z"]) r.{0}.pose.orientation.z = q["z"].as<double>();'.format(name))
-                cpp.append('        if (q["w"]) r.{0}.pose.orientation.w = q["w"].as<double>();'.format(name))
-                cpp.append('      }')
-                cpp.append('    }')
-            elif bt in ("geometry_msgs/TwistStamped", "geometry_msgs/TwistStamped") and not is_array:
-                cpp.append('    // geometry_msgs::msg::TwistStamped')
-                cpp.append('    if (n) {')
-                cpp.append('      if (n["frame_id"]) r.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
-                cpp.append('      if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
-                cpp.append('      if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
-                cpp.append('      if (auto lin = n["linear"]) {')
-                cpp.append('        if (lin["x"]) r.{0}.twist.linear.x = lin["x"].as<double>();'.format(name))
-                cpp.append('        if (lin["y"]) r.{0}.twist.linear.y = lin["y"].as<double>();'.format(name))
-                cpp.append('        if (lin["z"]) r.{0}.twist.linear.z = lin["z"].as<double>();'.format(name))
-                cpp.append('      }')
-                cpp.append('      if (auto ang = n["angular"]) {')
-                cpp.append('        if (ang["x"]) r.{0}.twist.angular.x = ang["x"].as<double>();'.format(name))
-                cpp.append('        if (ang["y"]) r.{0}.twist.angular.y = ang["y"].as<double>();'.format(name))
-                cpp.append('        if (ang["z"]) r.{0}.twist.angular.z = ang["z"].as<double>();'.format(name))
-                cpp.append('      }')
-                cpp.append('    }')
-            else:
+            if not emit_complex_field(cpp, name, base_type, is_array, 'r', interfaces_root):
                 cpp.append(f'    // TODO: parse field `{name}` of ROS type `{ty}` into r.{name} (complex type)')
     cpp.append('  }')
     cpp.append('  return r;')
@@ -348,6 +445,8 @@ def main():
             kind = kind.lower()
             header_snake = to_snake(base)
             if kind.startswith('action'):
+                if base in added_actions:
+                    continue
                 includes.add(f'"interfaces/action/{header_snake}.hpp"')
                 # locate .action file in common subdirs
                 action_dir = resolve_interface_subdir(interfaces_root, 'action')
@@ -376,9 +475,12 @@ def main():
                     fields = []
                 else:
                     fields = parse_action_file(action_file)
-                converters.append(gen_converter_action(base, fields, header_snake))
+                add_msg_includes_for_fields(fields, includes)
+                converters.append(gen_converter_action(base, fields, header_snake, interfaces_root))
                 added_actions.add(base)
             elif kind.startswith('srv') or kind.startswith('service'):
+                if base in added_srvs:
+                    continue
                 includes.add(f'"interfaces/srv/{header_snake}.hpp"')
                 srv_dir = resolve_interface_subdir(interfaces_root, 'srv')
                 tried_paths = []
@@ -404,7 +506,8 @@ def main():
                     fields = []
                 else:
                     fields = parse_srv_file(srv_file)
-                converters.append(gen_converter_srv(base, fields, header_snake))
+                add_msg_includes_for_fields(fields, includes)
+                converters.append(gen_converter_srv(base, fields, header_snake, interfaces_root))
                 added_srvs.add(base)
             # end of iface loop
             pass
@@ -423,7 +526,8 @@ def main():
             header_snake = to_snake(base_type)
             includes.add(f'"interfaces/action/{header_snake}.hpp"')
             fields = parse_action_file(os.path.join(action_dir, fname))
-            converters.append(gen_converter_action(base_type, fields, header_snake))
+            add_msg_includes_for_fields(fields, includes)
+            converters.append(gen_converter_action(base_type, fields, header_snake, interfaces_root))
             added_actions.add(base_type)
 
     srv_dir = resolve_interface_subdir(interfaces_root, 'srv')
@@ -438,7 +542,8 @@ def main():
             header_snake = to_snake(base_type)
             includes.add(f'"interfaces/srv/{header_snake}.hpp"')
             fields = parse_srv_file(os.path.join(srv_dir, fname))
-            converters.append(gen_converter_srv(base_type, fields, header_snake))
+            add_msg_includes_for_fields(fields, includes)
+            converters.append(gen_converter_srv(base_type, fields, header_snake, interfaces_root))
             added_srvs.add(base_type)
 
     # assemble header

src/scripts/gen_skill_config.py

@@ -14,6 +14,7 @@ The script is intentionally conservative: it emits empty default_topic values; y
 """
 
 import os
+import re
 import sys
 import yaml
 from pathlib import Path
@@ -95,6 +96,7 @@ def parse_iface_item(item):
         return None
 
 for entry in skills:
+    skill_name = entry.get('name', '')
     if 'interfaces' not in entry:
         continue
     for iface in entry['interfaces']:
@@ -105,13 +107,25 @@ for entry in skills:
         if kind == 'action':
             if base not in skill_action_types:
                 skill_action_types.append(base)
+            # Prefer default_topic from a skill whose name matches the interface base,
+            # otherwise don't overwrite if we already have one from a prior matching or non-matching entry.
             if dtopic:
-                action_default_map[base] = dtopic
+                if base not in action_default_map or skill_name == base:
+                    action_default_map[base] = dtopic
         elif kind == 'srv':
             if base not in skill_service_types:
                 skill_service_types.append(base)
             if dtopic:
-                service_default_map[base] = dtopic
+                if base not in service_default_map or skill_name == base:
+                    service_default_map[base] = dtopic
+
+# Ensure all available interfaces are included so traits exist for any action/srv used in code
+for a in actions:
+    if a not in skill_action_types:
+        skill_action_types.append(a)
+for s in services:
+    if s not in skill_service_types:
+        skill_service_types.append(s)
 
 # Helper to map base name to include path / C++ type name.
 # Assumption: interfaces live in package `interfaces` and types are under `interfaces::action::Name` or `interfaces::srv::Name`.
@@ -123,15 +137,10 @@ def service_cpp_type(base):
     return f"interfaces::srv::{base}"
 
 def to_snake_case(s: str) -> str:
-    out = []
-    for i, ch in enumerate(s):
-        if ch.isupper():
-            if i != 0:
-                out.append('_')
-            out.append(ch.lower())
-        else:
-            out.append(ch)
-    return ''.join(out)
+    # Convert CamelCase (including initialisms like ASRRecognize) to snake_case
+    s1 = re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', s)
+    s2 = re.sub(r'([a-z0-9])([A-Z])', r'\1_\2', s1)
+    return s2.lower()
 
 
 def parse_action_result_fields(action_file: Path):
@@ -251,11 +260,11 @@ with OUT_HEADER.open('w') as out:
             result_fields = parse_action_result_fields(action_file_found) if action_file_found is not None else {}
             success_field, message_field, success_kind = pick_success_and_message_fields(result_fields)
             if success_kind == 'bool' and success_field is not None:
-                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r) {{return r.{success_field};}}\n')
+                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r, const std::string & instance_name) {{(void)instance_name; return r.{success_field};}}\n')
             elif success_kind == 'int_zero' and success_field is not None:
-                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r) {{return (r.{success_field} == 0);}}\n')
+                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r, const std::string & instance_name) {{(void)instance_name; return (r.{success_field} == 0);}}\n')
             else:
-                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r) {{(void)r; return true;}}\n')
+                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r, const std::string & instance_name) {{(void)instance_name; (void)r; return true;}}\n')
             if message_field is not None:
                 out.write(f'  static std::string message(const {action_cpp_type(a)}::Result & r) {{return r.{message_field};}}\n')
             else:

src/scripts/hand_eye_cali.py

@@ -1,588 +0,0 @@
-import numpy as np
-import cv2
-from typing import List, Tuple
-import argparse
-import json
-import os
-import glob
-import csv
-
-def hand_eye_calibration(robot_poses: List[np.ndarray],
-                         camera_poses: List[np.ndarray],
-                         mode: str = 'eye_in_hand') -> np.ndarray:
-    """
-    执行手眼标定，解决AX = XB问题。
-
-    参数:
-        robot_poses (list of np.array): 机械臂末端在基座坐标系中的位姿列表 (4x4齐次矩阵)。
-        camera_poses (list of np.array): 标定板在相机坐标系中的位姿列表 (4x4齐次矩阵)。注意：这是 T_c_t（target->camera），与OpenCV的 R_target2cam/t_target2cam 一致。
-        mode (str): 标定模式。'eye_in_hand' 或 'eye_to_hand'。
-
-    返回:
-        X (np.array): 求解出的变换矩阵X (4x4齐次矩阵)。
-                      对于 eye_in_hand: X = camera^T_end_effector (相机在机械臂末端坐标系中的位姿)
-                      对于 eye_to_hand: X = base^T_camera (相机在机器人基座坐标系中的位姿)
-    """
-    # 输入验证
-    n = len(robot_poses)
-    if n != len(camera_poses):
-        raise ValueError("机器人位姿数量与相机位姿数量必须相同。")
-    if n < 3:
-        raise ValueError("至少需要三组数据（且包含显著旋转）才能进行稳定标定。")
-
-    # OpenCV calibrateHandEye 需要每一帧的绝对位姿：
-    #  - R_gripper2base, t_gripper2base: T_g^b（夹爪到基座）
-    #  - R_target2cam,  t_target2cam:  T_t^c（标定板到相机）
-    # 传入时请确保 robot_poses 为 T_b^g（基座到夹爪），camera_poses 为 T_c^t（目标到相机）
-    R_gripper2base: List[np.ndarray] = []
-    t_gripper2base: List[np.ndarray] = []
-    R_target2cam: List[np.ndarray] = []
-    t_target2cam: List[np.ndarray] = []
-
-    for i in range(n):
-        T_b_g = robot_poses[i]
-        if T_b_g.shape != (4, 4):
-            raise ValueError("robot_poses 中的矩阵必须是 4x4")
-        T_g_b = np.linalg.inv(T_b_g)
-        R_gripper2base.append(T_g_b[:3, :3])
-        t_gripper2base.append(T_g_b[:3, 3].reshape(3, 1))
-
-        T_c_t = camera_poses[i]
-        if T_c_t.shape != (4, 4):
-            raise ValueError("camera_poses 中的矩阵必须是 4x4")
-        R_target2cam.append(T_c_t[:3, :3])
-        t_target2cam.append(T_c_t[:3, 3].reshape(3, 1))
-
-    # 选择算法，失败时自动尝试其他方法
-    methods = [
-        getattr(cv2, 'CALIB_HAND_EYE_TSAI', None),
-        getattr(cv2, 'CALIB_HAND_EYE_PARK', None),
-        getattr(cv2, 'CALIB_HAND_EYE_HORAUD', None),
-        getattr(cv2, 'CALIB_HAND_EYE_ANDREFF', None),
-        getattr(cv2, 'CALIB_HAND_EYE_DANIILIDIS', None),
-    ]
-    methods = [m for m in methods if m is not None]
-
-    last_err = None
-    for method in methods:
-        try:
-            R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
-                R_gripper2base, t_gripper2base,
-                R_target2cam, t_target2cam,
-                method=method,
-            )
-            # 成功返回
-            X_cam_gripper = np.eye(4)
-            X_cam_gripper[:3, :3] = R_cam2gripper
-            X_cam_gripper[:3, 3] = t_cam2gripper.flatten()
-
-            if mode == 'eye_in_hand':
-                # 直接返回 camera^T_gripper
-                return X_cam_gripper
-            else:
-                # eye_to_hand：需要 base^T_camera。已知：
-                #  对每一帧 i，有 T_b^c_i = T_b^g_i * T_g^c（其中 T_g^c = (T_c^g)^{-1}）
-                T_g_c = np.linalg.inv(X_cam_gripper)
-                T_b_c_list = []
-                for T_b_g in robot_poses:
-                    T_b_c_list.append(T_b_g @ T_g_c)
-                return average_SE3(T_b_c_list)
-        except Exception as e:
-            last_err = e
-            continue
-
-    raise RuntimeError(f"hand-eye 标定失败：{last_err}")
-
-def create_homogeneous_matrix(rvec, tvec):
-    """
-    根据旋转向量和平移向量创建4x4齐次变换矩阵。
-
-    参数:
-        rvec: 3x1旋转向量
-        tvec: 3x1平移向量
-
-    返回:
-        T: 4x4齐次变换矩阵
-    """
-    R, _ = cv2.Rodrigues(rvec)
-    T = np.eye(4)
-    T[:3, :3] = R
-    T[:3, 3] = tvec.flatten()
-    return T
-
-
-def rot_to_quat(R: np.ndarray) -> np.ndarray:
-    """将旋转矩阵转为四元数 [x, y, z, w]（右手，单位四元数）。"""
-    tr = np.trace(R)
-    if tr > 0.0:
-        S = np.sqrt(tr + 1.0) * 2.0
-        qw = 0.25 * S
-        qx = (R[2, 1] - R[1, 2]) / S
-        qy = (R[0, 2] - R[2, 0]) / S
-        qz = (R[1, 0] - R[0, 1]) / S
-    else:
-        if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
-            S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
-            qx = 0.25 * S
-            qy = (R[0, 1] + R[1, 0]) / S
-            qz = (R[0, 2] + R[2, 0]) / S
-            qw = (R[2, 1] - R[1, 2]) / S
-        elif R[1, 1] > R[2, 2]:
-            S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
-            qx = (R[0, 1] + R[1, 0]) / S
-            qy = 0.25 * S
-            qz = (R[1, 2] + R[2, 1]) / S
-            qw = (R[0, 2] - R[2, 0]) / S
-        else:
-            S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
-            qx = (R[0, 2] + R[2, 0]) / S
-            qy = (R[1, 2] + R[2, 1]) / S
-            qz = 0.25 * S
-            qw = (R[1, 0] - R[0, 1]) / S
-    q = np.array([qx, qy, qz, qw])
-    return q / (np.linalg.norm(q) + 1e-12)
-
-
-def quat_to_rot(q: np.ndarray) -> np.ndarray:
-    """四元数 [x, y, z, w] 转旋转矩阵。"""
-    x, y, z, w = q
-    xx, yy, zz = x*x, y*y, z*z
-    xy, xz, yz = x*y, x*z, y*z
-    wx, wy, wz = w*x, w*y, w*z
-    R = np.array([
-        [1 - 2*(yy + zz), 2*(xy - wz),     2*(xz + wy)],
-        [2*(xy + wz),     1 - 2*(xx + zz), 2*(yz - wx)],
-        [2*(xz - wy),     2*(yz + wx),     1 - 2*(xx + yy)]
-    ])
-    return R
-
-
-def average_SE3(T_list: List[np.ndarray]) -> np.ndarray:
-    """对一组 SE(3) 变换做简单平均（四元数+平移平均）。"""
-    if not T_list:
-        raise ValueError("T_list 为空")
-    # 平移均值
-    t_stack = np.stack([T[:3, 3] for T in T_list], axis=0)
-    t_mean = np.mean(t_stack, axis=0)
-    # 旋转均值（单位四元数平均后归一化）
-    q_list = [rot_to_quat(T[:3, :3]) for T in T_list]
-    # 对齐四元数符号到第一个四元数的半球，避免相互抵消
-    q0 = q_list[0]
-    aligned = []
-    for q in q_list:
-        if np.dot(q, q0) < 0:
-            aligned.append(-q)
-        else:
-            aligned.append(q)
-    q = np.mean(np.stack(aligned, axis=0), axis=0)
-    q = q / (np.linalg.norm(q) + 1e-12)
-    R_mean = quat_to_rot(q)
-    T = np.eye(4)
-    T[:3, :3] = R_mean
-    T[:3, 3] = t_mean
-    return T
-
-
-def hand_eye_residual(robot_poses: List[np.ndarray], camera_poses: List[np.ndarray], X_cam_gripper: np.ndarray) -> Tuple[float, float]:
-    """基于 AX ≈ X B 的关系计算残差（旋转角度均值，平移均值）。
-    返回 (rot_err_deg, trans_err_m)。"""
-    rot_errs = []
-    trans_errs = []
-    n = len(robot_poses)
-    # 构造相对运动对，避免偏置
-    for i in range(n - 1):
-        A = np.linalg.inv(robot_poses[i + 1]) @ robot_poses[i]  # T_b^g_{i+1}^{-1} * T_b^g_i
-        B = np.linalg.inv(camera_poses[i + 1]) @ camera_poses[i]  # T_c^t_{i+1}^{-1} * T_c^t_i
-        # OpenCV定义 X 为 T_c^g（camera->gripper）
-        lhs = np.linalg.inv(A)  # OpenCV论文中常用 A 为 g2b，但这里用的相对位姿定义略有差异
-        # 直接用 AX 与 XB 的等价残差度量
-        AX = A @ np.linalg.inv(X_cam_gripper)
-        XB = np.linalg.inv(X_cam_gripper) @ B
-        Delta = np.linalg.inv(AX) @ XB
-        R = Delta[:3, :3]
-        t = Delta[:3, 3]
-        # 旋转角度
-        angle = np.rad2deg(np.arccos(np.clip((np.trace(R) - 1) / 2.0, -1.0, 1.0)))
-        rot_errs.append(abs(angle))
-        trans_errs.append(np.linalg.norm(t))
-    return float(np.mean(rot_errs)), float(np.mean(trans_errs))
-
-# ==================== 示例用法 ====================
-
-def generate_synthetic_dataset(num_poses: int = 12, seed: int = 42) -> Tuple[List[np.ndarray], List[np.ndarray]]:
-    """生成可解的模拟数据集 (T_b^g 列表, T_c^t 列表)。"""
-    rng = np.random.default_rng(seed)
-
-    def hat(v):
-        return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
-
-    def exp_so3(w):
-        th = np.linalg.norm(w)
-        if th < 1e-12:
-            return np.eye(3)
-        k = w / th
-        K = hat(k)
-        return np.eye(3) + np.sin(th) * K + (1 - np.cos(th)) * (K @ K)
-
-    def make_T(R, t):
-        T = np.eye(4)
-        T[:3, :3] = R
-        T[:3, 3] = t
-        return T
-
-    # 真实（未知）相机在夹爪中的位姿：T_c^g
-    R_c_g = exp_so3(np.deg2rad([10, -5, 15]))
-    t_c_g = np.array([0.03, -0.02, 0.15])
-    T_c_g_true = make_T(R_c_g, t_c_g)
-
-    # 世界（=基座）到标定板的固定位姿：T_b^t
-    R_b_t = exp_so3(np.deg2rad([0, 0, 0]))
-    t_b_t = np.array([0.4, 0.0, 0.3])
-    T_b_t = make_T(R_b_t, t_b_t)
-
-    robot_poses: List[np.ndarray] = []  # T_b^g
-    camera_poses: List[np.ndarray] = [] # T_c^t
-
-    for _ in range(num_poses):
-        # 生成多轴、较大幅度的运动（避免退化）
-        ang = np.deg2rad(rng.uniform([-30, -30, -30], [30, 30, 30]))
-        # 让 SO(3) 采样更丰富：
-        R_b_g = (np.eye(3) @ rot_perturb(ang))
-        t_b_g = rng.uniform([-0.2, -0.2, 0.3], [0.2, 0.2, 0.6])
-        T_b_g = make_T(R_b_g, t_b_g)
-        robot_poses.append(T_b_g)
-
-        # 相机位姿：T_b^c = T_b^g * T_g^c，其中 T_g^c = (T_c^g)^{-1}
-        T_g_c = np.linalg.inv(T_c_g_true)
-        T_b_c = T_b_g @ T_g_c
-
-        # 目标在相机坐标系：T_c^t = (T_b^c)^{-1} * T_b^t
-        T_c_t = np.linalg.inv(T_b_c) @ T_b_t
-        camera_poses.append(T_c_t)
-
-    return robot_poses, camera_poses
-
-
-def rot_perturb(ang_vec: np.ndarray) -> np.ndarray:
-    """给定欧拉角近似（rad）的向量，输出旋转矩阵（用罗德里格公式按轴角）。"""
-    axis = ang_vec
-    th = np.linalg.norm(axis)
-    if th < 1e-12:
-        return np.eye(3)
-    k = axis / th
-    K = np.array([[0, -k[2], k[1]], [k[2], 0, -k[0]], [-k[1], k[0], 0]])
-    return np.eye(3) + np.sin(th) * K + (1 - np.cos(th)) * (K @ K)
-
-
-def to_json_SE3(T: np.ndarray) -> dict:
-    R = T[:3, :3]
-    t = T[:3, 3]
-    q = rot_to_quat(R)
-    return {
-        "matrix": T.tolist(),
-        "rotation_matrix": R.tolist(),
-        "translation": t.tolist(),
-        "quaternion_xyzw": q.tolist(),
-    }
-
-
-def load_se3_matrix(path: str) -> np.ndarray:
-    """加载 4x4 齐次矩阵。支持 .npy / .npz(json-like)/.json。"""
-    if not os.path.exists(path):
-        raise FileNotFoundError(path)
-    ext = os.path.splitext(path)[1].lower()
-    if ext == ".npy":
-        M = np.load(path)
-    elif ext == ".npz":
-        d = np.load(path)
-        key = 'matrix' if 'matrix' in d.files else d.files[0]
-        M = d[key]
-    elif ext == ".json":
-        with open(path, 'r') as f:
-            data = json.load(f)
-        M = np.array(data.get('matrix', data))
-    else:
-        raise ValueError(f"不支持的文件类型: {ext}")
-    M = np.array(M)
-    if M.shape != (4, 4):
-        raise ValueError(f"矩阵形状应为(4,4)，实际为{M.shape}")
-    return M
-
-
-def load_camera_calib(path: str) -> Tuple[np.ndarray, np.ndarray]:
-    """加载相机内参，支持 OpenCV YAML/JSON（cameraMatrix, distCoeffs）或简单 JSON：{"K":[], "D":[]}。"""
-    import yaml
-    with open(path, 'r') as f:
-        txt = f.read()
-    data = None
-    try:
-        data = yaml.safe_load(txt)
-    except Exception:
-        try:
-            data = json.loads(txt)
-        except Exception:
-            raise ValueError("无法解析相机标定文件，支持 YAML/JSON，包含 camera_matrix/cameraMatrix/K 和 dist_coeffs/distCoeffs/D 字段")
-
-    # 支持多种键名
-    K = data.get('camera_matrix') or data.get('cameraMatrix') or data.get('K')
-    if isinstance(K, dict) and 'data' in K:
-        K = K['data']
-    D = data.get('dist_coeffs') or data.get('distCoeffs') or data.get('D')
-    if isinstance(D, dict) and 'data' in D:
-        D = D['data']
-    if K is None or D is None:
-        raise ValueError("标定文件需包含 camera_matrix/cameraMatrix/K 与 dist_coeffs/distCoeffs/D")
-    K = np.array(K, dtype=float).reshape(3, 3)
-    D = np.array(D, dtype=float).reshape(-1, 1)
-    return K, D
-
-
-def build_chessboard_object_points(rows: int, cols: int, square: float) -> np.ndarray:
-    """生成棋盘格 3D 角点（单位：米），Z=0 平面，原点在棋盘左上角角点。"""
-    objp = np.zeros((rows * cols, 3), np.float32)
-    grid = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
-    objp[:, :2] = grid * square
-    return objp
-
-
-def detect_chessboard(gray: np.ndarray, rows: int, cols: int) -> Tuple[bool, np.ndarray]:
-    flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
-    ok, corners = cv2.findChessboardCorners(gray, (cols, rows), flags)
-    if not ok:
-        return False, None
-    # 亚像素优化
-    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
-    corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
-    return True, corners.reshape(-1, 2)
-
-
-def detect_charuco(gray: np.ndarray, rows: int, cols: int, square: float, marker: float, dict_name: str = 'DICT_4X4_50') -> Tuple[bool, np.ndarray, np.ndarray]:
-    """检测 Charuco 角点，返回 (ok, charuco_corners, charuco_ids)。需要 opencv-contrib-python。"""
-    if not hasattr(cv2, 'aruco'):
-        raise RuntimeError("需要 opencv-contrib-python 才能使用 Charuco。请安装 opencv-contrib-python==4.11.0.86")
-    aruco = cv2.aruco
-    dictionary = getattr(aruco, dict_name)
-    aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, dict_name))
-    board = aruco.CharucoBoard((cols, rows), square, marker, aruco_dict)
-    params = aruco.DetectorParameters()
-    detector = aruco.ArucoDetector(aruco_dict, params)
-    corners, ids, _ = detector.detectMarkers(gray)
-    if ids is None or len(ids) == 0:
-        return False, None, None
-    retval, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
-    ok = retval is not None and retval >= 4
-    return ok, (charuco_corners.reshape(-1, 2) if ok else None), (charuco_ids if ok else None)
-
-
-def solve_pnp_from_image(image_path: str, K: np.ndarray, D: np.ndarray,
-                         pattern: str, rows: int, cols: int, square: float,
-                         charuco_marker: float = None) -> np.ndarray:
-    img = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
-    if img is None:
-        raise FileNotFoundError(f"无法读取图像: {image_path}")
-    if pattern == 'chessboard':
-        ok, corners = detect_chessboard(img, rows, cols)
-        if not ok:
-            raise RuntimeError(f"棋盘检测失败: {image_path}")
-        objp = build_chessboard_object_points(rows, cols, square)
-        # PnP
-        retval, rvec, tvec = cv2.solvePnP(objp, corners, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
-        if not retval:
-            raise RuntimeError(f"solvePnP 失败: {image_path}")
-        return create_homogeneous_matrix(rvec, tvec)
-    elif pattern == 'charuco':
-        if charuco_marker is None:
-            raise ValueError("使用 charuco 时必须提供 --charuco-marker（米）")
-        ok, charuco_corners, charuco_ids = detect_charuco(img, rows, cols, square, charuco_marker)
-        if not ok:
-            raise RuntimeError(f"Charuco 检测失败: {image_path}")
-        # 从 board 索引构建 3D 角点
-        aruco = cv2.aruco
-        aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, 'DICT_4X4_50'))
-        board = aruco.CharucoBoard((cols, rows), square, charuco_marker, aruco_dict)
-        obj_pts = []
-        img_pts = []
-        for idx, corner in zip(charuco_ids.flatten(), charuco_corners.reshape(-1, 2)):
-            obj_pts.append(board.getChessboardCorners()[idx][0])
-            img_pts.append(corner)
-        obj_pts = np.array(obj_pts, dtype=np.float32)
-        img_pts = np.array(img_pts, dtype=np.float32)
-        retval, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
-        if not retval:
-            raise RuntimeError(f"solvePnP 失败: {image_path}")
-        return create_homogeneous_matrix(rvec, tvec)
-    else:
-        raise ValueError("pattern 仅支持 chessboard 或 charuco")
-
-
-def load_robot_poses_file(path: str) -> List[np.ndarray]:
-    """从文件加载 robot_poses 列表。支持 .npz(robot_poses), .json(list or dict.matrix), .csv( image, tx,ty,tz,qx,qy,qz,qw )。"""
-    ext = os.path.splitext(path)[1].lower()
-    if ext == '.npz':
-        d = np.load(path)
-        arr = d['robot_poses']
-        assert arr.ndim == 3 and arr.shape[1:] == (4, 4)
-        return [arr[i] for i in range(arr.shape[0])]
-    if ext == '.json':
-        with open(path, 'r') as f:
-            data = json.load(f)
-        if isinstance(data, dict) and 'robot_poses' in data:
-            mats = data['robot_poses']
-        else:
-            mats = data
-        return [np.array(M).reshape(4, 4) for M in mats]
-    if ext == '.csv':
-        poses = []
-        with open(path, 'r') as f:
-            reader = csv.DictReader(f)
-            for row in reader:
-                tx, ty, tz = float(row['tx']), float(row['ty']), float(row['tz'])
-                qx, qy, qz, qw = float(row['qx']), float(row['qy']), float(row['qz']), float(row['qw'])
-                R = quat_to_rot(np.array([qx, qy, qz, qw]))
-                T = np.eye(4)
-                T[:3, :3] = R
-                T[:3, 3] = np.array([tx, ty, tz])
-                poses.append(T)
-        return poses
-    raise ValueError(f"不支持的 robot poses 文件类型: {ext}")
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser(description="Hand-Eye calibration (eye-in-hand / eye-to-hand)")
-    parser.add_argument("--data", type=str, default=None,
-                        help=".npz dataset with arrays 'robot_poses' and 'camera_poses' of shape (N,4,4)")
-    parser.add_argument("--mode", type=str, default=None, choices=["eye_in_hand", "eye_to_hand"],
-                        help="Calibration mode. If omitted, both modes are computed")
-    parser.add_argument("--out", type=str, default=None, help="Output JSON file for the calibration result")
-    parser.add_argument("--export-dataset", type=str, default=None,
-                        help="Export the dataset used (robot_poses,camera_poses) to .npz at this path")
-    parser.add_argument("--seed", type=int, default=42, help="Synthetic dataset RNG seed when no --data is provided")
-    parser.add_argument("--num-poses", type=int, default=12, help="Synthetic dataset pose count when no --data is provided")
-    parser.add_argument("--eval", action="store_true", help="Compute simple AX≈XB residual metrics (eye-in-hand)")
-    # Image-based camera pose estimation
-    parser.add_argument("--images", type=str, default=None,
-                        help="Directory or glob for images captured by the depth camera (color/IR). Sorted alphanumerically.")
-    parser.add_argument("--camera-calib", type=str, default=None, help="Camera intrinsic file (YAML/JSON)")
-    parser.add_argument("--pattern", type=str, default="chessboard", choices=["chessboard", "charuco"], help="Calibration board pattern")
-    parser.add_argument("--board-rows", type=int, default=6, help="Inner rows of chessboard/charuco")
-    parser.add_argument("--board-cols", type=int, default=9, help="Inner cols of chessboard/charuco")
-    parser.add_argument("--square-size", type=float, default=0.024, help="Square size in meters")
-    parser.add_argument("--charuco-marker", type=float, default=None, help="Charuco marker size in meters (for charuco only)")
-    parser.add_argument("--robot-poses-file", type=str, default=None, help="Path to robot poses file (.npz/.json/.csv)")
-    # Frames handling
-    parser.add_argument("--robot-poses-frame", type=str, default="robot_base", choices=["robot_base", "arm_base"],
-                        help="Frame of robot_poses in the dataset: robot_base (default) or arm_base")
-    parser.add_argument("--arm-to-robot-base", type=str, default=None,
-                        help="Optional path to 4x4 matrix of T_robot_base^arm_base (compose robot poses to robot base)")
-    args = parser.parse_args()
-
-    # Load or generate dataset
-    if args.data:
-        d = np.load(args.data)
-        robot_arr = d["robot_poses"]
-        cam_arr = d["camera_poses"]
-        assert robot_arr.ndim == 3 and robot_arr.shape[1:] == (4, 4)
-        assert cam_arr.ndim == 3 and cam_arr.shape[1:] == (4, 4)
-        robot_poses = [robot_arr[i] for i in range(robot_arr.shape[0])]
-        camera_poses = [cam_arr[i] for i in range(cam_arr.shape[0])]
-        print(f"已从 {args.data} 加载数据集，共 {len(robot_poses)} 组")
-    elif args.images and args.robot_poses_file and args.camera_calib:
-        # Build dataset from images + robot poses
-        # 1) images
-        if os.path.isdir(args.images):
-            img_paths = sorted(glob.glob(os.path.join(args.images, '*')))
-        else:
-            img_paths = sorted(glob.glob(args.images))
-        if not img_paths:
-            raise FileNotFoundError(f"未找到图像: {args.images}")
-        # 2) robot poses
-        robot_poses = load_robot_poses_file(args.robot_poses_file)
-        if len(robot_poses) != len(img_paths):
-            raise ValueError(f"robot poses 数量({len(robot_poses)})与图像数量({len(img_paths)})不一致。请确保一一对应且顺序匹配。")
-        # 3) camera intrinsics
-        K, D = load_camera_calib(args.camera_calib)
-        # 4) detect and solvePnP
-        camera_poses = []
-        for img in img_paths:
-            T_c_t = solve_pnp_from_image(img, K, D, args.pattern, args.board_rows, args.board_cols, args.square_size, args.charuco_marker)
-            camera_poses.append(T_c_t)
-        print(f"已从图像与位姿构建数据集，共 {len(camera_poses)} 组")
-    else:
-        robot_poses, camera_poses = generate_synthetic_dataset(num_poses=args.num_poses, seed=args.seed)
-        print(f"生成了 {len(robot_poses)} 组模拟位姿数据 (seed={args.seed})")
-
-    # Optional export of dataset for reproducibility
-    if args.export_dataset:
-        np.savez(args.export_dataset,
-                 robot_poses=np.stack(robot_poses, axis=0),
-                 camera_poses=np.stack(camera_poses, axis=0))
-        print(f"已导出数据集到: {args.export_dataset}")
-
-    # Frame composition: if robot_poses are in arm_base, optionally compose to robot_base
-    reference_base = "robot_base"
-    if args.robot_poses_frame == "arm_base":
-        reference_base = "arm_base"
-        if args.arm_to_robot_base:
-            T_rb_ab = load_se3_matrix(args.arm_to_robot_base)  # T_robot_base^arm_base
-            robot_poses = [T_rb_ab @ T_ab_g for T_ab_g in robot_poses]
-            reference_base = "robot_base"
-            print("已将 arm_base 框架下的末端位姿转换到 robot_base 框架")
-
-    def run_mode(mode_name: str) -> np.ndarray:
-        print(f"\n=== 标定模式: {mode_name} ===")
-        X = hand_eye_calibration(robot_poses, camera_poses, mode=mode_name)
-        print("标定结果 X:")
-        print(X)
-        if mode_name == 'eye_to_hand':
-            print(f"参考基座: {reference_base}")
-        print(f"平移部分: [{X[0, 3]:.4f}, {X[1, 3]:.4f}, {X[2, 3]:.4f}]")
-        return X
-
-    results = {}
-    try:
-        if args.mode:
-            X = run_mode(args.mode)
-            results[args.mode] = X
-        else:
-            X_eih = run_mode('eye_in_hand')
-            X_eth = run_mode('eye_to_hand')
-            results['eye_in_hand'] = X_eih
-            results['eye_to_hand'] = X_eth
-
-        if args.eval:
-            # 仅计算 eye-in-hand 残差（需要 X_cam_gripper），若未跑则先跑一次
-            if 'eye_in_hand' in results:
-                X_cam_gripper = results['eye_in_hand']
-            else:
-                X_cam_gripper = hand_eye_calibration(robot_poses, camera_poses, mode='eye_in_hand')
-            rot_err_deg, trans_err = hand_eye_residual(robot_poses, camera_poses, X_cam_gripper)
-            print(f"残差评估：旋转均值 {rot_err_deg:.3f} 度，平移均值 {trans_err:.4f} m")
-
-        if args.out:
-            # 若选择了特定模式，写该模式；否则写两者
-            payload = {"metadata": {"reference_base": reference_base,
-                                     "robot_poses_frame": args.robot_poses_frame,
-                                     "composed_with_arm_to_robot_base": bool(args.arm_to_robot_base)}}
-            if args.mode:
-                payload[args.mode] = to_json_SE3(results[args.mode])
-            else:
-                payload.update({k: to_json_SE3(v) for k, v in results.items()})
-            with open(args.out, 'w') as f:
-                json.dump(payload, f, indent=2)
-            print(f"已保存标定结果到: {args.out}")
-
-    except Exception as e:
-        print(f"标定过程中发生错误: {e}")
-
-    # ==================== 实际应用提示 ====================
-    if not args.data:
-        print("\n=== 实际应用说明 ===")
-        print("1. 从机器人控制器读取末端位姿 (通常是4x4齐次矩阵或位姿向量，注意这是 基座->末端 T_b^g)")
-        print("2. 使用 OpenCV solvePnP 从标定板图像解算 T_c^t (标定板到相机):")
-        print("   retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)")
-        print("   camera_pose = create_homogeneous_matrix(rvec, tvec)  # 即 T_c^t")
-        print("3. 收集多组对应的 T_b^g 和 T_c^t（包含多轴较大旋转，>= 8-12 组）")
-        print("4. 运行本脚本 --data dataset.npz --mode eye_to_hand --out result.json --eval")
-        print("\n若 robot_poses 在 arm_base 框架下：")
-        print("- 如无 arm_base->robot_base 变换，脚本将输出 arm_base^T_camera（eye_to_hand）")
-        print("- 若提供 --arm-to-robot-base T_robot_base_from_arm_base.npy，将自动转换并输出 robot_base^T_camera")

src/scripts/hand_eye_calibration.md

@@ -1,224 +0,0 @@
-# 手眼标定（Eye-in-Hand / Eye-to-Hand）操作指南
-
-本指南配合脚本 `src/scripts/hand_eye_cali.py` 使用，完成相机与机器人之间的外参（手眼）标定。脚本基于 OpenCV 的 `calibrateHandEye` 实现，支持“眼在手上 (eye-in-hand)”与“眼在手外 (eye-to-hand)”两种模式。
-
----
-
-## 一、原理与坐标定义
-
-- 机器人末端在基座坐标系下的位姿：`T_b^g`（4x4 齐次矩阵）
-- 标定板在相机坐标系下的位姿：`T_c^t`（4x4 齐次矩阵），可由 `solvePnP` 得到
-- 需求目标：
-  - 眼在手上 (eye-in-hand)：求 `T_c^g`（相机在末端坐标系下的位姿）
-  - 眼在手外 (eye-to-hand)：求 `T_b^c`（相机在基座坐标系下的位姿）
-
-OpenCV `calibrateHandEye` 的输入为每一帧的“绝对位姿”：
-- `R_gripper2base, t_gripper2base` 对应 `T_g^b`（由 `T_b^g` 求逆获得）
-- `R_target2cam,  t_target2cam` 对应 `T_t^c`（`T_c^t` 的旋转和平移部分）
-
----
-
-## 二、前置条件
-
-- 已有相机的内参（fx, fy, cx, cy, 畸变等），并能在图像中稳定检测到标定板角点或 AprilTag/ArUco 标记
-- 能够从机器人系统读取末端在基座下的位姿 `T_b^g`
-- 至少采集 8–12 组数据，包含多轴、较大幅度旋转和平移（信息量不足会导致求解失败或误差大）
-
----
-
-## 三、数据采集
-
-1. 固定标定板（推荐刚性固定在稳定位置），或在“眼在手上”场景中保持相机跟随末端运动
-2. 对每一帧：
-   - 记录机器人末端位姿 `T_b^g`（4x4）
-   - 在对应图像中用 `solvePnP` 算法求解 `T_c^t`（4x4）
-3. 采集方式一：直接保存数据集 `.npz`
-   - 数组名必须是 `robot_poses` 和 `camera_poses`
-   - 形状为 `(N, 4, 4)`
-
-数据格式示例（Python 生成）：
-```python
-np.savez('dataset.npz',
-         robot_poses=np.stack(robot_pose_list, axis=0),
-         camera_poses=np.stack(camera_pose_list, axis=0))
-```
-
-4. 采集方式二：仅保存图像与机器人位姿
-   - 保存 N 张图像（彩色或红外），用于检测棋盘/Charuco 角点
-   - 同时保存 N 条机器人末端位姿 `T_b^g`（或 `T_ab^g`）到 `.csv/.json/.npz`
-   - 运行时脚本会：
-     1) 从相机内参文件（YAML/JSON）读入 `K,D`
-     2) 对每张图像用 `solvePnP` 求解 `T_c^t`
-     3) 组合成数据集并求解手眼外参
-
-> 仅有机械臂基座（arm_base）的位姿可以吗？可以。
-> - 如果记录的是 `T_ab^g`（arm_base->末端），也能完成手眼标定。
-> - `eye_to_hand` 模式下，脚本将输出 `arm_base^T_camera`。若 `T_rb^ab`（robot_base->arm_base），可传给脚本自动合成 `robot_base^T_camera`。
-
-
-## 四、脚本使用方法
-
-脚本路径：`src/scripts/hand_eye_cali.py`
-
-- 计算两种模式并导出 JSON：
-```bash
-python src/scripts/hand_eye_cali.py \
-  --data dataset.npz \
-  --out handeye_result.json \
-  --eval
-```
-
-- 仅计算眼在手外 (eye-to-hand)：
-```bash
-python src/scripts/hand_eye_cali.py \
-  --data dataset.npz \
-  --mode eye_to_hand \
-  --out handeye_result.json
-```
-
-- 没有数据时，可生成模拟数据用于测试：
-```bash
-python src/scripts/hand_eye_cali.py \
-  --num-poses 12 --eval \
-  --out /tmp/handeye_result.json \
-  --export-dataset /tmp/handeye_dataset.npz
-```
-
-### 仅用图像 + 末端位姿进行标定
-
-前提：准备好相机标定文件（YAML/JSON，包含 `cameraMatrix`/`camera_matrix`/`K` 和 `distCoeffs`/`dist_coeffs`/`D`）。
-
-- 棋盘格（默认 6x9 内角点，格长 0.024 m）：
-```bash
-python src/scripts/hand_eye_cali.py \
-  --images data/images/*.png \
-  --camera-calib data/camera.yaml \
-  --robot-poses-file data/robot_poses.csv \
-  --pattern chessboard --board-rows 6 --board-cols 9 --square-size 0.024 \
-  --robot-poses-frame arm_base \
-  --mode eye_to_hand \
-  --out handeye_result.json --eval
-```
-
-- Charuco（需要 opencv-contrib-python）：
-```bash
-python src/scripts/hand_eye_cali.py \
-  --images data/images/*.png \
-  --camera-calib data/camera.yaml \
-  --robot-poses-file data/robot_poses.csv \
-  --pattern charuco --board-rows 6 --board-cols 9 --square-size 0.024 --charuco-marker 0.016 \
-  --robot-poses-frame robot_base \
-  --mode eye_to_hand \
-  --out handeye_result.json --eval
-```
-
-robot_poses.csv 示例（头部为列名，按行对齐图像顺序）：
-```csv
-image,tx,ty,tz,qx,qy,qz,qw
-0001.png,0.10,0.00,0.45,0.00,0.00,0.00,1.00
-0002.png,0.10,0.05,0.45,0.05,0.00,0.00,0.9987
-...
-```
-
-相机 YAML 示例（可兼容 OpenCV 格式）：
-```yaml
-camera_matrix: {data: [fx, 0, cx, 0, fy, cy, 0, 0, 1]}
-dist_coeffs: {data: [k1, k2, p1, p2, k3]}
-```
-
-### 参数说明
-- `--data`: 读取 `.npz` 数据集（包含 `robot_poses`、`camera_poses`）
-- `--mode`: `eye_in_hand` 或 `eye_to_hand`（缺省则两者都算）
-- `--out`: 输出 JSON 路径（包含 4x4 矩阵、旋转矩阵、平移、四元数）
-- `--eval`: 计算 AX≈XB 的简单残差（眼在手上），用于快速自检
-- `--export-dataset`: 将当前使用的数据集导出（便于复现）
-- `--num-poses`, `--seed`: 生成模拟数据的数量与种子（无 `--data` 时生效）
-- `--robot-poses-frame`: `robot_base`（默认）或 `arm_base`，用于指明 `robot_poses` 的基座框架
-- `--arm-to-robot-base`: `T_robot_base^arm_base` 的 4x4 矩阵文件路径（.npy/.npz/.json），若提供则在内部进行 `T_b^g = T_rb^ab @ T_ab^g` 组合
--. `--images`: 图像目录或通配（按文件名排序）
--. `--camera-calib`: 相机内参文件（YAML/JSON）
--. `--pattern`: `chessboard` 或 `charuco`
--. `--board-rows/--board-cols/--square-size`: 标定板参数（单位：米）
--. `--charuco-marker`: Charuco 的方块内 marker 尺寸（米）
--. `--robot-poses-file`: 末端位姿文件（.csv/.json/.npz），与图像一一对应
-
----
-
-## 五、结果解释与落地
-
-- 脚本输出 JSON 中字段：
-  - `matrix`: 4x4 齐次矩阵（行主序）
-  - `rotation_matrix`: 3x3 旋转矩阵
-  - `translation`: 3 维平移 `[x, y, z]`（单位：米）
-  - `quaternion_xyzw`: 四元数 `[x, y, z, w]`
-
-- 眼在手外结果：
-  - 若 `--robot-poses-frame robot_base` 或提供了 `--arm-to-robot-base`，则输出为 `robot_base^T_camera`
-  - 若 `--robot-poses-frame arm_base` 且未提供 arm->robot 变换，则输出为 `arm_base^T_camera`
-  - 发布 TF 示例：
-```bash
-ros2 run tf2_ros static_transform_publisher \
-  x y z qx qy qz qw \
-  base_link camera_link
-```
-将 `x y z qx qy qz qw` 替换为 JSON 中的 `translation` 与 `quaternion_xyzw`，帧名根据机器人而定（如 `base_link` 与 `camera_link`）。
-
-- 眼在手上（`camera^T_end_effector`）则在末端坐标系下描述相机位姿，常用于抓取/视觉伺服求解。
-
----
-
-## 六、常见问题与排查
-
-- 报错“Not enough informative motions”或残差很大：
-  - 增加数据数量（>=12）
-  - 扩大旋转幅度，覆盖多个轴，避免纯平移或单轴小角度
-  - 确认 `T_b^g` 与 `T_c^t` 的定义方向正确（基座->末端、标定板->相机）
-- `solvePnP` 不稳定/跳变：
-  - 使用更鲁棒的标定板（AprilTag/Charuco）
-  - 固定/稳定曝光，提升角点检测质量
-  - 确认相机内参/畸变准确
-- 结果帧名/方向不符合期望：
-  - 仔细对照：脚本的 `eye_to_hand` 输出的是 `base^T_camera`，`eye_in_hand` 输出的是 `camera^T_end_effector`
-  - 若需要 `end_effector^T_camera`，取逆即可
-
----
-
-## 七、建议采集策略
-
-- 首先让末端在 3 个轴上分别做正/反方向旋转，各配合一定平移
-- 保证每一帧采集时 `T_b^g` 与 `T_c^t` 时间匹配（尽量同步）
-- 目标板尽可能占据成像区域较大比例，避免深远距离下的姿态估计不稳定
-
----
-
-## 八、附录：数据制作参考
-
-使用 `solvePnP` 生成 `T_c^t`：
-```python
-# objectPoints: (N,3) mm 或 m；imagePoints: (N,2) 像素
-retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)
-T_c_t = create_homogeneous_matrix(rvec, tvec)
-```
-
-保存数据集：
-```python
-np.savez('dataset.npz',
-         robot_poses=np.stack(robot_pose_list, axis=0),
-         camera_poses=np.stack(camera_pose_list, axis=0))
-```
-
-若 `robot_poses` 是以机械臂基座 `arm_base` 为基准，且已知 `T_robot_base^arm_base`，可以在运行时提供：
-```bash
-python src/scripts/hand_eye_cali.py \
-  --data dataset.npz \
-  --robot-poses-frame arm_base \
-  --arm-to-robot-base T_robot_base_from_arm_base.npy \
-  --mode eye_to_hand \
-  --out handeye_result.json
-```
-
----
-## 九、数据处理脚本
-
-提供一个模板脚本，读取机器人驱动与视觉检测的实时话题，自动同步采样并生成 dataset.npz
-将手眼标定结果直接写为 ROS2 的 static_transform_publisher 配置或 URDF 片段，便于一键加载

src/scripts/images_to_quaternions.py

@@ -1,207 +0,0 @@
-#!/usr/bin/env python3
-"""
-Detect calibration board in images, estimate camera pose via solvePnP,
-and output quaternions (qx,qy,qz,qw) for up to N images.
-
-Supports:
-- chessboard or Charuco detection
-- YAML/JSON camera intrinsics (OpenCV-style keys)
-- Save to CSV and print to stdout
-
-Example:
-  python src/scripts/images_to_quaternions.py \
-    --images data/images/*.png \
-    --camera-calib data/camera.yaml \
-    --pattern chessboard --board-rows 6 --board-cols 9 --square-size 0.024 \
-    --limit 12 --out /tmp/quats.csv
-"""
-from __future__ import annotations
-
-import argparse
-import glob
-import json
-import os
-from typing import Tuple, List
-
-import cv2
-import numpy as np
-
-
-def load_camera_calib(path: str) -> Tuple[np.ndarray, np.ndarray]:
-    import yaml
-    with open(path, 'r') as f:
-        txt = f.read()
-    try:
-        data = yaml.safe_load(txt)
-    except Exception:
-        data = json.loads(txt)
-    K = data.get('camera_matrix') or data.get('cameraMatrix') or data.get('K')
-    if isinstance(K, dict) and 'data' in K:
-        K = K['data']
-    D = data.get('dist_coeffs') or data.get('distCoeffs') or data.get('D')
-    if isinstance(D, dict) and 'data' in D:
-        D = D['data']
-    if K is None or D is None:
-        raise ValueError('Calibration must contain camera_matrix/cameraMatrix/K and dist_coeffs/distCoeffs/D')
-    K = np.array(K, dtype=float).reshape(3, 3)
-    D = np.array(D, dtype=float).reshape(-1, 1)
-    return K, D
-
-
-def build_chessboard_object_points(rows: int, cols: int, square: float) -> np.ndarray:
-    objp = np.zeros((rows * cols, 3), np.float32)
-    grid = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
-    objp[:, :2] = grid * square
-    return objp
-
-
-def detect_chessboard(gray: np.ndarray, rows: int, cols: int) -> Tuple[bool, np.ndarray]:
-    flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
-    ok, corners = cv2.findChessboardCorners(gray, (cols, rows), flags)
-    if not ok:
-        return False, None
-    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
-    corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
-    return True, corners.reshape(-1, 2)
-
-
-def detect_charuco(gray: np.ndarray, rows: int, cols: int, square: float, marker: float) -> Tuple[bool, np.ndarray, np.ndarray]:
-    if not hasattr(cv2, 'aruco'):
-        raise RuntimeError('opencv-contrib-python is required for Charuco detection')
-    aruco = cv2.aruco
-    aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, 'DICT_4X4_50'))
-    board = aruco.CharucoBoard((cols, rows), square, marker, aruco_dict)
-    params = aruco.DetectorParameters()
-    detector = aruco.ArucoDetector(aruco_dict, params)
-    corners, ids, _ = detector.detectMarkers(gray)
-    if ids is None or len(ids) == 0:
-        return False, None, None
-    retval, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
-    ok = retval is not None and retval >= 4
-    return ok, (charuco_corners.reshape(-1, 2) if ok else None), (charuco_ids if ok else None)
-
-
-def rot_to_quat(R: np.ndarray) -> np.ndarray:
-    tr = np.trace(R)
-    if tr > 0.0:
-        S = np.sqrt(tr + 1.0) * 2.0
-        qw = 0.25 * S
-        qx = (R[2, 1] - R[1, 2]) / S
-        qy = (R[0, 2] - R[2, 0]) / S
-        qz = (R[1, 0] - R[0, 1]) / S
-    else:
-        if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
-            S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
-            qx = 0.25 * S
-            qy = (R[0, 1] + R[1, 0]) / S
-            qz = (R[0, 2] + R[2, 0]) / S
-            qw = (R[2, 1] - R[1, 2]) / S
-        elif R[1, 1] > R[2, 2]:
-            S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
-            qx = (R[0, 1] + R[1, 0]) / S
-            qy = 0.25 * S
-            qz = (R[1, 2] + R[2, 1]) / S
-            qw = (R[0, 2] - R[2, 0]) / S
-        else:
-            S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
-            qx = (R[0, 2] + R[2, 0]) / S
-            qy = (R[1, 2] + R[2, 1]) / S
-            qz = 0.25 * S
-            qw = (R[1, 0] - R[0, 1]) / S
-    q = np.array([qx, qy, qz, qw])
-    return q / (np.linalg.norm(q) + 1e-12)
-
-
-def pnp_quaternion_for_image(image_path: str, K: np.ndarray, D: np.ndarray,
-                              pattern: str, rows: int, cols: int, square: float,
-                              charuco_marker: float | None = None) -> Tuple[np.ndarray, np.ndarray]:
-    gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
-    if gray is None:
-        raise FileNotFoundError(f'Cannot read image: {image_path}')
-    if pattern == 'chessboard':
-        ok, corners = detect_chessboard(gray, rows, cols)
-        if not ok:
-            raise RuntimeError(f'Chessboard detection failed: {image_path}')
-        objp = build_chessboard_object_points(rows, cols, square)
-        retval, rvec, tvec = cv2.solvePnP(objp, corners, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
-        if not retval:
-            raise RuntimeError(f'solvePnP failed: {image_path}')
-    elif pattern == 'charuco':
-        if charuco_marker is None:
-            raise ValueError('charuco requires --charuco-marker')
-        ok, charuco_corners, charuco_ids = detect_charuco(gray, rows, cols, square, charuco_marker)
-        if not ok:
-            raise RuntimeError(f'Charuco detection failed: {image_path}')
-        aruco = cv2.aruco
-        aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, 'DICT_4X4_50'))
-        board = aruco.CharucoBoard((cols, rows), square, charuco_marker, aruco_dict)
-        obj_pts = []
-        img_pts = []
-        for idx, corner in zip(charuco_ids.flatten(), charuco_corners.reshape(-1, 2)):
-            obj_pts.append(board.getChessboardCorners()[idx][0])
-            img_pts.append(corner)
-        obj_pts = np.array(obj_pts, dtype=np.float32)
-        img_pts = np.array(img_pts, dtype=np.float32)
-        retval, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
-        if not retval:
-            raise RuntimeError(f'solvePnP failed: {image_path}')
-    else:
-        raise ValueError('pattern must be chessboard or charuco')
-
-    R, _ = cv2.Rodrigues(rvec)
-    q = rot_to_quat(R)
-    return q, tvec.flatten()
-
-
-def main():
-    parser = argparse.ArgumentParser(description='Estimate quaternions from board detections in images (solvePnP).')
-    parser.add_argument('--images', type=str, required=True, help='Directory or glob for images')
-    parser.add_argument('--camera-calib', type=str, required=True, help='Camera intrinsics (YAML/JSON)')
-    parser.add_argument('--pattern', type=str, default='chessboard', choices=['chessboard', 'charuco'])
-    parser.add_argument('--board-rows', type=int, default=6)
-    parser.add_argument('--board-cols', type=int, default=9)
-    parser.add_argument('--square-size', type=float, default=0.024, help='meters')
-    parser.add_argument('--charuco-marker', type=float, default=None, help='meters for Charuco marker size')
-    parser.add_argument('--limit', type=int, default=12, help='Max number of images to process')
-    parser.add_argument('--out', type=str, default=None, help='Output CSV path')
-
-    args = parser.parse_args()
-
-    # Collect images
-    if os.path.isdir(args.images):
-        img_paths = sorted(glob.glob(os.path.join(args.images, '*')))
-    else:
-        img_paths = sorted(glob.glob(args.images))
-    if not img_paths:
-        raise FileNotFoundError(f'No images found: {args.images}')
-    img_paths = img_paths[: args.limit]
-
-    # Load intrinsics
-    K, D = load_camera_calib(args.camera_calib)
-
-    # Process
-    results: List[Tuple[str, np.ndarray]] = []
-    for p in img_paths:
-        try:
-            q, t = pnp_quaternion_for_image(
-                p, K, D,
-                args.pattern, args.board_rows, args.board_cols, args.square_size,
-                args.charuco_marker,
-            )
-            results.append((p, q))
-            print(f'{os.path.basename(p)}: qx={q[0]:.8f}, qy={q[1]:.8f}, qz={q[2]:.8f}, qw={q[3]:.8f}')
-        except Exception as e:
-            print(f'WARN: {p}: {e}')
-
-    if args.out:
-        import csv
-        with open(args.out, 'w', newline='') as f:
-            writer = csv.writer(f)
-            writer.writerow(['image', 'qx', 'qy', 'qz', 'qw'])
-            for p, q in results:
-                writer.writerow([os.path.basename(p), f'{q[0]:.8f}', f'{q[1]:.8f}', f'{q[2]:.8f}', f'{q[3]:.8f}'])
-        print(f'Written CSV: {args.out}')
-
-
-if __name__ == '__main__':
-    main()

src/scripts/quaternion_conversion.md

@@ -1,115 +0,0 @@
-# 姿态到四元数转换说明
-
-本文档说明如何将姿态表示（欧拉角与旋转向量 Rodrigues/Axis-Angle）转换为四元数 (qx, qy, qz, qw)，包含原理、公式、边界情况与一个完整的计算示例，并给出现有脚本 `src/scripts/euler_to_quaternion.py` 的使用方法。
-
-## 基本约定
-- 四元数记为 q = [x, y, z, w]，其中 w 为实部，(x, y, z) 为虚部（与 ROS、OpenCV 常见约定一致）。
-- 旋转向量 r = (rx, ry, rz) 的方向为旋转轴，模长 |r| 为旋转角（弧度）。
-- 欧拉角使用“内在旋转”约定，支持顺序 xyz（默认）与 zyx。
-
----
-
-## 1) 旋转向量 (Rodrigues) → 四元数
-给定旋转向量 r = (rx, ry, rz)：
-1. 计算旋转角与单位旋转轴：
-   - 角度：θ = |r| = sqrt(rx^2 + ry^2 + rz^2)
-   - 轴：u = r / θ = (ux, uy, uz)
-2. 由轴角到四元数：
-   - s = sin(θ/2), c = cos(θ/2)
-   - q = (ux·s, uy·s, uz·s, c)
-3. 数值稳定性：若 θ 非常小（例如 < 1e-12），可近似认为 q ≈ (0, 0, 0, 1)。
-4. 归一化：实际实现中会做一次单位化 q ← q / ||q||，以避免数值误差。
-
-> 注：四元数 q 与 -q 表示同一旋转（双覆表示）。
-
-### 示例（完整计算过程）
-已知 r = (-1.433, 0.114, -0.430)（单位：rad）：
-- θ = |r| ≈ 1.5004615956
-- u = r / θ ≈ (-0.955039439, 0.075976620, -0.286578478)
-- s = sin(θ/2) ≈ 0.6818076141
-- c = cos(θ/2) ≈ 0.7315315286
-- q = (ux·s, uy·s, uz·s, c)
-    ≈ (-0.6511531610, 0.0518014378, -0.1953913882, 0.7315315286)
-
-因此，对应四元数为：
-- qx,qy,qz,qw ≈ -0.65115316, 0.05180144, -0.19539139, 0.73153153
-
----
-
-## 2) 欧拉角 → 四元数
-对于欧拉角 (rx, ry, rz)，若输入单位为弧度：
-- 以 xyz 顺序为例（依次绕自身 x、y、z 轴）：
-  1. 构造三个轴角四元数：
-     - qx = (sin(rx/2), 0, 0, cos(rx/2))
-     - qy = (0, sin(ry/2), 0, cos(ry/2))
-     - qz = (0, 0, sin(rz/2), cos(rz/2))
-  2. 乘法顺序为 q = qx ⊗ qy ⊗ qz（内在旋转，右乘积）。
-- 对于 zyx 顺序：q = qz ⊗ qy ⊗ qx。
-- 最后同样进行单位化。
-
-> 提示：不同库/软件可能有“外在 vs 内在”、“左乘 vs 右乘”、“轴顺序”等差异，需确保与使用方约定一致。
-
----
-
-## 3) 边界与常见问题
-- 零角或极小角：直接返回单位四元数 (0,0,0,1) 或采用泰勒展开近似。
-- 符号一致性：q 与 -q 表示同一旋转，批量处理时常将 w 约束为非负以保证连续性（可选）。
-- 单位：所有角度必须使用弧度；若源数据是度，请先转弧度（乘以 π/180）。
-- 数值稳定：建议在输出前做单位化，避免浮点累积误差。
-
----
-
-## 4) 使用脚本批量/单次转换
-脚本路径：`src/scripts/euler_to_quaternion.py`
-
-- 单次旋转向量 → 四元数：
-  ```bash
-  python src/scripts/euler_to_quaternion.py --mode rotvec --single RX RY RZ
-  # 例如：
-  python src/scripts/euler_to_quaternion.py --mode rotvec --single -1.433 0.114 -0.430
-  ```
-
-- CSV 批量（默认欧拉角，若为旋转向量请加 --mode rotvec）：
-  ```bash
-  python src/scripts/euler_to_quaternion.py \
-    --in input.csv --out output.csv \
-    --rx rx --ry ry --rz rz \
-    --mode rotvec
-  ```
-
-- 欧拉角（内在旋转顺序 xyz 或 zyx）：
-  ```bash
-  python src/scripts/euler_to_quaternion.py \
-    --mode euler --order xyz --single RX RY RZ
-  ```
-
-> 如果输入 CSV 同时包含位姿中的位置列（x,y,z），脚本支持可选的单位转换 `--mm-to-m`（将毫米换算为米，并额外输出 x_m,y_m,z_m 列）。
-
----
-
-## 5) 本次 12 组旋转向量的结果
-以下 12 组 (rx, ry, rz)（单位：rad）对应的四元数 (qx, qy, qz, qw)：
-
-1. -0.65115316,  0.05180144, -0.19539139, 0.73153153
-2. -0.71991924, -0.00710155, -0.17753865, 0.67092912
-3. -0.44521470, -0.25512818, -0.41269388, 0.75258039
-4. -0.72304324,  0.09631938, -0.32264833, 0.60318248
-5. -0.67311368,  0.06894426, -0.11793097, 0.72681287
-6. -0.73524204, -0.19261515, -0.19652833, 0.61943132
-7. -0.75500427, -0.08296268, -0.09788718, 0.64304265
-8. -0.88627353, -0.08089799, -0.15658968, 0.42831579
-9. -0.62408775, -0.13051614, -0.11718879, 0.76141106
-10. -0.67818166, -0.10516535, -0.18696062, 0.70289090
-11. -0.77275040, -0.19297175, -0.05741665, 0.60193194
-12. -0.66493346,  0.09013744, -0.16351565, 0.72318833
-
----
-
-## 6) 参考实现
-脚本 `euler_to_quaternion.py` 中的核心函数：
-- `rotvec_to_quaternion(rx, ry, rz)`：实现了第 1 节所述的 Rodrigues → 四元数转换，并在小角度与归一化方面做了稳健处理。
-- `euler_to_quaternion(rx, ry, rz, order, degrees)`：实现了第 2 节所述的欧拉角 → 四元数转换，支持 xyz/zyx 两种顺序与度/弧度输入。
-
-如需将结果保存为 CSV 或用于后续手眼标定、TF 发布等，可直接复用该脚本的命令行接口。
-
----

src/scripts/requirements.txt

@@ -1,3 +0,0 @@
-# Pinned to avoid NumPy 2.x ABI issues with some wheels
-numpy<2
-opencv-python==4.11.0.86

src/scripts/rotate_pose.py

@@ -1,119 +0,0 @@
-#!/usr/bin/env python3
-"""
-rotate_pose.py
-
-Small utility to rotate a 7-element pose (x,y,z,qx,qy,qz,qw or qw,qx,qy,qz)
-around a given axis by a specified angle (degrees).
-
-Usage examples:
-  ./rotate_pose.py --pose 0.1 0.2 0.3 -0.6 0.1 -0.1 0.7 --axis z --angle 180 --order qxqyqzqw --frame world
-  python3 rotate_pose.py --pose 0.179387 0.341708 -0.099286 -0.88140507 0.12851699 -0.17498077 0.41951188 --axis z --angle 180 --order qxqyqzqw --frame world
-
-Supported frames:
-  - world: apply rotation in world frame (q' = q_rot * q)
-  - body: apply rotation in body/local frame (q' = q * q_rot)
-
-Supported quaternion order strings: "qxqyqzqw" (default) or "wxyz" (qw,qx,qy,qz)
-
-"""
-import argparse
-import math
-from typing import List, Tuple
-
-
-def normalize_quat(q: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]:
-    x, y, z, w = q
-    n = math.sqrt(x * x + y * y + z * z + w * w)
-    if n == 0:
-        return (0.0, 0.0, 0.0, 1.0)
-    return (x / n, y / n, z / n, w / n)
-
-
-def axis_angle_to_quat(axis: str, angle_deg: float) -> Tuple[float, float, float, float]:
-    a = math.radians(angle_deg)
-    ca = math.cos(a / 2.0)
-    sa = math.sin(a / 2.0)
-    ax = axis.lower()
-    if ax == 'x':
-        return (sa, 0.0, 0.0, ca)
-    if ax == 'y':
-        return (0.0, sa, 0.0, ca)
-    if ax == 'z':
-        return (0.0, 0.0, sa, ca)
-    raise ValueError('axis must be x,y or z')
-
-
-def quat_mul(a: Tuple[float,float,float,float], b: Tuple[float,float,float,float]) -> Tuple[float,float,float,float]:
-    ax, ay, az, aw = a
-    bx, by, bz, bw = b
-    x = aw*bx + ax*bw + ay*bz - az*by
-    y = aw*by - ax*bz + ay*bw + az*bx
-    z = aw*bz + ax*by - ay*bx + az*bw
-    w = aw*bw - ax*bx - ay*by - az*bz
-    return (x, y, z, w)
-
-
-def rotate_position(pos: Tuple[float,float,float], axis: str, angle_deg: float) -> Tuple[float,float,float]:
-    # For 180-degree rotations we can do simple sign flips but implement general rotation using quaternion
-    qrot = axis_angle_to_quat(axis, angle_deg)
-    # convert pos to quaternion (v,0)
-    px, py, pz = pos
-    p = (px, py, pz, 0.0)
-    qrot_n = normalize_quat(qrot)
-    qrot_conj = (-qrot_n[0], -qrot_n[1], -qrot_n[2], qrot_n[3])
-    tmp = quat_mul(qrot_n, p)
-    p_rot = quat_mul(tmp, qrot_conj)
-    return (p_rot[0], p_rot[1], p_rot[2])
-
-
-def parse_pose(vals: List[float], order: str) -> Tuple[Tuple[float,float,float], Tuple[float,float,float,float]]:
-    if order == 'qxqyqzqw':
-        if len(vals) != 7:
-            raise ValueError('need 7 values for pose')
-        return (vals[0], vals[1], vals[2]), (vals[3], vals[4], vals[5], vals[6])
-    if order == 'wxyz':
-        if len(vals) != 7:
-            raise ValueError('need 7 values for pose')
-        return (vals[0], vals[1], vals[2]), (vals[3], vals[4], vals[5], vals[6]) if False else (vals[4], vals[5], vals[6], vals[3])
-    raise ValueError('unsupported order')
-
-
-def format_pose(pos: Tuple[float,float,float], quat: Tuple[float,float,float,float], order: str) -> List[float]:
-    if order == 'qxqyqzqw':
-        return [pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]]
-    if order == 'wxyz':
-        # qw,qx,qy,qz
-        return [pos[0], pos[1], pos[2], quat[3], quat[0], quat[1], quat[2]]
-    raise ValueError('unsupported order')
-
-
-def main():
-    p = argparse.ArgumentParser()
-    p.add_argument('--pose', nargs=7, type=float, required=True, help='pose values')
-    p.add_argument('--axis', choices=['x','y','z'], default='z')
-    p.add_argument('--angle', type=float, default=180.0)
-    p.add_argument('--order', choices=['qxqyqzqw','wxyz'], default='qxqyqzqw')
-    p.add_argument('--frame', choices=['world','body'], default='world')
-    args = p.parse_args()
-
-    pos, quat = parse_pose(list(args.pose), args.order)
-    # rotation quaternion
-    qrot = axis_angle_to_quat(args.axis, args.angle)
-    qrot = normalize_quat(qrot)
-
-    # apply rotation to position in world frame: pos' = R * pos
-    pos_rot = rotate_position(pos, args.axis, args.angle)
-
-    # apply rotation to orientation
-    if args.frame == 'world':
-        qnew = quat_mul(qrot, quat)
-    else:
-        qnew = quat_mul(quat, qrot)
-    qnew = normalize_quat(qnew)
-
-    out = format_pose(pos_rot, qnew, args.order)
-    # Print as numeric list without quotes
-    print('rotated_pose:', [float('{:.8f}'.format(x)) for x in out])
-
-if __name__ == '__main__':
-    main()

src/scripts/run.sh

@@ -1,141 +0,0 @@
-#!/usr/bin/env bash
-
-set -euo pipefail
-
-# Resolve workspace root (script may be invoked from repo root)
-SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
-WS_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
-
-# Source ROS 2 workspace (temporarily disable nounset to avoid unbound vars inside setup scripts)
-if [[ -f "${WS_ROOT}/install/setup.bash" ]]; then
-	# shellcheck source=/dev/null
-	set +u
-	source "${WS_ROOT}/install/setup.bash"
-	set -u
-else
-	echo "[run.sh] install/setup.bash not found at ${WS_ROOT}/install/setup.bash"
-	echo "[run.sh] Please build the workspace first, e.g.,: colcon build && source install/setup.bash"
-	exit 1
-fi
-
-# Human-friendly logging format with local wall-clock time and colors
-export RCUTILS_COLORIZED_OUTPUT=${RCUTILS_COLORIZED_OUTPUT:-1}
-export RCUTILS_CONSOLE_OUTPUT_FORMAT='[{time}] [{severity}] [{name}]: {message}'
-
-LOG_TO_FILES=0
-LOG_LEVEL="info"   # debug|info|warn|error|fatal
-RUN_BRAIN=1
-RUN_MOTION=1
-
-usage() {
-	cat <<EOF
-Usage: ${0##*/} [options]
-
-Options:
-	-l, --log-to-files        Write each node's stdout/stderr to log/run/<timestamp>/*.log
-	-L, --log-level LEVEL     Set ROS 2 log level (default: info). Ex: debug, warn, error
-			--brain-only          Run only brain node
-			--arm_control-only         Run only arm_control node
-	-h, --help                Show this help and exit
-
-Environment:
-	RCUTILS_COLORIZED_OUTPUT  Colorize console logs (default: 1)
-	RCUTILS_CONSOLE_OUTPUT_FORMAT  Log format (already configured here)
-EOF
-}
-
-# Parse args
-while [[ $# -gt 0 ]]; do
-	case "$1" in
-		-l|--log-to-files)
-			LOG_TO_FILES=1; shift ;;
-		-L|--log-level)
-			LOG_LEVEL="${2:-info}"; shift 2 ;;
-		--brain-only)
-			RUN_BRAIN=1; RUN_MOTION=0; shift ;;
-		--arm_control-only)
-			RUN_BRAIN=0; RUN_MOTION=1; shift ;;
-		-h|--help)
-			usage; exit 0 ;;
-		*)
-			echo "Unknown option: $1" >&2
-			usage; exit 2 ;;
-	esac
-done
-
-timestamp() { date +"%Y%m%d_%H%M%S"; }
-
-LOG_DIR="${WS_ROOT}/log/run/$(timestamp)"
-mkdir -p "${LOG_DIR}"
-
-pids=()
-names=()
-
-start_node() {
-	local name="$1"; shift
-	local pkg="$1"; shift
-	local exe="$1"; shift
-	local log_file="${LOG_DIR}/${name}.log"
-
-	if (( LOG_TO_FILES )); then
-		echo "[run.sh] Starting ${name} -> ros2 run ${pkg} ${exe} (logs: ${log_file})"
-		(
-			# Disable ANSI colors in log files for readability
-			RCUTILS_COLORIZED_OUTPUT=0 exec ros2 run "${pkg}" "${exe}" --ros-args --log-level "${LOG_LEVEL}"
-		) >>"${log_file}" 2>&1 &
-	else
-		echo "[run.sh] Starting ${name} -> ros2 run ${pkg} ${exe} --ros-args --log-level ${LOG_LEVEL}"
-		ros2 run "${pkg}" "${exe}" --ros-args --log-level "${LOG_LEVEL}" &
-	fi
-	local pid=$!
-	pids+=("${pid}")
-	names+=("${name}")
-}
-
-cleanup_done=0
-cleanup() {
-	(( cleanup_done )) && return 0
-	cleanup_done=1
-	echo ""
-	echo "[run.sh] Shutting down (${#pids[@]} processes)..."
-	# Send SIGINT first for graceful shutdown
-	for pid in "${pids[@]:-}"; do
-		if kill -0 "$pid" 2>/dev/null; then
-			kill -INT "$pid" 2>/dev/null || true
-		fi
-	done
-	# Give them a moment to exit gracefully
-	sleep 1
-	# Force kill any stubborn processes
-	for pid in "${pids[@]:-}"; do
-		if kill -0 "$pid" 2>/dev/null; then
-			kill -TERM "$pid" 2>/dev/null || true
-		fi
-	done
-	wait || true
-	echo "[run.sh] All processes stopped."
-}
-
-trap cleanup INT TERM
-
-# Start requested nodes
-if (( RUN_MOTION )); then
-	start_node "arm_control" "arm_control" "motion_node"
-fi
-if (( RUN_BRAIN )); then
-	start_node "brain" "brain" "brain_node"
-fi
-
-if (( ${#pids[@]} == 0 )); then
-	echo "[run.sh] Nothing to run. Use --brain-only or --arm_control-only to pick a node."
-	exit 0
-fi
-
-echo "[run.sh] Started: ${names[*]}"
-echo "[run.sh] Press Ctrl+C to stop."
-
-# Wait for all background processes
-wait
-
-# Ensure cleanup executes on normal exit too
-cleanup

src/scripts/send_arm_goal.sh

@@ -1,94 +0,0 @@
-#!/usr/bin/env bash
-set -euo pipefail
-
-# Defaults
-ACTION_NAME="/ArmSpaceControl"
-FRAME_ID="base_link"
-X=0.5
-Y=0.0
-Z=0.3
-YAW_DEG=0.0
-ANGLE_UNIT="deg"   # deg|rad
-DRY_RUN=0
-LOG_LEVEL="info"
-
-usage(){
-  cat <<EOF
-Usage: ${0##*/} [options]
-
-Options:
-  -a, --action NAME     Action name (default: /arm_space_control)
-  -f, --frame ID        Target frame_id (default: base_link)
-  --x VALUE             Position x (m) (default: 0.5)
-  --y VALUE             Position y (m) (default: 0.0)
-  --z VALUE             Position z (m) (default: 0.3)
-  --yaw VALUE           Yaw angle (default: 0.0)
-  --rad                 Interpret yaw as radians (default is degrees)
-  --deg                 Interpret yaw as degrees (default)
-  --log-level LEVEL     ROS 2 log level (default: info)
-  --dry-run             Print goal YAML and exit
-  -h, --help            Show this help
-
-Examples:
-  ${0##*/} --x 0.5 --y 0.0 --z 0.3 --yaw 90 --deg --frame base_link
-  ${0##*/} --action /ArmSpaceControl --yaw 1.57 --rad
-EOF
-}
-
-# Parse args
-while [[ $# -gt 0 ]]; do
-  case "$1" in
-    -a|--action) ACTION_NAME="$2"; shift 2 ;;
-    -f|--frame) FRAME_ID="$2"; shift 2 ;;
-    --x) X="$2"; shift 2 ;;
-    --y) Y="$2"; shift 2 ;;
-    --z) Z="$2"; shift 2 ;;
-    --yaw) YAW_DEG="$2"; shift 2 ;;
-    --rad) ANGLE_UNIT="rad"; shift ;;
-    --deg) ANGLE_UNIT="deg"; shift ;;
-    --log-level) LOG_LEVEL="$2"; shift 2 ;;
-    --dry-run) DRY_RUN=1; shift ;;
-    -h|--help) usage; exit 0 ;;
-    *) echo "Unknown option: $1" >&2; usage; exit 2 ;;
-  esac
-done
-
-# Compute quaternion from yaw only (roll=pitch=0)
-# qx=qy=0, qz=sin(yaw/2), qw=cos(yaw/2)
-python_quat='import math,sys; yaw=float(sys.argv[1]); print(f"{math.sin(yaw/2.0)} {math.cos(yaw/2.0)}")'
-
-if [[ "$ANGLE_UNIT" == "deg" ]]; then
-  # Convert degrees to radians in Python for accuracy
-  read -r QZ QW < <(python3 -c 'import math,sys; yaw_deg=float(sys.argv[1]); yaw=math.radians(yaw_deg); print(f"{math.sin(yaw/2.0)} {math.cos(yaw/2.0)}")' "$YAW_DEG")
-else
-  read -r QZ QW < <(python3 -c "$python_quat" "$YAW_DEG")
-fi
-
-# Build YAML (avoid shell-escaping issues; no quotes around frame_id)
-GOAL_YAML=$(cat <<YAML
-{target: {header: {frame_id: ${FRAME_ID}}, pose: {position: {x: ${X}, y: ${Y}, z: ${Z}}, orientation: {x: 0.0, y: 0.0, z: ${QZ}, w: ${QW}}}}}
-YAML
-)
-
-if (( DRY_RUN )); then
-  echo "Will send to action: ${ACTION_NAME}"
-  echo "Goal YAML:"
-  echo "$GOAL_YAML"
-  exit 0
-fi
-
-# Source workspace
-SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
-WS_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
-if [[ -f "${WS_ROOT}/install/setup.bash" ]]; then
-  # shellcheck source=/dev/null
-  source "${WS_ROOT}/install/setup.bash"
-else
-  echo "install/setup.bash not found at ${WS_ROOT}/install/setup.bash" >&2
-  exit 1
-fi
-
-# Send goal
-set -x
-ros2 action send_goal --feedback "${ACTION_NAME}" interfaces/action/ArmSpaceControl "$GOAL_YAML" --ros-args --log-level "${LOG_LEVEL}"
-set +x

src/thirdparty/SMACC2-humble/smacc2/CMakeLists.txt

@@ -19,6 +19,10 @@ find_package(Boost COMPONENTS thread REQUIRED)
 find_package(tracetools)
 find_package(LTTngUST REQUIRED)
 
+# hivecore logger SDK
+set(hivecore_logger_cpp_DIR "/opt/hivecore/logger-sdk/1.0.1/lib/cmake/hivecore_logger_cpp")
+find_package(hivecore_logger_cpp REQUIRED)
+
 set(dependencies
   rclcpp
   smacc2_msgs
@@ -26,7 +30,9 @@ set(dependencies
   tracetools
 )
 
-include_directories(include ${LTTNGUST_INCLUDE_DIRS})
+include_directories(include ${LTTNGUST_INCLUDE_DIRS}
+  /opt/hivecore/logger-sdk/1.0.1/include
+)
 
 file(GLOB_RECURSE SRC_FILES src *.cpp)
 add_library(smacc2 SHARED ${SRC_FILES})
@@ -42,6 +48,7 @@ ament_target_dependencies(smacc2 ${dependencies})
 target_link_libraries(smacc2
   ${Boost_LIBRARIES}
   ${LTTNGUST_LIBRARIES}
+  hivecore_logger_cpp::hivecore_logger_cpp
 )
 
 ament_export_include_directories(include)

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp

@@ -21,11 +21,13 @@
 #pragma once
 
 #include <smacc2/client_bases/smacc_action_client.hpp>
+#include <sstream>
 #include <smacc2/smacc_default_events.hpp>
 #include <smacc2/smacc_signal.hpp>
 
 #include <optional>
 #include <rclcpp_action/rclcpp_action.hpp>
+#include "hivecore_logger/logger.hpp"
 
 namespace smacc2
 {
@@ -86,9 +88,7 @@ public:
 
   void waitForServer()
   {
-    RCLCPP_INFO_STREAM(
-      this->getLogger(),
-      "Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>());
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>(); return _oss.str(); }());
     client_->wait_for_action_server();
   }
 
@@ -125,8 +125,7 @@ public:
     auto * ev = new EvType();
     // ev->client = this;
     // ev->resultMessage = *result;
-    RCLCPP_INFO(
-      getLogger(), "Action client Posting EVENT %s", demangleSymbol(typeid(ev).name()).c_str());
+    LOG_INFO("Action client Posting EVENT {}", demangleSymbol(typeid(ev).name()).c_str());
     this->postEvent(ev);
   }
 
@@ -148,7 +147,7 @@ public:
       actionFeedbackEvent->client = this;
       actionFeedbackEvent->feedbackMessage = msg;
       this->postEvent(actionFeedbackEvent);
-      RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", demangleType(typeid(*this)).c_str());
+      LOG_DEBUG("[{}] FEEDBACK EVENT", demangleType(typeid(*this)).c_str());
     };
 
     feedback_cb = [this](auto client, auto feedback) { this->onFeedback(client, feedback); };
@@ -275,13 +274,11 @@ public:
       // if goal ids are not matched, the older goa call this callback so ignore the result
       // if matched, it must be processed (including aborted)
 
-      RCLCPP_INFO_STREAM(
-        getLogger(), "[" << getName() << "]  Action result callback, getting shared future");
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "]  Action result callback, getting shared future"; return _oss.str(); }());
       // auto goalHandle = result->get();
       // goalHandle_ = lastRequest_->get();
-      RCLCPP_INFO_STREAM(
-        getLogger(), "[" << getName() << "]  Action client Result goal id: "
-                         << rclcpp_action::to_string(result.goal_id));
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "]  Action client Result goal id: "
+                         << rclcpp_action::to_string(result.goal_id); return _oss.str(); }());
 
       // if (goalHandle_->get_goal_id() == result.goal_id)
       // {
@@ -293,15 +290,13 @@ public:
 
       if (resultCallbackPtr != nullptr)
       {
-        RCLCPP_INFO_STREAM(
-          getLogger(), "[" << getName() << "]  Result CB calling user callback:"
-                           << demangleSymbol(typeid(*resultCallbackPtr).name()));
+        LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "]  Result CB calling user callback:"
+                           << demangleSymbol(typeid(*resultCallbackPtr).name()); return _oss.str(); }());
         (*resultCallbackPtr)(result);
       }
       else
       {
-        RCLCPP_INFO_STREAM(
-          getLogger(), "[" << getName() << "]  Result CB calling default callback");
+        LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "]  Result CB calling default callback"; return _oss.str(); }());
         this->onResult(result);
       }
 
@@ -323,23 +318,18 @@ public:
     //   RCLCPP_INFO_STREAM(getLogger(), getName() << ": no previous request.");
     // }
 
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[" << getName() << "] client ready clients: "
-                       << this->client_->get_number_of_ready_clients());
-    RCLCPP_INFO_STREAM(
-      getLogger(),
-      "[" << getName() << "] Waiting it is ready? " << client_->action_server_is_ready());
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] client ready clients: "
+                       << this->client_->get_number_of_ready_clients(); return _oss.str(); }());
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Waiting it is ready? " << client_->action_server_is_ready(); return _oss.str(); }());
 
-    RCLCPP_INFO_STREAM(getLogger(), getName() << ": async send goal.");
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << getName() << ": async send goal."; return _oss.str(); }());
     auto lastRequest = this->client_->async_send_goal(goal, options);
     this->lastRequest_ = lastRequest;
 
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[" << getName() << "] Action request "
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Action request "; return _oss.str(); }());
       // << rclcpp_action::to_string(this->goalHandle_->get_goal_id()) <<". Goal sent to " << this->action_endpoint_
       // << "\": " << std::endl
       // << goal
-    );
 
     // if (client_->isServerConnected())
     // {
@@ -395,46 +385,43 @@ protected:
     // const auto &resultType = this->getState();
     const auto & resultType = result_msg.code;
 
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[" << this->getName() << "] response result ["
-                       << rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response result ["
+                       << rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType; return _oss.str(); }());
 
     if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
     {
-      RCLCPP_INFO(getLogger(), "[%s] request result: Success", this->getName().c_str());
+      LOG_INFO("[{}] request result: Success", this->getName().c_str());
       onSucceeded_(result_msg);
       postSuccessEvent(result_msg);
     }
     else if (resultType == rclcpp_action::ResultCode::ABORTED)
     {
-      RCLCPP_INFO(getLogger(), "[%s] request result: Aborted", this->getName().c_str());
+      LOG_INFO("[{}] request result: Aborted", this->getName().c_str());
       onAborted_(result_msg);
       postAbortedEvent(result_msg);
     }
     else if (resultType == rclcpp_action::ResultCode::CANCELED)
     {
-      RCLCPP_INFO(getLogger(), "[%s] request result: Cancelled", this->getName().c_str());
+      LOG_INFO("[{}] request result: Cancelled", this->getName().c_str());
       onCancelled_(result_msg);
       postCancelledEvent(result_msg);
     }
     /*
     else if (resultType == actionlib::SimpleClientGoalState::REJECTED)
     {
-        RCLCPP_INFO(getLogger(),"[%s] request result: Rejected", this->getName().c_str());
+        LOG_INFO("[{}] request result: Rejected", this->getName().c_str());
         onRejected_(result_msg);
         postRejectedEvent(result_msg);
     }
     else if (resultType == actionlib::SimpleClientGoalState::PREEMPTED)
     {
-        RCLCPP_INFO(getLogger(),"[%s] request result: Preempted", this->getName().c_str());
+        LOG_INFO("[{}] request result: Preempted", this->getName().c_str());
         onPreempted_(result_msg);
         postPreemptedEvent(result_msg);
     }*/
     else
     {
-      RCLCPP_INFO(
-        getLogger(), "[%s] request result: NOT HANDLED TYPE: %d", this->getName().c_str(),
-        (int)resultType);
+      LOG_INFO("[{}] request result: NOT HANDLED TYPE: {}", this->getName().c_str(), (int)resultType);
     }
   }
 };

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_publisher_client.hpp

@@ -21,6 +21,8 @@
 #pragma once
 
 #include <optional>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_client.hpp>
 
 namespace smacc2
@@ -46,7 +48,7 @@ public:
     {
       if (!this->topicName)
       {
-        RCLCPP_ERROR(getLogger(), "topic publisher with no topic name set. Skipping advertising.");
+        LOG_ERROR("topic publisher with no topic name set. Skipping advertising.");
         return;
       }
 
@@ -58,8 +60,7 @@ public:
       qos.durability(*durability);
       qos.reliability(*reliability);
 
-      RCLCPP_INFO_STREAM(
-        getLogger(), "[" << this->getName() << "] Client Publisher to topic: " << topicName);
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Client Publisher to topic: " << topicName; return _oss.str(); }());
       pub_ = getNode()->create_publisher<MessageType>(*(this->topicName), qos);
 
       this->initialized_ = true;

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_service_client.hpp

@@ -21,6 +21,8 @@
 #pragma once
 
 #include <optional>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_client.hpp>
 
 namespace smacc2
@@ -43,12 +45,11 @@ public:
     {
       if (!serviceName_)
       {
-        RCLCPP_ERROR(getLogger(), "service client with no service name set. Skipping.");
+        LOG_ERROR("service client with no service name set. Skipping.");
       }
       else
       {
-        RCLCPP_INFO_STREAM(
-          getLogger(), "[" << this->getName() << "] Client Service: " << *serviceName_);
+        LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Client Service: " << *serviceName_; return _oss.str(); }());
         this->initialized_ = true;
         client_ = getNode()->create_client<ServiceType>(*serviceName_);
       }

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_service_server_client.hpp

@@ -21,6 +21,8 @@
 #pragma once
 
 #include <optional>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_client.hpp>
 #include <smacc2/smacc_signal.hpp>
 
@@ -67,14 +69,11 @@ public:
     {
       if (!serviceName_)
       {
-        RCLCPP_ERROR_STREAM(
-          getLogger(),
-          "[" << this->getName() << "] service server with no service name set. Skipping.");
+        LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] service server with no service name set. Skipping."; return _oss.str(); }());
       }
       else
       {
-        RCLCPP_INFO_STREAM(
-          getLogger(), "[" << this->getName() << "] Client Service: " << *serviceName_);
+        LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Client Service: " << *serviceName_; return _oss.str(); }());
 
         server_ = getNode()->create_service<TService>(
           *serviceName_, std::bind(

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_subscriber_client.hpp

@@ -21,6 +21,8 @@
 #pragma once
 
 #include <optional>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_client.hpp>
 #include <smacc2/smacc_signal.hpp>
 #include <smacc2/smacc_state_machine.hpp>
@@ -101,12 +103,11 @@ protected:
 
       if (!topicName)
       {
-        RCLCPP_ERROR(getLogger(), "topic client with no topic name set. Skipping subscribing");
+        LOG_ERROR("topic client with no topic name set. Skipping subscribing");
       }
       else
       {
-        RCLCPP_INFO_STREAM(
-          getLogger(), "[" << this->getName() << "] Subscribing to topic: " << *topicName);
+        LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Subscribing to topic: " << *topicName; return _oss.str(); }());
 
         rclcpp::SensorDataQoS qos;
         if (queueSize) qos.keep_last(*queueSize);

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_call_service.hpp

@@ -19,6 +19,8 @@
  ******************************************************************************************************************/
 #pragma once
 #include <smacc2/impl/smacc_asynchronous_client_behavior_impl.hpp>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_client_behavior.hpp>
 
 namespace smacc2
@@ -45,41 +47,36 @@ public:
 
   void onEntry() override
   {
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[" << this->getName() << "] creating ros service client: " << serviceName_);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] creating ros service client: " << serviceName_; return _oss.str(); }());
 
     client_ = getNode()->create_client<ServiceType>(serviceName_);
 
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[" << this->getName() << "] making service request to " << serviceName_);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] making service request to " << serviceName_; return _oss.str(); }());
 
     resultFuture_ = client_->async_send_request(request_).future.share();
 
     std::future_status status = resultFuture_.wait_for(0s);
 
-    RCLCPP_INFO_STREAM(
-      getLogger(), "thread state: " << (int)status << " ok " << rclcpp::ok() << " shutdown "
-                                    << this->isShutdownRequested() << "");
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "thread state: " << (int)status << " ok " << rclcpp::ok() << " shutdown "
+                                    << this->isShutdownRequested() << ""; return _oss.str(); }());
 
     while (status != std::future_status::ready && rclcpp::ok() && !this->isShutdownRequested())
     {
-      RCLCPP_INFO_STREAM_THROTTLE(
-        getLogger(), *getNode()->get_clock(), 1000,
-        "[" << this->getName() << "] waiting response ");
+      LOG_INFO_THROTTLE(1000, "{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] waiting response "; return _oss.str(); }());
       rclcpp::sleep_for(pollRate_);
       status = resultFuture_.wait_for(0s);
     }
 
     if (status == std::future_status::ready)
     {
-      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response received "; return _oss.str(); }());
       result_ = resultFuture_.get();
       onServiceResponse(result_);
       this->postSuccessEvent();
     }
     else
     {
-      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response not received ");
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response not received "; return _oss.str(); }());
       this->postFailureEvent();
     }
   }
@@ -97,7 +94,7 @@ protected:
 
   virtual void onServiceResponse(std::shared_ptr<typename ServiceType::Response> /*result*/)
   {
-    RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response received "; return _oss.str(); }());
   }
 };
 

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_sequence.hpp

@@ -22,6 +22,7 @@
 
 #include <functional>
 #include <rclcpp/rclcpp.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
 
 namespace smacc2
@@ -60,9 +61,7 @@ public:
     std::function<std::shared_ptr<smacc2::SmaccAsyncClientBehavior>()> delayedCBFactoryFn =
       [this, args...]()
     {
-      RCLCPP_INFO(
-        getLogger(), "[CbSequence] then creating new sub behavior %s ",
-        demangleSymbol<TBehavior>().c_str());
+      LOG_INFO("[CbSequence] then creating new sub behavior {} ", demangleSymbol<TBehavior>().c_str());
       auto createdBh = std::shared_ptr<TBehavior>(new TBehavior(args...));
 
       this->getCurrentState()->getOrthogonal<TOrthogonal>()->addClientBehavior(createdBh);

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_topic_message.hpp

@@ -21,7 +21,9 @@
 #pragma once
 
 #include <functional>
+#include <sstream>
 #include <rclcpp/rclcpp.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
 
 namespace smacc2
@@ -50,9 +52,8 @@ public:
     rclcpp::SensorDataQoS qos;
     // qos.reliable();
     // rclcpp::SubscriptionOptions sub_option;
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[CbWaitTopicMessage] waiting message from topic: "
-                     << topicname_ << "[" << demangledTypeName<TMessage>() << "]");
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbWaitTopicMessage] waiting message from topic: "
+                     << topicname_ << "[" << demangledTypeName<TMessage>() << "]"; return _oss.str(); }());
 
     // sub_ = getNode()->create_subscription<TMessage>(
     //   topicname_, qos,
@@ -70,7 +71,7 @@ public:
   {
     if (guardFn_ == nullptr || guardFn_(*msg))
     {
-      RCLCPP_INFO(getLogger(), "[CbWaitTopicMessage] message received.");
+      LOG_INFO("[CbWaitTopicMessage] message received.");
       success = true;
       this->postSuccessEvent();
     }

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_action_client.hpp

@@ -15,6 +15,7 @@
 #pragma once
 
 #include <smacc2/component.hpp>
+#include <sstream>
 #include <smacc2/smacc_default_events.hpp>
 #include <smacc2/smacc_signal.hpp>
 #include <smacc2/smacc_state_machine.hpp>
@@ -25,6 +26,7 @@
 #include <mutex>
 #include <optional>
 #include <rclcpp_action/rclcpp_action.hpp>
+#include "hivecore_logger/logger.hpp"
 
 namespace smacc2
 {
@@ -91,25 +93,23 @@ public:
     {
       std::lock_guard<std::mutex> lock(actionMutex_);
 
-      RCLCPP_INFO_STREAM(
-        getLogger(), "[" << this->getName() << "] Action result callback, goal id: "
-                         << rclcpp_action::to_string(result.goal_id));
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Action result callback, goal id: "
+                         << rclcpp_action::to_string(result.goal_id); return _oss.str(); }());
 
       auto resultCallbackPtr = resultCallback.lock();
       if (resultCallbackPtr != nullptr)
       {
-        RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Calling user result callback");
+        LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Calling user result callback"; return _oss.str(); }());
         (*resultCallbackPtr)(result);
       }
       else
       {
-        RCLCPP_INFO_STREAM(
-          getLogger(), "[" << this->getName() << "] Using default result handling");
+        LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Using default result handling"; return _oss.str(); }());
         this->onResult(result);
       }
     };
 
-    RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Sending goal to action server");
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Sending goal to action server"; return _oss.str(); }());
 
     auto goalFuture = client_->async_send_goal(goal, options);
     lastRequest_ = goalFuture;
@@ -123,7 +123,7 @@ public:
 
     if (lastRequest_ && lastRequest_->valid())
     {
-      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Cancelling current goal");
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Cancelling current goal"; return _oss.str(); }());
 
       auto cancelFuture = client_->async_cancel_all_goals();
       lastCancelResponse_ = cancelFuture;
@@ -131,7 +131,7 @@ public:
     }
     else
     {
-      RCLCPP_WARN_STREAM(getLogger(), "[" << this->getName() << "] No active goal to cancel");
+      LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] No active goal to cancel"; return _oss.str(); }());
       return false;
     }
   }
@@ -142,9 +142,7 @@ public:
   {
     if (client_)
     {
-      RCLCPP_INFO_STREAM(
-        getLogger(),
-        "[" << this->getName() << "] Waiting for action server: " << *actionServerName);
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Waiting for action server: " << *actionServerName; return _oss.str(); }());
       client_->wait_for_action_server();
     }
   }
@@ -154,13 +152,11 @@ public:
   {
     if (!actionServerName)
     {
-      RCLCPP_ERROR_STREAM(getLogger(), "[" << this->getName() << "] Action server name not set!");
+      LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Action server name not set!"; return _oss.str(); }());
       return;
     }
 
-    RCLCPP_INFO_STREAM(
-      getLogger(),
-      "[" << this->getName() << "] Initializing action client for: " << *actionServerName);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Initializing action client for: " << *actionServerName; return _oss.str(); }());
 
     client_ = rclcpp_action::create_client<ActionType>(getNode(), *actionServerName);
 
@@ -188,7 +184,7 @@ public:
       actionFeedbackEvent->client = this;
       actionFeedbackEvent->feedbackMessage = feedback;
       this->postEvent(actionFeedbackEvent);
-      RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", this->getName().c_str());
+      LOG_DEBUG("[{}] FEEDBACK EVENT", this->getName().c_str());
     };
   }
 
@@ -241,33 +237,30 @@ private:
   {
     const auto & resultType = result_msg.code;
 
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[" << this->getName() << "] Action result ["
-                       << rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Action result ["
+                       << rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType; return _oss.str(); }());
 
     if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
     {
-      RCLCPP_INFO(getLogger(), "[%s] Action result: Success", this->getName().c_str());
+      LOG_INFO("[{}] Action result: Success", this->getName().c_str());
       onActionSucceeded_(result_msg);
       postSuccessEvent(result_msg);
     }
     else if (resultType == rclcpp_action::ResultCode::ABORTED)
     {
-      RCLCPP_INFO(getLogger(), "[%s] Action result: Aborted", this->getName().c_str());
+      LOG_INFO("[{}] Action result: Aborted", this->getName().c_str());
       onActionAborted_(result_msg);
       postAbortedEvent(result_msg);
     }
     else if (resultType == rclcpp_action::ResultCode::CANCELED)
     {
-      RCLCPP_INFO(getLogger(), "[%s] Action result: Cancelled", this->getName().c_str());
+      LOG_INFO("[{}] Action result: Cancelled", this->getName().c_str());
       onActionCancelled_(result_msg);
       postCancelledEvent(result_msg);
     }
     else
     {
-      RCLCPP_WARN(
-        getLogger(), "[%s] Action result: Unhandled type: %d", this->getName().c_str(),
-        (int)resultType);
+      LOG_WARN("[{}] Action result: Unhandled type: {}", this->getName().c_str(), (int)resultType);
     }
   }
 
@@ -275,9 +268,7 @@ private:
   void postResultEvent(const WrappedResult & /* result */)
   {
     auto * ev = new EvType();
-    RCLCPP_INFO(
-      getLogger(), "[%s] Posting event: %s", this->getName().c_str(),
-      smacc2::demangleSymbol(typeid(ev).name()).c_str());
+    LOG_INFO("[{}] Posting event: {}", this->getName().c_str(), smacc2::demangleSymbol(typeid(ev).name()).c_str());
     this->postEvent(ev);
   }
 };

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_ros2_timer.hpp

@@ -15,8 +15,10 @@
 #pragma once
 
 #include <chrono>
+#include <sstream>
 #include <optional>
 #include <rclcpp/rclcpp.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/common.hpp>
 #include <smacc2/component.hpp>
 #include <smacc2/smacc_signal.hpp>
@@ -47,9 +49,8 @@ public:
   {
     if (!initialized_)
     {
-      RCLCPP_INFO_STREAM(
-        getLogger(), "[" << this->getName() << "] Initializing ROS2 Timer with duration: "
-                         << duration_.seconds() << "s, oneshot: " << (oneshot_ ? "true" : "false"));
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Initializing ROS2 Timer with duration: "
+                         << duration_.seconds() << "s, oneshot: " << (oneshot_ ? "true" : "false"); return _oss.str(); }());
 
       auto clock = this->getNode()->get_clock();
       timer_ = rclcpp::create_timer(
@@ -65,7 +66,7 @@ public:
   {
     if (timer_ && timer_->is_canceled())
     {
-      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Starting timer");
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Starting timer"; return _oss.str(); }());
       timer_->reset();
     }
   }
@@ -74,7 +75,7 @@ public:
   {
     if (timer_)
     {
-      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Stopping timer");
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Stopping timer"; return _oss.str(); }());
       timer_->cancel();
     }
   }
@@ -83,7 +84,7 @@ public:
   {
     if (timer_)
     {
-      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Cancelling timer");
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Cancelling timer"; return _oss.str(); }());
       timer_->cancel();
     }
   }
@@ -97,7 +98,7 @@ public:
 private:
   void timerCallback()
   {
-    RCLCPP_DEBUG_STREAM(getLogger(), "[" << this->getName() << "] Timer tick");
+    LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Timer tick"; return _oss.str(); }());
 
     if (!onTimerTick_.empty())
     {
@@ -106,8 +107,7 @@ private:
 
     if (oneshot_)
     {
-      RCLCPP_INFO_STREAM(
-        getLogger(), "[" << this->getName() << "] Oneshot timer completed, cancelling");
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Oneshot timer completed, cancelling"; return _oss.str(); }());
       timer_->cancel();
     }
   }

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_topic_publisher.hpp

@@ -20,7 +20,9 @@
 
 #pragma once
 #include <optional>
+#include <sstream>
 #include <rclcpp/rclcpp.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/common.hpp>
 #include <smacc2/component.hpp>
 
@@ -73,8 +75,7 @@ void CpTopicPublisher<T>::onInitialize()
     qos.durability(*durability);
     qos.reliability(*reliability);
 
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[" << this->getName() << "] Publisher to topic: " << topicName_);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Publisher to topic: " << topicName_; return _oss.str(); }());
 
     auto nh = this->getNode();
     pub_ = nh->template create_publisher<T>(this->topicName_, qos);

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_topic_subscriber.hpp

@@ -20,6 +20,8 @@
 
 #pragma once
 #include <optional>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/client_bases/smacc_subscriber_client.hpp>
 #include <smacc2/component.hpp>
 #include <smacc2/smacc_signal.hpp>
@@ -93,8 +95,7 @@ public:
 
       if (!queueSize) queueSize = 1;
 
-      RCLCPP_INFO_STREAM(
-        getLogger(), "[" << this->getName() << "] Subscribing to topic: " << topicName_);
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Subscribing to topic: " << topicName_; return _oss.str(); }());
 
       rclcpp::SensorDataQoS qos;
       if (queueSize) qos.keep_last(*queueSize);

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_asynchronous_client_behavior_impl.hpp

@@ -20,6 +20,7 @@
 
 #pragma once
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_state_machine.hpp>
 
 namespace smacc2
@@ -35,9 +36,7 @@ void SmaccAsyncClientBehavior::onOrthogonalAllocation()
 {
   if (postFinishEventFn_ || postSuccessEventFn_ || postFailureEventFn_)
   {
-    RCLCPP_WARN(
-      getLogger(),
-      "SmaccAsyncClientBehavior already has event posting functions assigned. Skipping "
+    LOG_WARN("SmaccAsyncClientBehavior already has event posting functions assigned. Skipping "
       "re-assignment. This could be a problem if you are using the same behavior in multiple "
       "states. This may be related with the deprecation of onOrthogonalAllocation in favor of "
       "onStateOrthogonalAllocation.");

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_client_behavior_impl.hpp

@@ -21,6 +21,7 @@
 #pragma once
 
 #include <smacc2/smacc_client_behavior.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_state_machine.hpp>
 
 namespace smacc2
@@ -30,9 +31,7 @@ void ISmaccClientBehavior::postEvent(const EventType & ev)
 {
   if (stateMachine_ == nullptr)
   {
-    RCLCPP_ERROR(
-      getLogger(),
-      "The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
+    LOG_ERROR("The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
       "post event call.");
   }
   else
@@ -46,9 +45,7 @@ void ISmaccClientBehavior::postEvent()
 {
   if (stateMachine_ == nullptr)
   {
-    RCLCPP_ERROR(
-      getLogger(),
-      "The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
+    LOG_ERROR("The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
       "post event call.");
   }
   else
@@ -75,9 +72,7 @@ void ISmaccClientBehavior::requiresComponent(
 {
   if (stateMachine_ == nullptr)
   {
-    RCLCPP_ERROR(
-      getLogger(),
-      "Cannot use the requiresComponent funcionality before assigning the client behavior to an "
+    LOG_ERROR("Cannot use the requiresComponent funcionality before assigning the client behavior to an "
       "orthogonal. Try using the OnEntry method to capture required components.");
   }
   else

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_client_impl.hpp

@@ -21,6 +21,7 @@
 #pragma once
 
 #include <smacc2/impl/smacc_state_machine_impl.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_client.hpp>
 
 namespace smacc2
@@ -99,11 +100,8 @@ SmaccComponentType * ISmaccClient::createNamedComponent(std::string name, TArgs.
   if (it == this->components_.end())
   {
     auto tname = demangledTypeName<SmaccComponentType>();
-    RCLCPP_INFO(
-      getLogger(),
-      "Creating a new component of type: %s smacc component is required by client '%s'. Creating a "
-      "new instance %s",
-      this->getName().c_str(), demangledTypeName<SmaccComponentType>().c_str(), tname.c_str());
+    LOG_INFO("Creating a new component of type: {} smacc component is required by client '{}'. Creating a "
+      "new instance {}", this->getName().c_str(), demangledTypeName<SmaccComponentType>().c_str(), tname.c_str());
 
     ret = std::shared_ptr<SmaccComponentType>(new SmaccComponentType(targs...));
     ret->setStateMachine(this->getStateMachine());
@@ -114,13 +112,11 @@ SmaccComponentType * ISmaccClient::createNamedComponent(std::string name, TArgs.
 
     this->components_[componentkey] =
       ret;  //std::dynamic_pointer_cast<smacc2::ISmaccComponent>(ret);
-    RCLCPP_DEBUG(getLogger(), "%s resource is required. Done.", tname.c_str());
+    LOG_DEBUG("{} resource is required. Done.", tname.c_str());
   }
   else
   {
-    RCLCPP_INFO(
-      getLogger(), "%s resource is required. Found resource in cache.",
-      demangledTypeName<SmaccComponentType>().c_str());
+    LOG_INFO("{} resource is required. Found resource in cache.", demangledTypeName<SmaccComponentType>().c_str());
     ret = dynamic_pointer_cast<SmaccComponentType>(it->second);
   }
 

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_component_impl.hpp

@@ -21,6 +21,8 @@
 #pragma once
 
 #include <string>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 
 #include <smacc2/component.hpp>
 #include <smacc2/impl/smacc_state_machine_impl.hpp>
@@ -48,17 +50,16 @@ void ISmaccComponent::requiresComponent(
 
   if (requiredComponentStorage == nullptr && throwExceptionIfNotExist)
   {
-    RCLCPP_DEBUG_STREAM(
-      this->getLogger(), std::string("Required component ") +
+    LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << std::string("Required component ") +
                            demangleSymbol(typeid(TComponent).name()) +
-                           " not found. Available components:");
+                           " not found. Available components:"; return _oss.str(); }());
 
     std::vector<std::shared_ptr<ISmaccComponent>> components;
     this->owner_->getComponents(components);
 
     for (auto c : components)
     {
-      RCLCPP_DEBUG(this->getLogger(), "- Component %s", c->getName().c_str());
+      LOG_DEBUG("- Component {}", c->getName().c_str());
     }
 
     throw std::runtime_error(
@@ -74,17 +75,16 @@ void ISmaccComponent::requiresComponent(
 
   if (requiredComponentStorage == nullptr && throwExceptionIfNotExist)
   {
-    RCLCPP_DEBUG_STREAM(
-      this->getLogger(), std::string("Required component with name: '") + name + "'" +
+    LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << std::string("Required component with name: '") + name + "'" +
                            demangleSymbol(typeid(TComponent).name()) +
-                           " not found. Available components:");
+                           " not found. Available components:"; return _oss.str(); }());
 
     std::vector<std::shared_ptr<ISmaccComponent>> components;
     this->owner_->getComponents(components);
 
     for (auto c : components)
     {
-      RCLCPP_DEBUG(this->getLogger(), " - Component %s", c->getName().c_str());
+      LOG_DEBUG(" - Component {}", c->getName().c_str());
     }
 
     throw std::runtime_error(

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_orthogonal_impl.hpp

@@ -20,6 +20,8 @@
 
 #pragma once
 #include <cassert>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_client.hpp>
 #include <smacc2/smacc_orthogonal.hpp>
 
@@ -35,10 +37,9 @@ bool ISmaccOrthogonal::requiresClient(SmaccClientType *& storage)
   }
 
   auto requiredClientName = demangledTypeName<SmaccClientType>();
-  RCLCPP_WARN_STREAM(
-    getLogger(), "Required client ["
+  LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Required client ["
                    << requiredClientName
-                   << "] not found in current orthogonal. Searching in other orthogonals.");
+                   << "] not found in current orthogonal. Searching in other orthogonals."; return _oss.str(); }());
 
   for (auto & orthoentry : this->getStateMachine()->getOrthogonals())
   {
@@ -47,19 +48,16 @@ bool ISmaccOrthogonal::requiresClient(SmaccClientType *& storage)
       storage = dynamic_cast<SmaccClientType *>(client.get());
       if (storage != nullptr)
       {
-        RCLCPP_WARN_STREAM(
-          getLogger(),
-          "Required client  [" << requiredClientName << "] found in other orthogonal.");
+        LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Required client  [" << requiredClientName << "] found in other orthogonal."; return _oss.str(); }());
         return true;
       }
     }
   }
 
-  RCLCPP_ERROR_STREAM(
-    getLogger(), "Required client ["
+  LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "Required client ["
                    << requiredClientName
                    << "] not found even in other orthogonals. Returning null pointer. If the "
-                      "requested client is used may result in a segmentation fault.");
+                      "requested client is used may result in a segmentation fault."; return _oss.str(); }());
   return false;
 }
 
@@ -68,9 +66,7 @@ void ISmaccOrthogonal::requiresComponent(SmaccComponentType *& storage)
 {
   if (stateMachine_ == nullptr)
   {
-    RCLCPP_ERROR(
-      getLogger(),
-      "Cannot use the requiresComponent funcionality from an orthogonal before onInitialize");
+    LOG_ERROR("Cannot use the requiresComponent funcionality from an orthogonal before onInitialize");
   }
   else
   {
@@ -177,10 +173,7 @@ public:
     //     return nullptr;
     // }
 
-    RCLCPP_INFO(
-      getLogger(), "[%s] Creating client object, type:'%s' object tag: '%s'",
-      demangleType(typeid(*this)).c_str(), demangledTypeName<TClient>().c_str(),
-      demangledTypeName<TOrthogonal>().c_str());
+    LOG_INFO("[{}] Creating client object, type:'{}' object tag: '{}'", demangleType(typeid(*this)).c_str(), demangledTypeName<TClient>().c_str(), demangledTypeName<TOrthogonal>().c_str());
 
     auto client = std::make_shared<ClientHandler<TOrthogonal, TClient>>(args...);
     this->template assignClientToOrthogonal<TOrthogonal, TClient>(client.get());

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_state_impl.hpp

@@ -20,6 +20,7 @@
 
 #pragma once
 #include <smacc2/introspection/introspection.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_client_behavior.hpp>
 #include <smacc2/smacc_orthogonal.hpp>
 #include <smacc2/smacc_state.hpp>
@@ -37,8 +38,7 @@ template <typename TOrthogonal, typename TBehavior, typename... Args>
 std::shared_ptr<TBehavior> ISmaccState::configure(Args &&... args)
 {
   std::string orthogonalkey = demangledTypeName<TOrthogonal>();
-  RCLCPP_INFO(
-    getLogger(), "[%s] Configuring orthogonal: %s", THIS_STATE_NAME, orthogonalkey.c_str());
+  LOG_INFO("[{}] Configuring orthogonal: {}", THIS_STATE_NAME, orthogonalkey.c_str());
 
   TOrthogonal * orthogonal = this->getOrthogonal<TOrthogonal>();
   if (orthogonal != nullptr)
@@ -53,9 +53,7 @@ std::shared_ptr<TBehavior> ISmaccState::configure(Args &&... args)
   }
   else
   {
-    RCLCPP_ERROR(
-      getLogger(), "[%s] Skipping client behavior creation in orthogonal [%s]. It does not exist.",
-      THIS_STATE_NAME, orthogonalkey.c_str());
+    LOG_ERROR("[{}] Skipping client behavior creation in orthogonal [{}]. It does not exist.", THIS_STATE_NAME, orthogonalkey.c_str());
     return nullptr;
   }
 }
@@ -80,11 +78,8 @@ void ISmaccState::requiresClient(SmaccClientType *& storage)
     if (storage != nullptr) return;
   }
 
-  RCLCPP_ERROR(
-    getLogger(),
-    "[%s] Client of type '%s' not found in any orthogonal of the current state machine. This may "
-    "produce a segmentation fault if the returned reference is used.",
-    sname, demangleSymbol<SmaccClientType>().c_str());
+  LOG_ERROR("[{}] Client of type '{}' not found in any orthogonal of the current state machine. This may "
+    "produce a segmentation fault if the returned reference is used.", sname, demangleSymbol<SmaccClientType>().c_str());
 }
 //-------------------------------------------------------------------------------------------------------
 

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp

@@ -21,6 +21,7 @@
 #pragma once
 
 #include <memory>
+#include "hivecore_logger/logger.hpp"
 #include <sstream>
 #include <string>
 
@@ -63,12 +64,9 @@ TOrthogonal * ISmaccStateMachine::getOrthogonal()
 
   if (it != orthogonals_.end())
   {
-    RCLCPP_DEBUG(
-      getLogger(),
-      "Orthogonal %s resource is being required from some state, client or component. Found "
+    LOG_DEBUG("Orthogonal {} resource is being required from some state, client or component. Found "
       "resource in "
-      "cache.",
-      orthogonalkey.c_str());
+      "cache.", orthogonalkey.c_str());
     ret = dynamic_cast<TOrthogonal *>(it->second.get());
     return ret;
   }
@@ -82,7 +80,7 @@ TOrthogonal * ISmaccStateMachine::getOrthogonal()
       ss << " - " << orthogonal.first << std::endl;
     }
 
-    RCLCPP_WARN_STREAM(getLogger(), ss.str());
+    LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << ss.str(); return _oss.str(); }());
 
     return nullptr;
   }
@@ -104,20 +102,19 @@ void ISmaccStateMachine::createOrthogonal()
 
     ret->setStateMachine(this);
 
-    RCLCPP_INFO(getLogger(), "%s Orthogonal is created", orthogonalkey.c_str());
+    LOG_INFO("{} Orthogonal is created", orthogonalkey.c_str());
   }
   else
   {
-    RCLCPP_WARN_STREAM(
-      getLogger(), "There were already one existing orthogonal of type "
-                     << orthogonalkey.c_str() << ". Skipping creation orthogonal request. ");
+    LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "There were already one existing orthogonal of type "
+                     << orthogonalkey.c_str() << ". Skipping creation orthogonal request. "; return _oss.str(); }());
     std::stringstream ss;
     ss << "The existing orthogonals are the following: " << std::endl;
     for (auto & orthogonal : orthogonals_)
     {
       ss << " - " << orthogonal.first << std::endl;
     }
-    RCLCPP_WARN_STREAM(getLogger(), ss.str());
+    LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << ss.str(); return _oss.str(); }());
   }
   //this->unlockStateMachine("create orthogonal");
 }
@@ -126,9 +123,7 @@ void ISmaccStateMachine::createOrthogonal()
 template <typename SmaccComponentType>
 void ISmaccStateMachine::requiresComponent(SmaccComponentType *& storage, bool throwsException)
 {
-  RCLCPP_DEBUG(
-    getLogger(), "component %s is required",
-    demangleSymbol(typeid(SmaccComponentType).name()).c_str());
+  LOG_DEBUG("component {} is required", demangleSymbol(typeid(SmaccComponentType).name()).c_str());
   std::lock_guard<std::recursive_mutex> lock(m_mutex_);
 
   for (auto ortho : this->orthogonals_)
@@ -143,9 +138,7 @@ void ISmaccStateMachine::requiresComponent(SmaccComponentType *& storage, bool t
     }
   }
 
-  RCLCPP_WARN(
-    getLogger(), "component %s is required but it was not found in any orthogonal",
-    demangleSymbol(typeid(SmaccComponentType).name()).c_str());
+  LOG_WARN("component {} is required but it was not found in any orthogonal", demangleSymbol(typeid(SmaccComponentType).name()).c_str());
 
   if (throwsException)
     throw std::runtime_error("component is required but it was not found in any orthogonal");
@@ -188,10 +181,8 @@ void ISmaccStateMachine::postEvent(EventType * ev, EventLifeTime evlifetime)
     (stateMachineCurrentAction == StateMachineInternalAction::STATE_EXITING ||
      stateMachineCurrentAction == StateMachineInternalAction::TRANSITIONING))
   {
-    RCLCPP_WARN_STREAM(
-      getLogger(),
-      "[ISmaccStateMachine] CURRENT STATE SCOPED EVENT DISCARDED, state is exiting/transitioning "
-        << eventtypename);
+    LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ISmaccStateMachine] CURRENT STATE SCOPED EVENT DISCARDED, state is exiting/transitioning "
+        << eventtypename; return _oss.str(); }());
     return;
     // in this case we may lose/skip events, if this is not right for some cases we should create a
     // queue to lock the events during the transitions. This issues appeared when a client
@@ -203,7 +194,7 @@ void ISmaccStateMachine::postEvent(EventType * ev, EventLifeTime evlifetime)
   // we reach this place. Now, we propagate the events to all the state state reactors to generate
   // some more events
 
-  RCLCPP_DEBUG_STREAM(getLogger(), "[PostEvent entry point] " << eventtypename);
+  LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[PostEvent entry point] " << eventtypename; return _oss.str(); }());
   for (auto currentState : currentState_)
   {
     propagateEventToStateReactors(currentState, ev);
@@ -216,7 +207,7 @@ template <typename EventType>
 void ISmaccStateMachine::postEvent(EventLifeTime evlifetime)
 {
   auto evname = smacc2::introspection::demangleSymbol<EventType>();
-  RCLCPP_INFO_STREAM(getLogger(), "Event: " << evname);
+  LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Event: " << evname; return _oss.str(); }());
   auto * ev = new EventType();
   this->postEvent(ev, evlifetime);
 }
@@ -247,7 +238,7 @@ bool ISmaccStateMachine::getGlobalSMData(std::string name, T & ret)
     }
     catch (boost::bad_any_cast & ex)
     {
-      RCLCPP_ERROR(getLogger(), "bad any cast: %s", ex.what());
+      LOG_ERROR("bad any cast: {}", ex.what());
       success = false;
     }
   }
@@ -282,9 +273,7 @@ void ISmaccStateMachine::mapBehavior()
 {
   std::string stateFieldName = demangleSymbol(typeid(StateField).name());
   std::string behaviorType = demangleSymbol(typeid(BehaviorType).name());
-  RCLCPP_INFO(
-    getLogger(), "Mapping state field '%s' to stateReactor '%s'", stateFieldName.c_str(),
-    behaviorType.c_str());
+  LOG_INFO("Mapping state field '{}' to stateReactor '{}'", stateFieldName.c_str(), behaviorType.c_str());
   SmaccClientBehavior * globalreference;
   if (!this->getGlobalSMData(stateFieldName, globalreference))
   {
@@ -433,12 +422,8 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
     std::is_base_of<ISmaccOrthogonal, TSmaccObjectType>::value ||
     std::is_base_of<ISmaccStateMachine, TSmaccObjectType>::value)
   {
-    RCLCPP_INFO(
-      getLogger(),
-      "[StateMachine] Long life-time SMACC signal subscription created. Subscriber is %s. Callback "
-      "is: %s",
-      demangledTypeName<TSmaccObjectType>().c_str(),
-      demangleSymbol(typeid(callback).name()).c_str());
+    LOG_INFO("[StateMachine] Long life-time SMACC signal subscription created. Subscriber is {}. Callback "
+      "is: {}", demangledTypeName<TSmaccObjectType>().c_str(), demangleSymbol(typeid(callback).name()).c_str());
 
     connection = binder.bindaux(signal, callback, object, nullptr);
   }
@@ -447,10 +432,7 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
     std::is_base_of<StateReactor, TSmaccObjectType>::value ||
     std::is_base_of<ISmaccClientBehavior, TSmaccObjectType>::value)
   {
-    RCLCPP_INFO(
-      getLogger(),
-      "[StateMachine] Life-time constrained SMACC signal subscription created. Subscriber is %s",
-      demangledTypeName<TSmaccObjectType>().c_str());
+    LOG_INFO("[StateMachine] Life-time constrained SMACC signal subscription created. Subscriber is {}", demangledTypeName<TSmaccObjectType>().c_str());
 
     std::shared_ptr<CallbackCounterSemaphore> callbackCounterSemaphore;
     if (stateCallbackConnections.count(object))
@@ -469,9 +451,7 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
   }
   else  // state life-time objects
   {
-    RCLCPP_WARN(
-      getLogger(),
-      "[StateMachine] Connecting signal to an unknown object with unknown lifetime "
+    LOG_WARN("[StateMachine] Connecting signal to an unknown object with unknown lifetime "
       "behavior. An exception may occur if the object is destroyed during the execution.");
 
     connection = binder.bindaux(signal, callback, object, nullptr);
@@ -485,11 +465,8 @@ void ISmaccStateMachine::notifyOnStateEntryStart(StateType * state)
 {
   std::lock_guard<std::recursive_mutex> lock(m_mutex_);
 
-  RCLCPP_DEBUG(
-    getLogger(),
-    "[State Machine] Initializing a new state '%s' and updating current state. Getting state "
-    "meta-information. number of orthogonals: %ld",
-    demangleSymbol(typeid(StateType).name()).c_str(), this->orthogonals_.size());
+  LOG_DEBUG("[State Machine] Initializing a new state '{}' and updating current state. Getting state "
+    "meta-information. number of orthogonals: {}", demangleSymbol(typeid(StateType).name()).c_str(), this->orthogonals_.size());
 
   stateSeqCounter_++;
   currentState_.push_back(state);
@@ -499,14 +476,12 @@ void ISmaccStateMachine::notifyOnStateEntryStart(StateType * state)
 template <typename StateType>
 void ISmaccStateMachine::notifyOnStateEntryEnd(StateType *)
 {
-  RCLCPP_INFO(
-    getLogger(), "[%s] State OnEntry code finished.",
-    demangleSymbol(typeid(StateType).name()).c_str());
+  LOG_INFO("[{}] State OnEntry code finished.", demangleSymbol(typeid(StateType).name()).c_str());
 
   auto currentState = this->currentState_.back();
   for (auto pair : this->orthogonals_)
   {
-    RCLCPP_DEBUG(getLogger(), "Orthogonal onEntry: %s.", pair.second->getName().c_str());
+    LOG_DEBUG("Orthogonal onEntry: {}.", pair.second->getName().c_str());
     auto & orthogonal = pair.second;
     try
     {
@@ -514,46 +489,37 @@ void ISmaccStateMachine::notifyOnStateEntryEnd(StateType *)
     }
     catch (const std::exception & e)
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[Orthogonal %s] Exception on Entry - continuing with next orthogonal. Exception info: %s",
-        pair.second->getName().c_str(), e.what());
+      LOG_ERROR("[Orthogonal {}] Exception on Entry - continuing with next orthogonal. Exception info: {}", pair.second->getName().c_str(), e.what());
     }
   }
 
   for (auto & sr : currentState->getStateReactors())
   {
     auto srname = smacc2::demangleSymbol(typeid(*sr).name());
-    RCLCPP_INFO_STREAM(getLogger(), "State reactor onEntry: " << srname);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "State reactor onEntry: " << srname; return _oss.str(); }());
     try
     {
       sr->onEntry();
     }
     catch (const std::exception & e)
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[State Reactor %s] Exception on Entry - continuing with next state reactor. Exception "
-        "info: %s",
-        srname.c_str(), e.what());
+      LOG_ERROR("[State Reactor {}] Exception on Entry - continuing with next state reactor. Exception "
+        "info: {}", srname.c_str(), e.what());
     }
   }
 
   for (auto & eg : currentState->getEventGenerators())
   {
     auto egname = smacc2::demangleSymbol(typeid(*eg).name());
-    RCLCPP_INFO_STREAM(getLogger(), "Event generator onEntry: " << egname);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Event generator onEntry: " << egname; return _oss.str(); }());
     try
     {
       eg->onEntry();
     }
     catch (const std::exception & e)
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[Event generator %s] Exception on Entry - continuing with next state reactor. Exception "
-        "info: %s",
-        egname.c_str(), e.what());
+      LOG_ERROR("[Event generator {}] Exception on Entry - continuing with next state reactor. Exception "
+        "info: {}", egname.c_str(), e.what());
     }
   }
 
@@ -594,10 +560,10 @@ void ISmaccStateMachine::notifyOnStateExitting(StateType * state)
 {
   stateMachineCurrentAction = StateMachineInternalAction::STATE_EXITING;
   auto fullname = demangleSymbol(typeid(StateType).name());
-  RCLCPP_WARN_STREAM(getLogger(), "Exiting state: " << fullname);
+  LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Exiting state: " << fullname; return _oss.str(); }());
   // this->set_parameter("destroyed", true);
 
-  RCLCPP_INFO_STREAM(getLogger(), "Notification state exit: leaving state " << state);
+  LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Notification state exit: leaving state " << state; return _oss.str(); }());
   for (auto pair : this->orthogonals_)
   {
     auto & orthogonal = pair.second;
@@ -607,46 +573,37 @@ void ISmaccStateMachine::notifyOnStateExitting(StateType * state)
     }
     catch (const std::exception & e)
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[Orthogonal %s] Exception onExit - continuing with next orthogonal. Exception info: %s",
-        pair.second->getName().c_str(), e.what());
+      LOG_ERROR("[Orthogonal {}] Exception onExit - continuing with next orthogonal. Exception info: {}", pair.second->getName().c_str(), e.what());
     }
   }
 
   for (auto & sr : state->getStateReactors())
   {
     auto srname = smacc2::demangleSymbol(typeid(*sr).name());
-    RCLCPP_INFO_STREAM(getLogger(), "State reactor OnExit: " << srname);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "State reactor OnExit: " << srname; return _oss.str(); }());
     try
     {
       sr->onExit();
     }
     catch (const std::exception & e)
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
-        "info: %s",
-        srname.c_str(), e.what());
+      LOG_ERROR("[State Reactor {}] Exception on OnExit - continuing with next state reactor. Exception "
+        "info: {}", srname.c_str(), e.what());
     }
   }
 
   for (auto & eg : state->getEventGenerators())
   {
     auto egname = smacc2::demangleSymbol(typeid(*eg).name());
-    RCLCPP_INFO_STREAM(getLogger(), "Event generator OnExit: " << egname);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Event generator OnExit: " << egname; return _oss.str(); }());
     try
     {
       eg->onExit();
     }
     catch (const std::exception & e)
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
-        "info: %s",
-        egname.c_str(), e.what());
+      LOG_ERROR("[State Reactor {}] Exception on OnExit - continuing with next state reactor. Exception "
+        "info: {}", egname.c_str(), e.what());
     }
   }
 }
@@ -659,9 +616,9 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
   signalDetector_->notifyStateExited(state);
 
   auto fullname = demangleSymbol(typeid(StateType).name());
-  RCLCPP_WARN_STREAM(getLogger(), "Exiting state: " << fullname);
+  LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Exiting state: " << fullname; return _oss.str(); }());
 
-  RCLCPP_INFO_STREAM(getLogger(), "Notification state disposing: leaving state" << state);
+  LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Notification state disposing: leaving state" << state; return _oss.str(); }());
   for (auto pair : this->orthogonals_)
   {
     auto & orthogonal = pair.second;
@@ -671,46 +628,37 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
     }
     catch (const std::exception & e)
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[Orthogonal %s] Exception onDispose - continuing with next orthogonal. Exception info: %s",
-        pair.second->getName().c_str(), e.what());
+      LOG_ERROR("[Orthogonal {}] Exception onDispose - continuing with next orthogonal. Exception info: {}", pair.second->getName().c_str(), e.what());
     }
   }
 
   for (auto & sr : state->getStateReactors())
   {
     auto srname = smacc2::demangleSymbol(typeid(*sr).name()).c_str();
-    RCLCPP_INFO(getLogger(), "State reactor disposing: %s", srname);
+    LOG_INFO("State reactor disposing: {}", srname);
     try
     {
       this->disconnectSmaccSignalObject(sr.get());
     }
     catch (const std::exception & e)
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
-        "info: %s",
-        srname, e.what());
+      LOG_ERROR("[State Reactor {}] Exception on OnDispose - continuing with next state reactor. Exception "
+        "info: {}", srname, e.what());
     }
   }
 
   for (auto & eg : state->getEventGenerators())
   {
     auto egname = smacc2::demangleSymbol(typeid(*eg).name()).c_str();
-    RCLCPP_INFO(getLogger(), "Event generator disposing: %s", egname);
+    LOG_INFO("Event generator disposing: {}", egname);
     try
     {
       this->disconnectSmaccSignalObject(eg.get());
     }
     catch (const std::exception & e)
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
-        "info: %s",
-        egname, e.what());
+      LOG_ERROR("[State Reactor {}] Exception on OnDispose - continuing with next state reactor. Exception "
+        "info: {}", egname, e.what());
     }
   }
 
@@ -718,7 +666,7 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
   currentState_.pop_back();
 
   // then call exit state
-  RCLCPP_WARN_STREAM(getLogger(), "State exit: " << fullname);
+  LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "State exit: " << fullname; return _oss.str(); }());
 
   stateMachineCurrentAction = StateMachineInternalAction::TRANSITIONING;
   this->unlockStateMachine("state exit");
@@ -727,9 +675,7 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
 template <typename EventType>
 void ISmaccStateMachine::propagateEventToStateReactors(ISmaccState * st, EventType * ev)
 {
-  RCLCPP_DEBUG(
-    getLogger(), "PROPAGATING EVENT [%s] TO SRs [%s]: ", demangleSymbol<EventType>().c_str(),
-    st->getClassName().c_str());
+  LOG_DEBUG("PROPAGATING EVENT [{}] TO SRs [{}]: ", demangleSymbol<EventType>().c_str(), st->getClassName().c_str());
   for (auto & sb : st->getStateReactors())
   {
     sb->notifyEvent(ev);

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/impl/smacc_state_reactor_impl.hpp

@@ -20,6 +20,8 @@
 
 #pragma once
 #include <smacc2/introspection/introspection.hpp>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_state_reactor.hpp>
 
 namespace smacc2
@@ -41,8 +43,7 @@ void StateReactor::setOutputEvent()
 {
   this->postEventFn = [this]()
   {
-    RCLCPP_INFO_STREAM(
-      this->getLogger(), "[State Reactor Base] postingfn posting event: " << demangleSymbol<TEv>());
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[State Reactor Base] postingfn posting event: " << demangleSymbol<TEv>(); return _oss.str(); }());
     auto * ev = new TEv();
     this->ownerState->getStateMachine().postEvent(ev);
   };
@@ -84,9 +85,7 @@ void StateReactorHandler::addInputEvent()
   StateReactorCallbackFunctor functor;
   functor.fn = [this](std::shared_ptr<smacc2::StateReactor> sr)
   {
-    RCLCPP_INFO(
-      nh_->get_logger(), "[%s] State Reactor adding input event: %s",
-      srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
+    LOG_INFO("[{}] State Reactor adding input event: {}", srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
     sr->addInputEvent<TEv>();
   };
 
@@ -105,9 +104,7 @@ void StateReactorHandler::setOutputEvent()
   StateReactorCallbackFunctor functor;
   functor.fn = [this](std::shared_ptr<smacc2::StateReactor> sr)
   {
-    RCLCPP_INFO(
-      nh_->get_logger(), "[%s] State Reactor setting output event: %s",
-      srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
+    LOG_INFO("[{}] State Reactor setting output event: {}", srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
     sr->setOutputEvent<TEv>();
   };
 

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/introspection/smacc_state_machine_info.hpp

@@ -21,6 +21,8 @@
 #pragma once
 
 #include <map>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <memory>
 #include <string>
 #include <vector>
@@ -160,9 +162,7 @@ template <typename T>
 typename enable_if<boost::mpl::is_sequence<T>>::type processTransitions(
   std::shared_ptr<SmaccStateInfo> & sourceState)
 {
-  RCLCPP_INFO(
-    globalNh_->get_logger(), "State %s Walker has transition list",
-    sourceState->fullStateName.c_str());
+  LOG_INFO("State {} Walker has transition list", sourceState->fullStateName.c_str());
   using boost::mpl::_1;
   using wrappedList = typename boost::mpl::transform<T, add_type_wrapper<_1>>::type;
   boost::mpl::for_each<wrappedList>(AddTransition(sourceState));
@@ -184,9 +184,7 @@ void processTransition(
   smacc2::Transition<Ev, Dst, Tag> * t, std::shared_ptr<SmaccStateInfo> & sourceState)
 {
   auto transitionTypeInfo = TypeInfo::getTypeInfoFromType<smacc2::Transition<Ev, Dst, Tag>>();
-  RCLCPP_INFO(
-    globalNh_->get_logger(), "State %s Walker transition: %s", sourceState->toShortName().c_str(),
-    demangleSymbol(typeid(Ev).name()).c_str());
+  LOG_INFO("State {} Walker transition: {}", sourceState->toShortName().c_str(), demangleSymbol(typeid(Ev).name()).c_str());
   processTransitionAux(t, sourceState, false, transitionTypeInfo);
 }
 
@@ -195,9 +193,7 @@ void processTransitionAux(
   smacc2::Transition<Ev, Dst, Tag> *, std::shared_ptr<SmaccStateInfo> & sourceState, bool history,
   TypeInfo::Ptr & transitionTypeInfo)
 {
-  RCLCPP_INFO(
-    globalNh_->get_logger(), "State %s Walker transition: %s", sourceState->toShortName().c_str(),
-    demangleSymbol(typeid(Ev).name()).c_str());
+  LOG_INFO("State {} Walker transition: {}", sourceState->toShortName().c_str(), demangleSymbol(typeid(Ev).name()).c_str());
   std::string transitionTag;
   std::string transitionType;
 
@@ -205,7 +201,7 @@ void processTransitionAux(
   {
     transitionTag = demangleSymbol<Tag>();
     transitionType = getTransitionType<Tag>();
-    RCLCPP_DEBUG_STREAM(globalNh_->get_logger(), "TRANSITION TYPE:" << transitionType);
+    LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "TRANSITION TYPE:" << transitionType; return _oss.str(); }());
   }
   else
   {
@@ -214,7 +210,7 @@ void processTransitionAux(
     automaticTransitionType<Ev>(transitionType);
   }
 
-  RCLCPP_INFO_STREAM(globalNh_->get_logger(), "Transition tag: " << transitionTag);
+  LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Transition tag: " << transitionTag; return _oss.str(); }());
 
   if (!sourceState->stateMachine_->containsState<Dst>())
   {
@@ -350,17 +346,14 @@ template <typename T>
 typename std::enable_if<HasOnDefinition<T>::value, void>::type CallOnDefinition()
 {
   /* something when T has toString ... */
-  RCLCPP_INFO_STREAM(
-    globalNh_->get_logger(), "EXECUTING ONDEFINITION: " << demangleSymbol(typeid(T).name()));
+  LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "EXECUTING ONDEFINITION: " << demangleSymbol(typeid(T).name()); return _oss.str(); }());
   T::staticConfigure();
 }
 
 template <typename T>
 typename std::enable_if<!HasOnDefinition<T>::value, void>::type CallOnDefinition()
 {
-  RCLCPP_INFO_STREAM(
-    globalNh_->get_logger(),
-    "static OnDefinition: dont exist for " << demangleSymbol(typeid(T).name()));
+  LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "static OnDefinition: dont exist for " << demangleSymbol(typeid(T).name()); return _oss.str(); }());
   /* something when T has toString ... */
 }
 
@@ -371,7 +364,7 @@ template <typename> typename EvType, typename Tag, typename DestinyState >
 typename disable_if<boost::mpl::is_sequence<TTransition<EvType<TevSource>,DestinyState, Tag>>>::type
 processTransitions(std::shared_ptr<SmaccStateInfo> &sourceState)
 {
-    RCLCPP_INFO(getLogger(),"DETECTED COMPLEX TRANSITION **************");
+    LOG_INFO("DETECTED COMPLEX TRANSITION **************");
     // RCLCPP_INFO_STREAM(getLogger(),"state transition from: " << sourceState->demangledStateName
     << " of type: " << demangledTypeName<T>());
     TTransition<EvType<TevSource>,DestinyState, Tag> *dummy;
@@ -383,7 +376,7 @@ template <typename> typename EvType, typename DestinyState >
 typename disable_if<boost::mpl::is_sequence<TTransition<EvType<TevSource>,DestinyState>>>::type
 processTransitions(std::shared_ptr<SmaccStateInfo> &sourceState)
 {
-    RCLCPP_INFO(getLogger(),"DETECTED COMPLEX TRANSITION **************");
+    LOG_INFO("DETECTED COMPLEX TRANSITION **************");
     // RCLCPP_INFO_STREAM(getLogger(),"state transition from: " << sourceState->demangledStateName
     << " of type: " << demangledTypeName<T>());
     TTransition<EvType<TevSource>,DestinyState> *dummy;
@@ -480,7 +473,7 @@ std::shared_ptr<SmaccStateInfo> SmaccStateMachineInfo::createState(
   auto * statetid = &(typeid(StateType));
 
   auto demangledName = demangledTypeName<StateType>();
-  RCLCPP_INFO_STREAM(getLogger(), "Creating State Info: " << demangledName);
+  LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Creating State Info: " << demangledName; return _oss.str(); }());
 
   auto state = std::make_shared<SmaccStateInfo>(statetid, parent, thisptr);
   state->demangledStateName = demangledName;
@@ -510,8 +503,7 @@ std::shared_ptr<SmaccStateInfo> SmaccStateInfo::createChildState()
 
   auto childState = this->stateMachine_->createState<StateType>(realparentState);
 
-  RCLCPP_WARN_STREAM(
-    getLogger(), "Real parent state> " << demangleSymbol<typename StateType::TContext>());
+  LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Real parent state> " << demangleSymbol<typename StateType::TContext>(); return _oss.str(); }());
 
   /*auto contextInfo = TypeInfo::getTypeInfoFromType<InitialStateType>();
     auto parentState2= getState<InitialStateType::TContext>();

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/smacc_state_base.hpp

@@ -20,6 +20,8 @@
 
 #pragma once
 #include <smacc2/introspection/state_traits.hpp>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/smacc_event_generator.hpp>
 #include <smacc2/smacc_state.hpp>
 #include <smacc2/smacc_state_machine.hpp>
@@ -76,7 +78,7 @@ public:
     logger_.reset(new rclcpp::Logger(
       rclcpp::get_logger(smacc2::utils::cleanShortTypeName(typeid(MostDerived)))));
 
-    RCLCPP_INFO(getLogger(), "[%s] Creating state ", STATE_NAME);
+    LOG_INFO("[{}] Creating state ", STATE_NAME);
     this->set_context(ctx.pContext_);
 
     node_ = this->getStateMachine().getNode();
@@ -345,7 +347,7 @@ public:
     {
       this->postEvent<EvLoopEnd<MostDerived>>();
     }
-    RCLCPP_INFO(getLogger(), "[%s] POST THROW CONDITION", STATE_NAME);
+    LOG_INFO("[{}] POST THROW CONDITION", STATE_NAME);
   }
 
   void throwSequenceFinishedEvent() { this->postEvent<EvSequenceFinished<MostDerived>>(); }
@@ -406,38 +408,32 @@ private:
       SmaccStateInfo::staticBehaviorInfo[tindex] = std::vector<ClientBehaviorInfoEntry>();
 
     SmaccStateInfo::staticBehaviorInfo[tindex].push_back(bhinfo);
-    RCLCPP_INFO_STREAM(
-      rclcpp::get_logger("static"), "[states walking] State "
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[states walking] State "
                                       << smacc2::utils::cleanShortTypeName(*tindex)
                                       << "client behavior count: "
-                                      << SmaccStateInfo::staticBehaviorInfo[tindex].size());
+                                      << SmaccStateInfo::staticBehaviorInfo[tindex].size(); return _oss.str(); }());
   }
 
   void entryStateInternal()
   {
     // finally we go to the derived state onEntry Function
 
-    RCLCPP_DEBUG(getLogger(), "[%s] State object created. Initializating...", STATE_NAME);
+    LOG_DEBUG("[{}] State object created. Initializating...", STATE_NAME);
     this->getStateMachine().notifyOnStateEntryStart(static_cast<MostDerived *>(this));
 
-    RCLCPP_DEBUG_STREAM(
-      getLogger(), "[" << smacc2::utils::cleanShortTypeName(typeid(MostDerived)).c_str()
-                       << "] creating ros subnode");
+    LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << smacc2::utils::cleanShortTypeName(typeid(MostDerived)).c_str()
+                       << "] creating ros subnode"; return _oss.str(); }());
 
     // before dynamic runtimeConfigure, we execute the staticConfigure behavior configurations
     {
-      RCLCPP_DEBUG(getLogger(), "[%s] -- STATIC STATE DESCRIPTION --", STATE_NAME);
+      LOG_DEBUG("[{}] -- STATIC STATE DESCRIPTION --", STATE_NAME);
 
       for (const auto & clientBehavior : SmaccStateInfo::staticBehaviorInfo)
       {
-        RCLCPP_DEBUG(
-          getLogger(), "[%s] client behavior info: %s", STATE_NAME,
-          demangleSymbol(clientBehavior.first->name()).c_str());
+        LOG_DEBUG("[{}] client behavior info: {}", STATE_NAME, demangleSymbol(clientBehavior.first->name()).c_str());
         for (auto & cbinfo : clientBehavior.second)
         {
-          RCLCPP_DEBUG(
-            getLogger(), "[%s] client behavior: %s", STATE_NAME,
-            demangleSymbol(cbinfo.behaviorType->name()).c_str());
+          LOG_DEBUG("[{}] client behavior: {}", STATE_NAME, demangleSymbol(cbinfo.behaviorType->name()).c_str());
         }
       }
 
@@ -448,45 +444,36 @@ private:
 
       for (auto & ortho : this->getStateMachine().getOrthogonals())
       {
-        RCLCPP_INFO(
-          getLogger(), "[%s] Initializing orthogonal: %s", STATE_NAME,
-          demangleSymbol(typeid(*ortho.second).name()).c_str());
+        LOG_INFO("[{}] Initializing orthogonal: {}", STATE_NAME, demangleSymbol(typeid(*ortho.second).name()).c_str());
         ortho.second->initState(this);
       }
 
-      RCLCPP_DEBUG_STREAM(
-        getLogger(), "Finding static client behaviors. State Database: "
+      LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Finding static client behaviors. State Database: "
                        << SmaccStateInfo::staticBehaviorInfo.size() << ". Current state "
                        << cleanShortTypeName(*tindex)
-                       << " cbs: " << SmaccStateInfo::staticBehaviorInfo[tindex].size());
+                       << " cbs: " << SmaccStateInfo::staticBehaviorInfo[tindex].size(); return _oss.str(); }());
       for (auto & bhinfo : staticDefinedBehaviors)
       {
-        RCLCPP_DEBUG(
-          getLogger(), "[%s] Creating static client behavior: %s", STATE_NAME,
-          demangleSymbol(bhinfo.behaviorType->name()).c_str());
+        LOG_DEBUG("[{}] Creating static client behavior: {}", STATE_NAME, demangleSymbol(bhinfo.behaviorType->name()).c_str());
         bhinfo.factoryFunction(this);
       }
 
       for (auto & sr : staticDefinedStateReactors)
       {
-        RCLCPP_DEBUG(
-          getLogger(), "[%s] Creating static state reactor: %s", STATE_NAME,
-          sr->stateReactorType->getFullName().c_str());
+        LOG_DEBUG("[{}] Creating static state reactor: {}", STATE_NAME, sr->stateReactorType->getFullName().c_str());
         sr->factoryFunction(this);
       }
 
       for (auto & eg : staticDefinedEventGenerators)
       {
-        RCLCPP_DEBUG(
-          getLogger(), "[%s] Creating static event generator: %s", STATE_NAME,
-          eg->eventGeneratorType->getFullName().c_str());
+        LOG_DEBUG("[{}] Creating static event generator: {}", STATE_NAME, eg->eventGeneratorType->getFullName().c_str());
         eg->factoryFunction(this);
       }
 
-      RCLCPP_DEBUG(getLogger(), "[%s] ---- END STATIC DESCRIPTION", STATE_NAME);
+      LOG_DEBUG("[{}] ---- END STATIC DESCRIPTION", STATE_NAME);
     }
 
-    RCLCPP_DEBUG(getLogger(), "[%s] State runtime configuration", STATE_NAME);
+    LOG_DEBUG("[{}] State runtime configuration", STATE_NAME);
 
     auto * derivedthis = static_cast<MostDerived *>(this);
 
@@ -500,7 +487,7 @@ private:
 
     this->getStateMachine().notifyOnRuntimeConfigurationFinished(derivedthis);
 
-    RCLCPP_DEBUG(getLogger(), "[%s] State OnEntry", STATE_NAME);
+    LOG_DEBUG("[{}] State OnEntry", STATE_NAME);
 
     static_cast<MostDerived *>(this)->onEntry();
 

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/smacc_state_machine_base.hpp

@@ -19,6 +19,7 @@
  ******************************************************************************************************************/
 #pragma once
 #include <smacc2/common.hpp>
+#include "hivecore_logger/logger.hpp"
 
 #include <smacc2/smacc_state_base.hpp>
 #include <smacc2/smacc_state_machine.hpp>
@@ -74,15 +75,15 @@ public:
     // this is before because this creates orthogonals
     this->onInitialize();
 
-    RCLCPP_INFO(getLogger(), "[SmaccStateMachine] Introspecting state machine via typeWalker");
+    LOG_INFO("[SmaccStateMachine] Introspecting state machine via typeWalker");
     this->buildStateMachineInfo<InitialStateType>();
 
-    RCLCPP_INFO(getLogger(), "[SmaccStateMachine] initiate_impl");
+    LOG_INFO("[SmaccStateMachine] initiate_impl");
     auto shortname = smacc2::utils::cleanShortTypeName(typeid(DerivedStateMachine));
 
     this->initializeROS(shortname);
 
-    RCLCPP_INFO(getLogger(), "[SmaccStateMachine] Initializing ROS communication mechanisms");
+    LOG_INFO("[SmaccStateMachine] Initializing ROS communication mechanisms");
     this->onInitialized();
 
     // publish startup state machine transition info
@@ -90,7 +91,7 @@ public:
     transitionInfo->destinyState = this->stateMachineInfo_->getState<InitialStateType>();
     this->publishTransition(*transitionInfo);
 
-    RCLCPP_INFO(getLogger(), "[SmaccStateMachine] Initializing state machine");
+    LOG_INFO("[SmaccStateMachine] Initializing state machine");
     sc::state_machine<DerivedStateMachine, InitialStateType, SmaccAllocator>::initiate();
   }
 };

src/thirdparty/SMACC2-humble/smacc2/include/smacc2/smacc_transition.hpp

@@ -21,6 +21,7 @@
 #pragma once
 
 #include <smacc2/introspection/introspection.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/introspection/state_traits.hpp>
 namespace smacc2
 {
@@ -40,7 +41,7 @@ private:
   {
     static boost::statechart::result react_without_action(State & stt)
     {
-      RCLCPP_DEBUG(stt.getLogger(), "[Smacc Transition] REACT WITHOUT ACTION");
+      LOG_DEBUG("[Smacc Transition] REACT WITHOUT ACTION");
       typedef smacc2::Transition<Event, Destination, Tag, TransitionContext, pTransitionAction>
         Transtype;
       TRANSITION_TAG mock;
@@ -52,7 +53,7 @@ private:
 
     static boost::statechart::result react_with_action(State & stt, const Event & evt)
     {
-      RCLCPP_DEBUG(stt.getLogger(), "[Smacc Transition] REACT WITH ACTION AND EVENT");
+      LOG_DEBUG("[Smacc Transition] REACT WITH ACTION AND EVENT");
       typedef smacc2::Transition<Event, Destination, Tag, TransitionContext, pTransitionAction>
         Transtype;
       TRANSITION_TAG mock;

src/thirdparty/SMACC2-humble/smacc2/src/smacc2/callback_counter_semaphore.cpp

@@ -22,6 +22,7 @@
 #include <iostream>
 #include <mutex>
 #include <rclcpp/rclcpp.hpp>
+#include "hivecore_logger/logger.hpp"
 #include <smacc2/callback_counter_semaphore.hpp>
 #include <thread>
 
@@ -35,24 +36,18 @@ CallbackCounterSemaphore::CallbackCounterSemaphore(std::string name, int count)
 bool CallbackCounterSemaphore::acquire()
 {
   std::unique_lock<std::mutex> lock(mutex_);
-  RCLCPP_DEBUG(
-    rclcpp::get_logger(name_), "[CallbackCounterSemaphore] acquire callback %s %ld", name_.c_str(),
-    (long)this);
+  LOG_DEBUG("[CallbackCounterSemaphore] acquire callback {} {}", name_.c_str(), (long)this);
 
   if (finalized)
   {
-    RCLCPP_DEBUG(
-      rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callback rejected %s %ld",
-      name_.c_str(), (long)this);
+    LOG_DEBUG("[CallbackCounterSemaphore] callback rejected {} {}", name_.c_str(), (long)this);
     return false;
   }
 
   ++count_;
   cv_.notify_one();
 
-  RCLCPP_DEBUG(
-    rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callback accepted %s %ld", name_.c_str(),
-    (long)this);
+  LOG_DEBUG("[CallbackCounterSemaphore] callback accepted {} {}", name_.c_str(), (long)this);
   return true;
 }
 
@@ -62,9 +57,7 @@ void CallbackCounterSemaphore::release()
   --count_;
   cv_.notify_one();
 
-  RCLCPP_DEBUG(
-    rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callback finished %s %ld", name_.c_str(),
-    (long)this);
+  LOG_DEBUG("[CallbackCounterSemaphore] callback finished {} {}", name_.c_str(), (long)this);
 }
 
 void CallbackCounterSemaphore::finalize()
@@ -83,9 +76,7 @@ void CallbackCounterSemaphore::finalize()
   }
 
   connections_.clear();
-  RCLCPP_DEBUG(
-    rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callbacks finalized %s %ld",
-    name_.c_str(), (long)this);
+  LOG_DEBUG("[CallbackCounterSemaphore] callbacks finalized {} {}", name_.c_str(), (long)this);
 }
 
 void CallbackCounterSemaphore::addConnection(boost::signals2::connection conn)
@@ -94,10 +85,7 @@ void CallbackCounterSemaphore::addConnection(boost::signals2::connection conn)
 
   if (finalized)
   {
-    RCLCPP_DEBUG(
-      rclcpp::get_logger(name_),
-      "[CallbackCounterSemaphore] ignoring adding callback, already finalized %s %ld",
-      name_.c_str(), (long)this);
+    LOG_DEBUG("[CallbackCounterSemaphore] ignoring adding callback, already finalized {} {}", name_.c_str(), (long)this);
     return;
   }
 

src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_bases/smacc_ros_launch_client.cpp

@@ -19,6 +19,8 @@
  ******************************************************************************************************************/
 
 #include <smacc2/client_bases/smacc_ros_launch_client.hpp>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 
 namespace smacc2
 {
@@ -46,8 +48,7 @@ std::future<std::string> ClRosLaunch::executeRosLaunch(
     std::launch::async,
     [=]()
     {
-      RCLCPP_WARN_STREAM(
-        rclcpp::get_logger("smacc2"), "[ClRosLaunch static] starting ros launch thread ");
+      LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch static] starting ros launch thread "; return _oss.str(); }());
 
       std::stringstream cmd;
       cmd << "ros2 launch " << packageName << " " << launchFileName;
@@ -70,8 +71,7 @@ std::future<std::string> ClRosLaunch::executeRosLaunch(
           ss << buffer.data();
           if (cancelled)  // break_pipe
           {
-            RCLCPP_WARN_STREAM(
-              rclcpp::get_logger("smacc2"), "[ClRosLaunch static] cancelling ros launch thread ");
+            LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch static] cancelling ros launch thread "; return _oss.str(); }());
 
             // break pipe
             pclose(pipe.release());
@@ -86,8 +86,7 @@ std::future<std::string> ClRosLaunch::executeRosLaunch(
       }
 
       result = ss.str();
-      RCLCPP_WARN_STREAM(
-        rclcpp::get_logger("smacc2"), "[ClRosLaunch static]] RESULT = \n " << ss.str());
+      LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch static]] RESULT = \n " << ss.str(); return _oss.str(); }());
       return result;
     });
 }

src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_bases/smacc_ros_launch_client_2.cpp

@@ -18,6 +18,8 @@
  *
  ******************************************************************************************************************/
 #include <errno.h>  // Agrega esta inclusión
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 #include <fcntl.h>  // Agrega esta inclusión
 #include <poll.h>
 #include <signal.h>
@@ -75,7 +77,7 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
     std::launch::async,
     [packageName, launchFileName, cancelCondition, client]()
     {
-      RCLCPP_WARN_STREAM(rclcpp::get_logger("smacc2"), "[ClRosLaunch2] Starting ros launch thread");
+      LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch2] Starting ros launch thread"; return _oss.str(); }());
 
       std::stringstream cmd;
       cmd << "ros2 launch " << packageName << " " << launchFileName;
@@ -119,7 +121,7 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
         else
         {
           // Error de lectura
-          RCLCPP_ERROR(rclcpp::get_logger("smacc2"), "Error de lectura en pipe");
+          LOG_ERROR("Error de lectura en pipe");
           break;
         }
       }
@@ -138,25 +140,23 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
         if (WIFEXITED(status))
         {
           int exit_status = WEXITSTATUS(status);
-          RCLCPP_INFO(
-            rclcpp::get_logger("smacc2"), "Child process exited with status: %d", exit_status);
+          LOG_INFO("Child process exited with status: {}", exit_status);
         }
         else if (WIFSIGNALED(status))
         {
           int term_signal = WTERMSIG(status);
-          RCLCPP_WARN(
-            rclcpp::get_logger("smacc2"), "Child process terminated by signal: %d", term_signal);
+          LOG_WARN("Child process terminated by signal: {}", term_signal);
         }
       }
       else
       {
-        RCLCPP_ERROR(rclcpp::get_logger("smacc2"), "Error waiting for child process.");
+        LOG_ERROR("Error waiting for child process.");
       }
 
       pclose(child.pipe);
       close(child.pipe->_fileno);  // Close pipe file descriptor but not processes
 
-      RCLCPP_WARN_STREAM(rclcpp::get_logger("smacc2"), "[ClRosLaunch2] RESULT:\n" << result);
+      LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch2] RESULT:\n" << result; return _oss.str(); }());
 
       // Devuelve una std::future con el resultado
       // return std::async(std::launch::async, [result]() { return result; });
@@ -251,7 +251,7 @@ void killProcessesRecursive(pid_t pid)
   int res = kill(pid, SIGTERM);
   if (res == 0)
   {
-    RCLCPP_FATAL(rclcpp::get_logger("smacc2"), "Killed process %d", pid);
+    LOG_FATAL("Killed process {}", pid);
   }
 }
 

src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_behaviors/cb_ros_launch.cpp

@@ -18,6 +18,8 @@
  *
  ******************************************************************************************************************/
 #include <smacc2/client_behaviors/cb_ros_launch.hpp>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 
 namespace smacc2
 {
@@ -47,7 +49,7 @@ void onOrthogonalAllocation()
 
 void CbRosLaunch::onEntry()
 {
-  RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch] OnEntry");
+  LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] OnEntry"; return _oss.str(); }());
 
   std::string packageName, launchFileName;
   if (launchFileName_ && packageName_)
@@ -63,9 +65,8 @@ void CbRosLaunch::onEntry()
       breakfunction = []() -> bool { return false; };
     }
 
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[CbRosLaunch] launching: " << *packageName_ << " , " << *launchFileName_
-                                               << "LaunchMode: " << (int)launchMode_);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] launching: " << *packageName_ << " , " << *launchFileName_
+                                               << "LaunchMode: " << (int)launchMode_; return _oss.str(); }());
 
     auto fut = smacc2::client_bases::ClRosLaunch::executeRosLaunch(
       *packageName_, *launchFileName_, breakfunction);
@@ -77,22 +78,19 @@ void CbRosLaunch::onEntry()
   }
   else
   {
-    RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch] finding Ros Launch client");
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] finding Ros Launch client"; return _oss.str(); }());
 
     this->requiresClient(client_);
     if (client_ != nullptr)
     {
-      RCLCPP_INFO_STREAM(
-        getLogger(), "[CbRosLaunch] launching from client: " << client_->packageName_ << " , "
-                                                             << client_->launchFileName_);
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] launching from client: " << client_->packageName_ << " , "
+                                                             << client_->launchFileName_; return _oss.str(); }());
 
       client_->launch();
     }
     else
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[CbRosLaunch] Inccorrect ros launch operation. No Ros Launch client specified neither "
+      LOG_ERROR("[CbRosLaunch] Inccorrect ros launch operation. No Ros Launch client specified neither "
         "package/roslaunch file path.");
     }
   }

src/thirdparty/SMACC2-humble/smacc2/src/smacc2/client_behaviors/cb_ros_launch_2.cpp

@@ -18,6 +18,8 @@
  *
  ******************************************************************************************************************/
 #include <smacc2/client_behaviors/cb_ros_launch_2.hpp>
+#include <sstream>
+#include "hivecore_logger/logger.hpp"
 
 namespace smacc2
 {
@@ -48,7 +50,7 @@ void onOrthogonalAllocation()
 
 void CbRosLaunch2::onEntry()
 {
-  RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch2] OnEntry");
+  LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] OnEntry"; return _oss.str(); }());
 
   std::string packageName, launchFileName;
   if (
@@ -59,9 +61,8 @@ void CbRosLaunch2::onEntry()
 
     breakfunction = std::bind(&CbRosLaunch2::isShutdownRequested, this);
 
-    RCLCPP_INFO_STREAM(
-      getLogger(), "[CbRosLaunch2] launching: " << *packageName_ << " , " << *launchFileName_
-                                                << "LaunchMode: " << (int)launchMode_);
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] launching: " << *packageName_ << " , " << *launchFileName_
+                                                << "LaunchMode: " << (int)launchMode_; return _oss.str(); }());
 
     auto result_ = smacc2::client_bases::ClRosLaunch2::executeRosLaunch(
       // this->result_ = smacc2::client_bases::ClRosLaunch2::executeRosLaunch(
@@ -76,30 +77,25 @@ void CbRosLaunch2::onEntry()
       client_->launchFileName_ = *launchFileName_;
     }
 
-    RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch2] finding Ros Launch client");
+    LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] finding Ros Launch client"; return _oss.str(); }());
 
     // this->requiresClient(client_);
     if (client_ != nullptr)
     {
-      RCLCPP_INFO_STREAM(
-        getLogger(), "[CbRosLaunch2] launching from client: " << client_->packageName_ << " , "
-                                                              << client_->launchFileName_);
+      LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] launching from client: " << client_->packageName_ << " , "
+                                                              << client_->launchFileName_; return _oss.str(); }());
 
       client_->launch();
     }
     else
     {
-      RCLCPP_ERROR(
-        getLogger(),
-        "[CbRosLaunch2] Inccorrect ros launch operation. No Ros Launch client specified neither "
+      LOG_ERROR("[CbRosLaunch2] Inccorrect ros launch operation. No Ros Launch client specified neither "
         "package/roslaunch file path.");
     }
   }
   else
   {
-    RCLCPP_ERROR(
-      getLogger(),
-      "[CbRosLaunch2] Inccorrect ros launch operation. Not supported case. Invalid argument.");
+    LOG_ERROR("[CbRosLaunch2] Inccorrect ros launch operation. Not supported case. Invalid argument.");
   }
 }