HiveCoreRD/hivecore_robot_brain
6
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

develop #2

Closed
david wants to merge 50 commits from develop into main
pull from: develop
merge into: HiveCoreRD:main
Conversation 0 Commits 50 Files Changed 53 +13335 -1161
53 changed files with 13335 additions and 1161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
src/brain/CMakeLists.txt
View File

@@ -14,7 +14,6 @@ find_package(rclcpp_components REQUIRED)
# find_package(<dependency> REQUIRED)
find_package(interfaces REQUIRED)
find_package(brain_interfaces REQUIRED)
find_package(std_msgs REQUIRED)
find_package(smacc2 REQUIRED)
find_package(smacc2_msgs REQUIRED)
@@ -35,13 +34,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>
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
interfaces/include
)
target_compile_features(brain_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
@@ -51,7 +55,6 @@ ament_target_dependencies(brain_node
rclcpp_action
behaviortree_cpp
interfaces
brain_interfaces
std_msgs
smacc2
smacc2_msgs
@@ -68,6 +71,7 @@ ament_target_dependencies(brain_node
target_link_libraries(brain_node
Boost::thread
yaml-cpp
/usr/local/lib/libapi_cpp.so
)
install(TARGETS brain_node
@@ -76,6 +80,8 @@ install(TARGETS brain_node
install(DIRECTORY config/
DESTINATION share/${PROJECT_NAME}/config)
set (BUILD_TESTING OFF)
if(BUILD_TESTING)
if(NOT DEFINED ENV{SKIP_LINT})
find_package(ament_lint_auto REQUIRED)
@@ -103,7 +109,6 @@ if(BUILD_TESTING)
rclcpp
rclcpp_action
interfaces
brain_interfaces
nav2_msgs
std_srvs
tf2
@@ -158,16 +163,18 @@ 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
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_cerebellum_node Boost::thread)
target_link_libraries(test_cerebellum_node yaml-cpp)
target_link_libraries(test_cerebellum_node yaml-cpp /usr/local/lib/libapi_cpp.so)
endif()
ament_add_gtest(test_cerebrum_node
@@ -183,7 +190,6 @@ if(BUILD_TESTING)
rclcpp_action
behaviortree_cpp
interfaces
brain_interfaces
smacc2
smacc2_msgs
ament_index_cpp
@@ -193,9 +199,12 @@ 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
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_cerebrum_node Boost::thread)
target_link_libraries(test_cerebrum_node yaml-cpp)
target_link_libraries(test_cerebrum_node yaml-cpp /usr/local/lib/libapi_cpp.so)
endif()
ament_add_gtest(test_sm_cerebellum
@@ -208,14 +217,17 @@ 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
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_sm_cerebellum /usr/local/lib/libapi_cpp.so)
endif()
# ExecuteBtAction end-to-end single skill test
@@ -231,7 +243,6 @@ if(BUILD_TESTING)
rclcpp_action
behaviortree_cpp
interfaces
brain_interfaces
smacc2
smacc2_msgs
ament_index_cpp
@@ -241,8 +252,11 @@ if(BUILD_TESTING)
tf2_ros
tf2_geometry_msgs
)
target_include_directories(test_execute_bt_action PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp)
target_include_directories(test_execute_bt_action PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp /usr/local/lib/libapi_cpp.so)
endif()
# Extended ExecuteBtAction scenario tests (multi-skill, failure, RUN interpolation, timeout, stats, cancel)
@@ -258,7 +272,6 @@ if(BUILD_TESTING)
rclcpp_action
behaviortree_cpp
interfaces
brain_interfaces
smacc2
smacc2_msgs
ament_index_cpp
@@ -268,8 +281,11 @@ if(BUILD_TESTING)
tf2_ros
tf2_geometry_msgs
)
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)
target_include_directories(test_execute_bt_action_extended PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_execute_bt_action_extended Boost::thread yaml-cpp /usr/local/lib/libapi_cpp.so)
endif()
# Additional isolated ExecuteBtAction scenario tests
@@ -283,9 +299,12 @@ 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)
target_link_libraries(${test_name} Boost::thread yaml-cpp)
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
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(${test_name} Boost::thread yaml-cpp /usr/local/lib/libapi_cpp.so)
endif()
endforeach()
@@ -298,9 +317,12 @@ 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)
target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp)
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
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp /usr/local/lib/libapi_cpp.so)
endif()
endif()

308
src/brain/config/bt_carry_boxes_sch1.params.yaml
View File

@@ -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,32 @@
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]
'

54
src/brain/config/bt_carry_boxes_sch1.xml
View File

@@ -2,36 +2,54 @@
<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>
<!-- <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>

332
src/brain/config/bt_carry_boxes_sch2.params.yaml
View File

@@ -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,6 @@
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]
'

39
src/brain/config/bt_carry_boxes_sch2.xml
View File

@@ -2,36 +2,53 @@
<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>
<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>

216
src/brain/config/bt_vision_grasp.params.yaml
View File

@@ -6,14 +6,64 @@
params: '{}
'
- name: VisionObjectRecognition_H
params: '{}
- name: s1_move_waist_turn_right_90
params: 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- name: Arm_H
params: 'body_id: 0
- name: s1_gripper_open
params: 'loc: 0
data_type: 0
speed: 20
torque: 20
mode: 2
'
- name: s1_right_arm_pre_cam_take_photo
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
'
- name: s1_wheel_move_to_origin_pickup_position
params: 'move_distance: 0.5
move_angle: 0.0
'
- name: s2_right_arm_pre_cam_take_photo
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 60.27, -20.97, -87.36, 5.58, -68.56, 58.21]
'
- name: s2_top_camera_vision_recg
params: 'camera_position: top
'
- name: s2_right_arm_cam_take_photo
params: 'body_id: 1
data_type: 100
data_length: 0
@@ -24,9 +74,157 @@
data_array: []
'
- name: HandControl_H
params: 'mode: 0
effort: 0.0
- name: s3_right_camera_vision_recg
params: 'camera_position: right
'
- name: s3_right_arm_vision_pre_grasp
params: 'body_id: 1
data_type: 101
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_right_arm_vision_grasp
params: 'body_id: 1
data_type: 102
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_gripper_close
params: 'loc: 200
speed: 150
torque: 100
mode: 1
'
- name: s4_right_arm_take_box
params: 'body_id: 1
data_type: 110
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s5_right_arm_grasp_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, -45, -22.88, -16.30, 112.10]
'
- 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_right_arm_grasp_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
'
- name: s7_right_arm_put_down_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 85.78, -88.95, 13.97, 22.88, -16.29, 67.99]
'
- name: s8_gripper_open
params: 'loc: 0
speed: 20
torque: 20
mode: 2
'
- name: s9_right_arm_grasp_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
'
- name: s10_move_waist_turn_right_90
params: 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- name: s11_right_arm_pre_cam_take_photo
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
'

42
src/brain/config/bt_vision_grasp.xml
View File

@@ -1,15 +1,45 @@
<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">
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
<Sequence>
<VisionObjectRecognition_H name="VisionObjectRecognition_H" />
<Arm_H name="Arm_H" />
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Parallel name="parallel_action_1">
<MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
<GripperCmd_H name="s1_gripper_open" />
<Arm_H name="s2_right_arm_pre_cam_take_photo" />
</Parallel>
<RetryUntilSuccessful name="retry_camera_vision_recg_1" num_attempts="10">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
<Arm_H name="s2_right_arm_cam_take_photo" />
<RetryUntilSuccessful name="retry_camera_vision_recg_2" num_attempts="10">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd_H name="s4_gripper_close" />
<Arm_H name="s4_right_arm_take_box" />
<Arm_H name="s5_right_arm_grasp_box" />
<Parallel name="parallel_action_2">
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
<MoveWaist_H name="s6_move_waist_turn_left_90" />
</Parallel>
<Arm_H name="s7_right_arm_grasp_box" />
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd_H name="s8_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<Parallel name="parallel_action_3">
<MoveWaist_H name="s10_move_waist_turn_right_90" />
<Arm_H name="s11_right_arm_pre_cam_take_photo" />
</Parallel>
</Sequence>
</RetryUntilSuccessful>
<!-- After Arm_H eventually succeeds, run the hand -->
<HandControl_H name="HandControl_H" />
</Sequence>
</BehaviorTree>
</root>

6
src/brain/config/robot_config.yaml
View File

@@ -6,12 +6,14 @@
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.xml, param: /config/bt_vision_grasp.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.xml, param: /config/bt_vision_grasp.params.yaml}
]

9
src/brain/config/robot_skills.yaml
View File

@@ -61,4 +61,11 @@
description: "回到原点位姿"
interfaces:
- name: MoveHome.action
default_topic: "MoveHome"
default_topic: "MoveHome"
- name: GripperCmd
version: 1.0.0
description: "夹爪控制"
interfaces:
- name: GripperCmd.action
default_topic: "gripper_cmd0"

BIN
src/brain/config/voice/inFinished.mp3 Normal file
View File

Binary file not shown.

BIN
src/brain/config/voice/inScan.mp3 Normal file
View File

Binary file not shown.

BIN
src/brain/config/voice/inStart.mp3 Normal file
View File

Binary file not shown.

BIN
src/brain/config/voice/outFinished.mp3 Normal file
View File

Binary file not shown.

BIN
src/brain/config/voice/outScan.mp3 Normal file
View File

Binary file not shown.

BIN
src/brain/config/voice/outStart.mp3 Normal file
View File

Binary file not shown.

BIN
src/brain/config/voice/scanFinished.mp3 Normal file
View File

Binary file not shown.

BIN
src/brain/config/voice/scaning.mp3 Normal file
View File

Binary file not shown.

81
src/brain/include/brain/action_registry.hpp
View File

@@ -406,6 +406,87 @@ public:
return wrapped_result;
}
template<typename ActionT>
std::optional<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult> send_and_wait_with_goal(
const std::string & name,
rclcpp::Logger logger,
const typename ActionT::Goal & goal,
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());
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());
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());
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) {
RCLCPP_ERROR(logger, "Timed out waiting for goal response on '%s'", 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) {
RCLCPP_ERROR(logger, "Failed to get goal handle for '%s': %s", name.c_str(), ex.what());
return std::nullopt;
}
if (!goal_handle) {
RCLCPP_WARN(logger, "Goal rejected for '%s'", 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) {
RCLCPP_WARN(logger, "Goal response callback threw for '%s': %s", 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());
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());
}
return std::nullopt;
}
typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult wrapped_result;
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());
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());
}
}
return wrapped_result;
}
private:
rclcpp::Node * node_;
std::unordered_map<std::string, std::unique_ptr<EntryBase>> entries_;

66
src/brain/include/brain/calc_grasp_pose.hpp Normal file
View File

@@ -0,0 +1,66 @@
#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>
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
);
static int rm_arm_inverse_kinematics(
const tf2::Vector3& target_pos,
const tf2::Quaternion& target_quat,
std::vector<double>& joint_angles
);
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
);
static void rm_arm_algo_init(void);
};
} // namespace brain

35
src/brain/include/brain/cerebellum_hooks.hpp Normal file
View File

@@ -0,0 +1,35 @@
#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 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_

80
src/brain/include/brain/cerebellum_node.hpp
View File

@@ -18,8 +18,10 @@
#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"
namespace brain
{
@@ -41,7 +43,29 @@ public:
*/
explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
std::vector<geometry_msgs::msg::Pose> right_hand_take_photo_poses_;
std::vector<geometry_msgs::msg::Pose> pre_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> grasp_poses_;
std::vector<geometry_msgs::msg::Pose> right_hand_take_object_pose_;
std::vector<std::vector<double>> right_hand_take_photo_angles_;
std::vector<std::vector<double>> pre_grasp_angles_;
std::vector<std::vector<double>> grasp_angles_;
std::vector<std::vector<double>> right_hand_take_object_angles_;
std::vector<double> left_arm_joint_angles_;
std::vector<double> right_arm_joint_angles_;
std::mutex joint_state_mutex_;
std::vector<double> right_arm_take_photo_joint_angles_;
std::string camera_position_;
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_;
@@ -80,12 +104,20 @@ private:
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"};
std::vector<std::string> target_frames_{"apple", "bottle", "medicine_box"};
std::string target_frame_{"medicine_box"};
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> grab_width_;
// Current accepted sequence epoch (from client) used to filter/reject stale goals
std::atomic<uint32_t> current_epoch_{0};
@@ -103,6 +135,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,8 +148,9 @@ 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);
@@ -131,26 +166,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.
@@ -270,6 +285,8 @@ private:
const std::string & skill,
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,
@@ -291,6 +308,8 @@ private:
const std::string & skill,
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,
@@ -355,6 +374,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,6 +401,14 @@ 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,

16
src/brain/include/brain/cerebrum_node.hpp
View File

@@ -237,6 +237,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).
@@ -258,6 +270,9 @@ private:
* @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,6 +381,7 @@ 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 */

140
src/brain/include/brain/payload_converters.hpp
View File

@@ -9,6 +9,8 @@
#include "interfaces/action/arm.hpp"
#include "interfaces/action/arm_space_control.hpp"
#include "interfaces/action/camera_take_photo.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 +20,12 @@
#include "interfaces/action/move_wheel.hpp"
#include "interfaces/action/slam_mode.hpp"
#include "interfaces/action/vision_grasp_object.hpp"
#include "interfaces/srv/bt_rebuild.hpp"
#include "interfaces/srv/gripper_cmd.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/vision_object_recognition.hpp"
@@ -153,6 +159,70 @@ inline interfaces::action::MoveHome::Goal from_yaml<interfaces::action::MoveHome
return g;
}
// GripperCmd::Goal: loc, speed, torque, mode
template<>
inline interfaces::action::GripperCmd::Goal from_yaml<interfaces::action::GripperCmd::Goal>(const YAML::Node & n)
{
interfaces::action::GripperCmd::Goal g;
if (n && n.IsMap()) {
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;
}
// ExecuteBtAction::Goal: epoch, action_name, calls
template<>
inline interfaces::action::ExecuteBtAction::Goal from_yaml<interfaces::action::ExecuteBtAction::Goal>(const YAML::Node & n)
{
interfaces::action::ExecuteBtAction::Goal g;
if (n && n.IsMap()) {
if (n["epoch"]) g.epoch = n["epoch"].as<uint32_t>();
if (n["action_name"]) g.action_name = n["action_name"].as<std::string>();
// TODO: parse field `calls` of ROS type `interfaces/SkillCall[]` into g.calls (complex type)
}
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()) {
// 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>();
}
}
}
return g;
}
// VisionGraspObject::Goal: object_id
template<>
inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action::VisionGraspObject::Goal>(const YAML::Node & n)
{
interfaces::action::VisionGraspObject::Goal g;
if (n && n.IsMap()) {
if (n["object_id"]) g.object_id = n["object_id"].as<std::string>();
}
return g;
}
// MoveToPosition::Goal: target_x, target_y
template<>
inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
@@ -203,41 +273,61 @@ inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::A
return g;
}
// LegControl::Goal: target
// GripperCmd::Request: devid, loc, speed, torque
template<>
inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(const YAML::Node & n)
inline interfaces::srv::GripperCmd::Request from_yaml<interfaces::srv::GripperCmd::Request>(const YAML::Node & n)
{
interfaces::action::LegControl::Goal g;
interfaces::srv::GripperCmd::Request r;
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["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 g;
return r;
}
// VisionGraspObject::Goal: object_id
// MotorInfo::Request: target, type, motor_ids
template<>
inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action::VisionGraspObject::Goal>(const YAML::Node & n)
inline interfaces::srv::MotorInfo::Request from_yaml<interfaces::srv::MotorInfo::Request>(const YAML::Node & n)
{
interfaces::action::VisionGraspObject::Goal g;
interfaces::srv::MotorInfo::Request r;
if (n && n.IsMap()) {
if (n["object_id"]) g.object_id = n["object_id"].as<std::string>();
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 g;
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;
}
// 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;
}
} // namespace brain

21
src/brain/include/brain/skill_config.hpp
View File

@@ -6,6 +6,7 @@
#include "interfaces/action/arm.hpp"
#include "interfaces/action/camera_take_photo.hpp"
#include "interfaces/action/gripper_cmd.hpp"
#include "interfaces/action/hand_control.hpp"
#include "interfaces/action/move_home.hpp"
#include "interfaces/action/move_leg.hpp"
@@ -30,7 +31,8 @@ using SkillActionTypes = std::tuple<
interfaces::action::MoveWaist,
interfaces::action::MoveLeg,
interfaces::action::MoveWheel,
interfaces::action::MoveHome
interfaces::action::MoveHome,
interfaces::action::GripperCmd
>;
using SkillServiceTypes = std::tuple<
@@ -70,7 +72,7 @@ template<>
struct SkillActionTrait<interfaces::action::MoveWaist>
{
static constexpr const char * skill_name = "MoveWaist";
static constexpr const char * default_topic = "";
static constexpr const char * default_topic = "MoveWaist";
static bool success(const interfaces::action::MoveWaist::Result & r) {return r.success;}
static std::string message(const interfaces::action::MoveWaist::Result & r) {return r.message;}
};
@@ -79,7 +81,7 @@ template<>
struct SkillActionTrait<interfaces::action::MoveLeg>
{
static constexpr const char * skill_name = "MoveLeg";
static constexpr const char * default_topic = "";
static constexpr const char * default_topic = "MoveLeg";
static bool success(const interfaces::action::MoveLeg::Result & r) {return r.success;}
static std::string message(const interfaces::action::MoveLeg::Result & r) {return r.message;}
};
@@ -88,7 +90,7 @@ template<>
struct SkillActionTrait<interfaces::action::MoveWheel>
{
static constexpr const char * skill_name = "MoveWheel";
static constexpr const char * default_topic = "";
static constexpr const char * default_topic = "MoveWheel";
static bool success(const interfaces::action::MoveWheel::Result & r) {return r.success;}
static std::string message(const interfaces::action::MoveWheel::Result & r) {return r.message;}
};
@@ -97,11 +99,20 @@ template<>
struct SkillActionTrait<interfaces::action::MoveHome>
{
static constexpr const char * skill_name = "MoveHome";
static constexpr const char * default_topic = "";
static constexpr const char * default_topic = "MoveHome";
static bool success(const interfaces::action::MoveHome::Result & r) {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) {return (r.result == 1);}
static std::string message(const interfaces::action::GripperCmd::Result & r) {return r.state;}
};
template<>
struct SkillServiceTrait<interfaces::srv::VisionObjectRecognition>
{

8
src/brain/include/brain/sm_cerebellum.hpp
View File

@@ -149,10 +149,10 @@ 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) {
RCLCPP_WARN(this->getLogger(), "[SM] Still waiting for execute_fn to be set...");
last_warn = now;
}
}
try {
auto res = d.execute_fn(this->getLogger());

1
src/brain/package.xml
View File

@@ -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>

297
src/brain/src/calc_grasp_pose.cpp Normal file
View File

@@ -0,0 +1,297 @@
#include "brain/calc_grasp_pose.hpp"
#include <cmath>
#include <iostream>
#include <algorithm>
#include <vector>
#include <utility>
#include <string.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/logger.hpp>
#include "arm_action_define.h"
#include "rm_interface.h"
namespace brain {
bool GraspPoseCalculator::rm_arm_algo_init_done_ = false;
GraspPoseCalculator::GraspPoseCalculator()
{
rm_arm_algo_init();
}
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;
(void)arm;
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);
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;
#if ENABLE_TOLERANCE
// Check IK and adjust position if needed (Tolerance +/- 2cm)
std::vector<double> q_out(7, 0.0);
// Try original position first
if (rm_arm_inverse_kinematics(result.grasp_pose.position, result.grasp_pose.orientation, q_out) != 0) {
// Search in +/- 2cm box
bool found = false;
double tolerance = 0.02;
double step = 0.005; // 5mm step
for (double dx = -tolerance; dx <= tolerance + 1e-5; dx += step) {
for (double dy = -tolerance; dy <= tolerance + 1e-5; dy += step) {
for (double dz = -tolerance; dz <= tolerance + 1e-5; dz += step) {
if (std::abs(dx) < 1e-5 && std::abs(dy) < 1e-5 && std::abs(dz) < 1e-5) continue;
tf2::Vector3 new_target = target_pos + tf2::Vector3(dx, dy, dz);
GraspResult new_result = create_grasp_matrix(new_target, approach_vec, palm_normal, safety_dist, finger_length);
new_result.name = name_suffix;
if (rm_arm_inverse_kinematics(new_result.grasp_pose.position, new_result.grasp_pose.orientation, q_out) == 0) {
result = new_result;
found = true;
RCLCPP_INFO(rclcpp::get_logger("GraspPoseCalculator"),
"Adjusted grasp target by [%.3f, %.3f, %.3f] to satisfy IK.", dx, dy, dz);
break;
}
}
if (found) break;
}
if (found) break;
}
if (!found) {
RCLCPP_WARN(rclcpp::get_logger("GraspPoseCalculator"), "Could not find valid IK within tolerance.");
}
}
#endif
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;
}
void GraspPoseCalculator::rm_arm_algo_init(void)
{
rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
rm_force_type_e Type = RM_MODEL_RM_B_E;
rm_algo_init_sys_data(Mode, Type);
rm_arm_algo_init_done_ = true;
}
int GraspPoseCalculator::rm_arm_inverse_kinematics(
const tf2::Vector3& target_pos,
const tf2::Quaternion& target_quat,
std::vector<double>& joint_angles
) {
if (!rm_arm_algo_init_done_) {
rm_arm_algo_init();
}
float joints[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
if (joint_angles.size() < USED_ARM_DOF) {
joint_angles.resize(USED_ARM_DOF, 0.0);
}
for (int i = 0; i < USED_ARM_DOF; i++) {
joints[i] = static_cast<float>(joint_angles[i]);
}
rm_inverse_kinematics_params_t ik_params;
memcpy(ik_params.q_in, joints, sizeof(joints));
ik_params.q_pose.position.x = target_pos.x();
ik_params.q_pose.position.y = target_pos.y();
ik_params.q_pose.position.z = target_pos.z();
ik_params.q_pose.quaternion.x = target_quat.x();
ik_params.q_pose.quaternion.y = target_quat.y();
ik_params.q_pose.quaternion.z = target_quat.z();
ik_params.q_pose.quaternion.w = target_quat.w();
ik_params.flag = 0;
#ifdef RM_ALGO_IK_DEBUG
RCLCPP_INFO(rclcpp::get_logger("GraspPoseCalculator"),
"IK Target Pos: [%.4f, %.4f, %.4f], Quat: [%.4f, %.4f, %.4f, %.4f]",
ik_params.q_pose.position.x, ik_params.q_pose.position.y, ik_params.q_pose.position.z,
ik_params.q_pose.quaternion.x, ik_params.q_pose.quaternion.y,
ik_params.q_pose.quaternion.z, ik_params.q_pose.quaternion.w);
RCLCPP_INFO(rclcpp::get_logger("GraspPoseCalculator"),
"IK Input Joints: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]);
#endif
int result = rm_algo_inverse_kinematics(NULL, ik_params, joints);
if (result != 0) {
// Downgrade to DEBUG to avoid spamming during search
RCLCPP_DEBUG(rclcpp::get_logger("GraspPoseCalculator"), "Inverse kinematics failed with error code %d", result);
return ARM_AIM_CANNOT_REACH;
}
for (int i = 0; i < USED_ARM_DOF; i++) {
joint_angles[i] = static_cast<double>(joints[i]);
}
return 0;
}
} // namespace brain

957
src/brain/src/cerebellum_hooks.cpp Normal file
View File

@@ -0,0 +1,957 @@
#include "brain/cerebellum_hooks.hpp"
#include "brain/cerebellum_node.hpp"
#include "brain/payload_converters.hpp"
#include "arm_action_define.h"
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include <iomanip>
#include <mutex>
namespace brain
{
void CerebellumHooks::ConfigureActionHooks(CerebellumNode * node)
{
if (!node->skill_manager_) {
RCLCPP_WARN(node->get_logger(), "SkillManager unavailable, skipping action hook configuration");
return;
}
ConfigureArmHooks(node);
ConfigureHandControlHooks(node);
ConfigureCameraTakePhotoHooks(node);
ConfigureMoveWaistHooks(node);
ConfigureMoveLegHooks(node);
ConfigureMoveWheelHooks(node);
ConfigureMoveHomeHooks(node);
ConfigureGripperCmdControlHooks(node);
}
void CerebellumHooks::ConfigureArmSpaceControlHooks(CerebellumNode * node)
{
(void)node;
}
void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
{
// Arm
SkillManager::ActionHookOptions<interfaces::action::Arm> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::Arm::Goal goal{};
if (!node->TryParseCallPayload<interfaces::action::Arm::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
RCLCPP_INFO(node->get_logger(), "[%s]Received Parse Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);
goal.body_id = (CerebellumNode::tls_arm_body_id == LEFT_ARM ||
CerebellumNode::tls_arm_body_id == RIGHT_ARM) ? CerebellumNode::tls_arm_body_id : goal.body_id;
goal.command_id = ++node->command_id_;
if (goal.body_id == LEFT_ARM) {
node->command_id_left_arm_ = goal.command_id;
} else if (goal.body_id == RIGHT_ARM) {
node->command_id_right_arm_ = goal.command_id;
}
goal.frame_time_stamp = node->get_clock()->now().nanoseconds();
if (goal.data_type == ARM_COMMAND_TYPE_POSE_DIRECT_MOVE) {
if (goal.data_length == POSE_DIMENSION*2 && goal.data_array.size() == POSE_DIMENSION*2) {
if (goal.body_id == LEFT_ARM) {
goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]};
} else if (goal.body_id == RIGHT_ARM) {
goal.data_array = {goal.data_array[7], goal.data_array[8], goal.data_array[9],
goal.data_array[10], goal.data_array[11], goal.data_array[12], goal.data_array[13]};
}
goal.data_length = POSE_DIMENSION;
}
} else if (goal.data_type == ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE) {
if (goal.data_length == USED_ARM_DOF*2 && goal.data_array.size() == USED_ARM_DOF*2) {
if (goal.body_id == LEFT_ARM) {
goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
goal.data_array[3], goal.data_array[4], goal.data_array[5]};
} else if (goal.body_id == RIGHT_ARM) {
goal.data_array = {goal.data_array[6], goal.data_array[7], goal.data_array[8], goal.data_array[9],
goal.data_array[10], goal.data_array[11]};
}
goal.data_length = USED_ARM_DOF;
}
} else if (goal.data_type >= 100 && goal.data_type <= 110) {
geometry_msgs::msg::Pose pose;
std::vector<double> angle{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
if (goal.data_type == 100) { //take photo
if (!node->right_hand_take_photo_poses_.empty()) {
pose = node->right_hand_take_photo_poses_[0];
}
if (!node->right_hand_take_photo_angles_.empty()) {
angle = node->right_hand_take_photo_angles_[node->right_hand_take_photo_angles_.size()-1];
}
} else if (goal.data_type == 101) { // pre-grasp
if (!node->pre_grasp_poses_.empty()) {
pose = node->pre_grasp_poses_[0];
}
if (!node->pre_grasp_angles_.empty()) {
angle = node->pre_grasp_angles_[node->pre_grasp_angles_.size()-1];
}
} else if (goal.data_type == 102) { // grasp
if (!node->grasp_poses_.empty()) {
pose = node->grasp_poses_[0];
}
if (!node->grasp_angles_.empty()) {
angle = node->grasp_angles_[node->grasp_angles_.size()-1];
}
} else if (goal.data_type == 110) { //take object
if (!node->right_hand_take_object_pose_.empty()) {
pose = node->right_hand_take_object_pose_[0];
}
if (!node->right_hand_take_object_angles_.empty()) {
angle = node->right_hand_take_object_angles_[node->right_hand_take_object_angles_.size()-1];
}
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_type");
return goal;
}
// goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
// goal.data_length = POSE_DIMENSION;
// goal.data_array = {pose.position.x, pose.position.y, pose.position.z,
// pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
if (angle.size() == USED_ARM_DOF) {
goal.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE;
goal.data_length = USED_ARM_DOF;
goal.data_array = {angle[0], angle[1], angle[2], angle[3], angle[4], angle[5]};
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid angle size: %ld/%d", angle.size(), USED_ARM_DOF);
}
}
else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: data_type[%d] not support", goal.data_type);
return goal;
}
if (goal.data_array.size() >= USED_ARM_DOF) {
RCLCPP_INFO(node->get_logger(), "[%s] Control params: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_array size[%ld]", goal.data_array.size());
}
return goal;
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::Arm>::SharedPtr,
const std::shared_ptr<const interfaces::action::Arm::Feedback> & feedback) {
if (!feedback) {
RCLCPP_WARN(node->get_logger(), "[%s] feedback is empty", skill_name.c_str());
return;
}
(void)feedback;
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::Arm>::WrappedResult & res) {
const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == OK);
const std::string message = res.result ? std::string("action end") : std::string("No return information");
geometry_msgs::msg::Pose pose = res.result->pose;
std::string res_arm_name = "";
if (res.result->command_id == node->command_id_left_arm_) {
node->final_arm_pose_["left_arm"] = pose;
res_arm_name = "Left Arm";
} else if (res.result->command_id == node->command_id_right_arm_) {
node->final_arm_pose_["right_arm"] = pose;
res_arm_name = "Right Arm";
}
RCLCPP_WARN(node->get_logger(), "[%s] %s Final position: %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f",
skill_name.c_str(), res_arm_name.c_str(), pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] %s command_id %ld completed: %s",
skill_name.c_str(), res_arm_name.c_str(), res.result->command_id, message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::Arm>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::Arm>("Arm", std::move(hooks));
}
void CerebellumHooks::ConfigureHandControlHooks(CerebellumNode * node)
{
// HandControl
SkillManager::ActionHookOptions<interfaces::action::HandControl> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::HandControl::Goal goal{};
// Plan A: per-call YAML overrides
if (!node->TryParseCallPayload<interfaces::action::HandControl::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
const std::string param = "hand_control.target_pose";
if (!node->has_parameter(param)) {
node->declare_parameter<std::vector<double>>(param, {0.0, 0.0, 0.0});
}
const auto values = node->get_parameter(param).as_double_array();
RCLCPP_INFO(node->get_logger(), "[%s] Target parameters size=%zu", skill_name.c_str(), values.size());
(void)values;
goal.mode = 1;
return goal;
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::SharedPtr,
const std::shared_ptr<const interfaces::action::HandControl::Feedback> & feedback) {
if (!feedback) {return;}
(void)feedback;
RCLCPP_INFO(node->get_logger(), "[%s] Current progress: %.2f", skill_name.c_str(), feedback->progress);
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::WrappedResult & res) {
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::HandControl>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::HandControl>("HandControl", std::move(hooks));
}
void CerebellumHooks::ConfigureCameraTakePhotoHooks(CerebellumNode * node)
{
// CameraTakePhoto
SkillManager::ActionHookOptions<interfaces::action::CameraTakePhoto> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::CameraTakePhoto::Goal goal{};
if (!node->TryParseCallPayload<interfaces::action::CameraTakePhoto::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
return goal;
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>::WrappedResult & res) {
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::CameraTakePhoto>("CameraTakePhoto", std::move(hooks));
}
void CerebellumHooks::ConfigureLegControlHooks(CerebellumNode * node)
{
(void)node;
}
void CerebellumHooks::ConfigureVisionGraspObjectHooks(CerebellumNode * node)
{
(void)node;
}
void CerebellumHooks::ConfigureSlamModeHooks(CerebellumNode * node)
{
(void)node;
}
void CerebellumHooks::ConfigureNavigateToPoseHooks(CerebellumNode * node)
{
(void)node;
}
void CerebellumHooks::ConfigureMoveWaistHooks(CerebellumNode * node)
{
// MoveWaist
SkillManager::ActionHookOptions<interfaces::action::MoveWaist> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::MoveWaist::Goal goal{};
// Plan A: load per-call YAML payload into goal if present
if (!node->TryParseCallPayload<interfaces::action::MoveWaist::Goal>(skill_name, goal)) {
RCLCPP_INFO(node->get_logger(), "Loaded MoveWaist goal from YAML payload");
return goal;
}
RCLCPP_INFO(node->get_logger(), "[%s] move_pitch_degree: %f, move_yaw_degree: %f",
skill_name.c_str(), goal.move_pitch_degree, goal.move_yaw_degree);
return goal;
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>::SharedPtr,
const std::shared_ptr<const interfaces::action::MoveWaist::Feedback> & feedback) {
if (!feedback) {return;}
(void)skill_name;
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>::WrappedResult & res) {
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::MoveWaist>("MoveWaist", std::move(hooks));
}
void CerebellumHooks::ConfigureMoveLegHooks(CerebellumNode * node)
{
// MoveLeg
SkillManager::ActionHookOptions<interfaces::action::MoveLeg> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::MoveLeg::Goal goal{};
if (!node->TryParseCallPayload<interfaces::action::MoveLeg::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
return goal;
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>::SharedPtr,
const std::shared_ptr<const interfaces::action::MoveLeg::Feedback> & feedback) {
if (!feedback) {return;}
(void)skill_name;
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>::WrappedResult & res) {
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::MoveLeg>("MoveLeg", std::move(hooks));
}
void CerebellumHooks::ConfigureMoveWheelHooks(CerebellumNode * node)
{
// MoveWheel
SkillManager::ActionHookOptions<interfaces::action::MoveWheel> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::MoveWheel::Goal goal{};
// Plan A: per-call YAML overrides
if (!node->TryParseCallPayload<interfaces::action::MoveWheel::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
RCLCPP_INFO(node->get_logger(), "[%s] move_distance=%f, move_angle=%f",
skill_name.c_str(), goal.move_distance, goal.move_angle);
return goal;
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>::SharedPtr,
const std::shared_ptr<const interfaces::action::MoveWheel::Feedback> & feedback) {
if (!feedback) {return;}
(void)skill_name;
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>::WrappedResult & res) {
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::MoveWheel>("MoveWheel", std::move(hooks));
}
void CerebellumHooks::ConfigureMoveHomeHooks(CerebellumNode * node)
{
// MoveHome
SkillManager::ActionHookOptions<interfaces::action::MoveHome> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::MoveHome::Goal goal{};
// Plan A: per-call YAML overrides (even though goal is empty)
if (!node->TryParseCallPayload<interfaces::action::MoveHome::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
return goal;
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>::SharedPtr,
const std::shared_ptr<const interfaces::action::MoveHome::Feedback> & feedback) {
if (!feedback) {return;}
(void)skill_name;
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>::WrappedResult & res) {
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
const std::string message = res.result ? res.result->message : std::string("No return information");
if (success) {
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::MoveHome>("MoveHome", std::move(hooks));
}
void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
{
// GripperCmd
SkillManager::ActionHookOptions<interfaces::action::GripperCmd> hooks;
hooks.make_goal = [node](const std::string & skill_name) {
interfaces::action::GripperCmd::Goal goal{};
// Plan A: per-call YAML overrides
if (!node->TryParseCallPayload<interfaces::action::GripperCmd::Goal>(skill_name, goal)) {
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
// goal.loc = 30; //grab_width_; //TODO transfer
RCLCPP_INFO(node->get_logger(), "[%s] action make_goal params, loc: %d, speed: %d, torque: %d, mode: %d",
skill_name.c_str(), goal.loc, goal.speed, goal.torque, goal.mode);
return goal;
};
hooks.on_feedback = [node](
const std::string & skill_name,
ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>::SharedPtr,
const std::shared_ptr<const interfaces::action::GripperCmd::Feedback> & feedback) {
if (!feedback) {return;}
RCLCPP_INFO(node->get_logger(), "[%s] action feedback, loc: %d, speed: %d, torque: %d",
skill_name.c_str(), feedback->loc, feedback->speed, feedback->torque);
};
hooks.on_result = [node](
const std::string & skill_name,
const ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>::WrappedResult & res) {
if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else if (res.code == rclcpp_action::ResultCode::SUCCEEDED) {
RCLCPP_INFO(node->get_logger(), "[%s] action on_result, state: %s",
skill_name.c_str(), res.result->state.c_str());
} else {
RCLCPP_ERROR(node->get_logger(), "[%s] was failed(code=%d)", skill_name.c_str(), static_cast<int>(res.code));
}
};
hooks.on_goal_response = [node](
const std::string & skill_name,
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>> & handle) {
if (static_cast<bool>(handle)) {
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
} else {
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
}
};
node->skill_manager_->configure_action_hooks<interfaces::action::GripperCmd>("GripperCmd", std::move(hooks));
}
#define DEBUG_GRASP_POSE_CALCULATION
void CerebellumHooks::ConfigureServiceHooks(CerebellumNode * node)
{
if (!node->skill_manager_) {
RCLCPP_WARN(node->get_logger(), "SkillManager unavailable, skipping service hook configuration");
return;
}
ConfigureVisionObjectRecognitionHooks(node);
ConfigureMapLoadHooks(node);
ConfigureMapSaveHooks(node);
}
#define ENABLE_CAL_GRASP_POSE
void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * node)
{
using VisionSrv = interfaces::srv::VisionObjectRecognition;
auto UpdateTargetPose = [node](const interfaces::msg::PoseClassAndID & obj, const std_msgs::msg::Header & header) -> bool {
for (auto &tf: node->target_frames_) {
if (tf == obj.class_name) {
node->target_pose_[tf].header = header;
node->target_pose_[tf].header.frame_id = "RightLink6";
node->target_pose_[tf].pose = obj.pose;
node->grab_width_.clear();
int size = (int)(obj.grab_width.size());
for (int i = 0; i < size; i++) {
node->grab_width_.push_back(obj.grab_width[i]);
RCLCPP_INFO(node->get_logger(), "grab_width : %.4f", obj.grab_width[i]);
}
const auto & p = obj.pose;
RCLCPP_INFO(node->get_logger(), "target_frame: %s, grab_width_size: %zu, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
tf.c_str(), node->grab_width_.size(),
p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
return true;
} else {
RCLCPP_WARN(node->get_logger(), "object class_name: %s, target_frame: %s not match", obj.class_name.c_str(), tf.c_str());
}
}
RCLCPP_WARN(node->get_logger(), "object class_name not match target frames");
return false;
};
auto CalculateGraspPoses = [node](const std::string & skill_name) -> bool {
// const auto & p = node->target_pose_[node->target_frame_].pose;
// RCLCPP_INFO(node->get_logger(), "target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
// p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
std::vector<double> current_joint_angles;
{
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if ((node->arm_target_frame_.find("right") != std::string::npos)) {
current_joint_angles = node->right_arm_joint_angles_;
} else {
current_joint_angles = node->left_arm_joint_angles_;
}
}
if (node->camera_position_ == "top") {
node->right_hand_take_photo_poses_.clear();
node->right_hand_take_photo_angles_.clear();
geometry_msgs::msg::Pose pose = node->target_pose_[node->target_frame_].pose;
RCLCPP_INFO(node->get_logger(), "[%s] target pose %s frame, camera_position: %s",
skill_name.c_str(), node->arm_target_frame_.c_str(), node->camera_position_.c_str());
RCLCPP_INFO(node->get_logger(), "target pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
pose.position.x, pose.position.y, pose.position.z, pose.orientation.x,
pose.orientation.y, pose.orientation.z, pose.orientation.w);
pose.position.x += 0.08f;
// pose.position.y -= 0.40f;
pose.position.y = 0.34f;
pose.position.z += 0.03f;
pose.orientation.x = -0.7071;
pose.orientation.y = 0.0000;
pose.orientation.z = -0.0000;
pose.orientation.w = 0.7071;
RCLCPP_INFO(node->get_logger(), "cam take photo pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
pose.position.x, pose.position.y, pose.position.z, pose.orientation.x,
pose.orientation.y, pose.orientation.z, pose.orientation.w);
if (pose.position.x < 0.1 || pose.position.y < 0.1 || pose.position.z < 0.1 ||
pose.position.x > 0.8 || pose.position.y > 0.8 || pose.position.z > 1.0) {
RCLCPP_ERROR(node->get_logger(), "invalid pose value, cal grasp pose failed");
return false;
}
std::vector<double> joint_angles = current_joint_angles;
tf2::Vector3 target_pos = {pose.position.x, pose.position.y, pose.position.z};
tf2::Quaternion target_quat = {pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(target_pos, target_quat, joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "right_hand take_photo_pose rm arm inverse kinematics failed.");
return false;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "right_hand take_photo angles: ");
for (const auto & angles: joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
}
#endif
node->right_hand_take_photo_poses_.push_back(pose);
node->right_hand_take_photo_angles_.push_back(joint_angles);
return true;
}
geometry_msgs::msg::PoseStamped pose_in_arm;
const bool transformed = node->TransformPoseToArmFrame(node->target_pose_[node->target_frame_], pose_in_arm);
if (!transformed) {
RCLCPP_WARN(node->get_logger(), "[%s] coordinate transformation failed, continuing with %s frame data",
skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
return false;
}
{
RCLCPP_INFO(node->get_logger(), "[%s] target pose transformed to %s frame", skill_name.c_str(), node->arm_target_frame_.c_str());
RCLCPP_INFO(node->get_logger(), "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z,
pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z);
tf2::Quaternion target_quat(pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
// std::vector<double> best_angles = brain::GraspPoseCalculator::find_optimal_grasp_angles(target_pos, node->arm_target_frame_, 3);
// if (best_angles.empty()) {
// RCLCPP_WARN(node->get_logger(), "[%s]no valid grasp angles found", skill_name.c_str());
// return false;
// }
// best_angles.clear();
std::vector<double> best_angles;
best_angles.push_back(0);
{
std::vector<std::string> palm_facings = {"down"}; //{"up", "down"};
for (double angle : best_angles) {
for (const auto& palm : palm_facings) {
brain::GraspResult result = brain::GraspPoseCalculator::calculate(target_pos,
target_quat, "side", angle, palm, 0.10, 0.20, node->arm_target_frame_);
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, pre_grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(), result.pre_grasp_pose.orientation.w());
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
#endif
//pre grasp
std::vector<double> pre_grasp_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.pre_grasp_pose.position,
result.pre_grasp_pose.orientation, pre_grasp_joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "[%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
continue;
}
//grasp
result.grasp_pose.position.setY(result.grasp_pose.position.y() + 0.02); //补偿抓取的长度
std::vector<double> grasp_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.grasp_pose.position,
result.grasp_pose.orientation, grasp_joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "[%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
continue;
}
//take object
auto take_object_pose = result.grasp_pose;
take_object_pose.position.setX(take_object_pose.position.x() - 0.10); //向上拿起物体
std::vector<double> take_object_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(take_object_pose.position,
take_object_pose.orientation, take_object_joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "[%s] angle %f, palm %s take_object joint_angles rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
continue;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "pre_grasp_joint_angles: ");
for (const auto & angles: pre_grasp_joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
}
RCLCPP_INFO(node->get_logger(), "grasp_joint_angles: ");
for (const auto & angles: grasp_joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
}
RCLCPP_INFO(node->get_logger(), "take_object_joint_angles: ");
for (const auto & angles: take_object_joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
}
#endif
geometry_msgs::msg::Pose pre_grasp_msg;
pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
pre_grasp_msg.position.z = result.pre_grasp_pose.position.z();
pre_grasp_msg.orientation.x = result.pre_grasp_pose.orientation.x();
pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
geometry_msgs::msg::Pose grasp_msg;
grasp_msg.position.x = result.grasp_pose.position.x();
grasp_msg.position.y = result.grasp_pose.position.y();
grasp_msg.position.z = result.grasp_pose.position.z();
grasp_msg.orientation.x = result.grasp_pose.orientation.x();
grasp_msg.orientation.y = result.grasp_pose.orientation.y();
grasp_msg.orientation.z = result.grasp_pose.orientation.z();
grasp_msg.orientation.w = result.grasp_pose.orientation.w();
node->pre_grasp_poses_.push_back(pre_grasp_msg);
node->grasp_poses_.push_back(grasp_msg);
node->right_hand_take_object_pose_.push_back(grasp_msg);
node->pre_grasp_angles_.push_back(pre_grasp_joint_angles);
node->grasp_angles_.push_back(grasp_joint_angles);
node->right_hand_take_object_angles_.push_back(take_object_joint_angles);
RCLCPP_INFO(node->get_logger(), "Generated Pose Success (grasp_type: %s, Angle: %.1f, Palm: %s)", result.name.c_str(), angle, palm.c_str());
}
}
}
}
return true;
};
{
SkillManager::ServiceHookOptions<VisionSrv> hooks;
hooks.wait_timeout = [](const std::string &) {
return std::chrono::seconds(2);
};
hooks.call_timeout = [](const std::string &) {
return std::chrono::seconds(5);
};
hooks.make_request = [node](const std::string & skill_name) {
auto req = std::make_shared<VisionSrv::Request>();
// Prefer payload_yaml from calls
if (!node->TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
RCLCPP_ERROR(node->get_logger(), "[%s] Payload Parse failed", skill_name.c_str());
return req;
}
// fallback to parameter
// const std::string param = "vision_object_recognition.camera_position";
// if (!node->has_parameter(param)) {
// node->declare_parameter<std::string>(param, std::string("right"));
// }
// req->camera_position = node->get_parameter(param).as_string();
node->camera_position_ = req->camera_position;
RCLCPP_INFO(node->get_logger(), "[%s] camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
return req;
};
hooks.on_response = [node, UpdateTargetPose, CalculateGraspPoses](
const std::string & skill_name,
const SkillManager::ServiceHookOptions<VisionSrv>::ClientPtr &,
const SkillManager::ServiceHookOptions<VisionSrv>::ResponsePtr & resp,
std::string & detail) {
if (!resp->success) {
detail = resp->info.empty() ? std::string("Not success") : resp->info;
RCLCPP_WARN(node->get_logger(), "[%s] VisionObjectRecognition failed: %s",
skill_name.c_str(), detail.c_str());
return false;
}
detail = resp->info.empty() ? std::string("success pose") : resp->info;
const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
RCLCPP_INFO(node->get_logger(), "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
skill_name.c_str(), resp->success, resp->objects.size(),
resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
node->pre_grasp_poses_.clear();
node->grasp_poses_.clear();
node->right_hand_take_object_pose_.clear();
node->pre_grasp_angles_.clear();
node->grasp_angles_.clear();
node->right_hand_take_object_angles_.clear();
for (size_t i = 0; i < resp->objects.size(); ++i) {
const auto & obj = resp->objects[i];
// RCLCPP_INFO(node->get_logger(), " [%zu] class='%s' id=%d", i, obj.class_name.c_str(), obj.class_id);
if (!UpdateTargetPose(obj, resp->header)) {
continue;
}
#ifdef ENABLE_CAL_GRASP_POSE
if (!CalculateGraspPoses(skill_name)) {
continue;
}
#endif
}
if (node->camera_position_ == "top") {
if (node->right_hand_take_photo_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo poses generated", skill_name.c_str());
return false;
}
if (node->right_hand_take_photo_angles_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo angles generated", skill_name.c_str());
return false;
}
} else {
if (node->pre_grasp_poses_.empty() || node->grasp_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->pre_grasp_poses_.size() != node->grasp_poses_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] pre_grasp_poses_ size %zu not match grasp_poses_ size %zu",
skill_name.c_str(), node->pre_grasp_poses_.size(), node->grasp_poses_.size());
return false;
}
if (node->pre_grasp_angles_.empty() || node->grasp_angles_.empty() || node->right_hand_take_object_angles_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->pre_grasp_angles_.size() != node->grasp_angles_.size() ||
node->grasp_angles_.size() != node->right_hand_take_object_angles_.size() ||
node->pre_grasp_angles_.size() != node->right_hand_take_object_angles_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
skill_name.c_str(), node->pre_grasp_angles_.size(), node->grasp_angles_.size(),
node->right_hand_take_object_angles_.size());
return false;
}
}
node->right_arm_take_photo_joint_angles_.clear();
node->right_arm_take_photo_joint_angles_ = node->right_arm_joint_angles_;
return true;
};
node->skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));
}
}
void CerebellumHooks::ConfigureMapLoadHooks(CerebellumNode * node)
{
using MapLoadSrv = interfaces::srv::MapLoad;
SkillManager::ServiceHookOptions<MapLoadSrv> hooks;
hooks.wait_timeout = [](const std::string &) { return std::chrono::seconds(2); };
hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
hooks.make_request = [node](const std::string & skill_name) {
auto req = std::make_shared<MapLoadSrv::Request>();
if (!node->TryParseCallPayload<MapLoadSrv::Request>(skill_name, *req)) { return req; }
// Fallback to parameter
if (!node->has_parameter("map_load_url")) {
node->declare_parameter<std::string>("map_load_url", std::string(""));
}
req->map_url = node->get_parameter("map_load_url").as_string();
return req;
};
hooks.on_response = [node](
const std::string & skill_name,
const SkillManager::ServiceHookOptions<MapLoadSrv>::ClientPtr &,
const SkillManager::ServiceHookOptions<MapLoadSrv>::ResponsePtr & resp,
std::string & detail) {
// MapLoad response schema may not include a message field; rely on success only
detail = resp->success ? std::string("ok") : std::string("failed");
if (!resp->success) {
RCLCPP_WARN(node->get_logger(), "[%s] MapLoad failed: %s", skill_name.c_str(), detail.c_str());
} else {
RCLCPP_INFO(node->get_logger(), "[%s] MapLoad: %s", skill_name.c_str(), detail.c_str());
}
return resp->success;
};
node->skill_manager_->configure_service_hooks<MapLoadSrv>("MapLoad", std::move(hooks));
}
void CerebellumHooks::ConfigureMapSaveHooks(CerebellumNode * node)
{
using MapSaveSrv = interfaces::srv::MapSave;
SkillManager::ServiceHookOptions<MapSaveSrv> hooks;
hooks.wait_timeout = [](const std::string &) { return std::chrono::seconds(2); };
hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
hooks.make_request = [node](const std::string & skill_name) {
auto req = std::make_shared<MapSaveSrv::Request>();
if (!node->TryParseCallPayload<MapSaveSrv::Request>(skill_name, *req)) { return req; }
// Fallback to parameter group
if (!node->has_parameter("map_save_url")) {
node->declare_parameter<std::string>("map_save_url", std::string("/tmp/humanoid_map"));
}
req->map_url = node->get_parameter("map_save_url").as_string();
return req;
};
hooks.on_response = [node](
const std::string & skill_name,
const SkillManager::ServiceHookOptions<MapSaveSrv>::ClientPtr &,
const SkillManager::ServiceHookOptions<MapSaveSrv>::ResponsePtr & resp,
std::string & detail) {
// MapSave response schema may not include a message field; rely on success only
detail = resp->success ? std::string("ok") : std::string("failed");
if (!resp->success) {
RCLCPP_WARN(node->get_logger(), "[%s] MapSave failed: %s", skill_name.c_str(), detail.c_str());
} else {
RCLCPP_INFO(node->get_logger(), "[%s] MapSave: %s", skill_name.c_str(), detail.c_str());
}
return resp->success;
};
node->skill_manager_->configure_service_hooks<MapSaveSrv>("MapSave", std::move(hooks));
}
} // namespace brain

974
src/brain/src/cerebellum_node.cpp
View File

File diff suppressed because it is too large Load Diff

352
src/brain/src/cerebrum_node.cpp
View File

@@ -75,7 +75,7 @@ struct UpdatingFlagGuard
// Simple, file-local cooldown between successive BT leaf actions.
// We use steady_clock to avoid ROS time/clock-type complications.
static std::chrono::steady_clock::time_point g_bt_resume_time_steady{};
static constexpr std::chrono::milliseconds kInterActionDelayMs{0};
static constexpr std::chrono::milliseconds kInterActionDelayMs{200};
} // namespace
@@ -201,72 +201,32 @@ void CerebrumNode::RegisterActionClient()
}
robot_work_info_.bt_node_status = "RUNNING";
// Debug: log outgoing calls being sent to Cerebellum
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal with calls_count=%zu", g.calls.size());
// RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal with calls_count=%zu", g.calls.size());
for (size_t i = 0; i < g.calls.size(); ++i) {
const auto & c = g.calls[i];
RCLCPP_INFO(this->get_logger(), " out_call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
}
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction]Sending goal epoch=%u skills=%s", g.epoch, g.action_name.c_str());
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal epoch=%u skills=%s", g.epoch, g.action_name.c_str());
return g;
},
// Feedback
[this](
typename ActionClientRegistry::GoalHandle<EBA>::SharedPtr,
const std::shared_ptr<const EBA::Feedback> feedback) {
if (!feedback) {
if (!feedback) { return; }
if (!epoch_filter_.accept(feedback->epoch, feedback->current_skill, bt_pending_run_)) {
return;
}
if (!epoch_filter_.accept(
feedback->epoch, feedback->current_skill, bt_pending_run_))
{
return;
}
{
std::lock_guard<std::mutex> lk(exec_mutex_);
// Match per-skill BT node by skill_name.
const std::string bt_type = feedback->current_skill + std::string("_H");
auto it = per_node_exec_.find(bt_type);
if (it != per_node_exec_.end()) {
auto & info = it->second;
if (!info.in_flight && !info.result) {
// Late feedback for an already closed node; ignore.
} else {
if (feedback->stage == "RUN" && feedback->progress + 1e-4 < info.last_progress) {
return; // non-monotonic
}
if (feedback->stage == "RUN") {
info.last_progress = feedback->progress;
}
if (feedback->stage == "END") {
info.result_code = ParseResultDetail(feedback->detail);
info.result = (info.result_code == ExecResultCode::SUCCESS);
if (info.result_code == ExecResultCode::SUCCESS) {
info.awaiting_result = true;
info.end_feedback_time = this->now();
} else {
info.in_flight = false;
info.awaiting_result = false;
}
}
}
}
}
RCLCPP_INFO(
this->get_logger(),
// For parallel per-instance leaves, we only log feedback; mapping to instance is ambiguous.
RCLCPP_INFO(this->get_logger(),
"[ExecuteBtAction][fb][epoch=%u] stage=%s skill=%s progress=%.2f detail=%s",
feedback->epoch,
feedback->stage.c_str(),
feedback->current_skill.c_str(),
feedback->progress,
feedback->detail.c_str());
feedback->epoch, feedback->stage.c_str(), feedback->current_skill.c_str(),
feedback->progress, feedback->detail.c_str());
},
// Result
[this](const ActionClientRegistry::GoalHandle<EBA>::WrappedResult & res) {
bool ok = (
res.code == rclcpp_action::ResultCode::SUCCEEDED &&
res.result &&
res.result->success);
const bool ok = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success);
if (ok) {
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] OK: %s",
res.result ? res.result->message.c_str() : "<null>");
@@ -274,17 +234,7 @@ void CerebrumNode::RegisterActionClient()
RCLCPP_ERROR(this->get_logger(), "[ExecuteBtAction][result] FAIL code=%d msg=%s",
static_cast<int>(res.code), res.result ? res.result->message.c_str() : "<null>");
}
for (auto & kv : per_node_exec_) {
auto & info = kv.second;
if (info.in_flight || info.awaiting_result) {
info.in_flight = false;
info.awaiting_result = false;
info.result = ok; // authoritative final
info.result_code = ok ? ExecResultCode::SUCCESS : ExecResultCode::FAILURE;
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] Finalizing node %s result=%s",
kv.first.c_str(), info.result ? "SUCCESS" : "FAILURE");
}
}
// Per-instance worker threads handle updating node status; avoid mutating shared state here.
});
}
@@ -409,6 +359,10 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
}
epoch_filter_.reset_epoch(new_epoch, seq_skills);
per_node_exec_.clear();
{
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
}
bt_pending_run_ = true;
RCLCPP_INFO(this->get_logger(), "BT built from file epoch=%llu", static_cast<unsigned long long>(new_epoch));
}
@@ -447,6 +401,10 @@ void CerebrumNode::UpdateBehaviorTree()
}
epoch_filter_.reset_epoch(new_epoch, seq_skills);
per_node_exec_.clear();
{
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
}
bt_pending_run_ = true;
RCLCPP_INFO(this->get_logger(), "BT updated epoch=%llu sequence=%s",
static_cast<unsigned long long>(new_epoch), active_sequence_.c_str());
@@ -474,93 +432,84 @@ void CerebrumNode::RegisterSkillBtActions()
const std::string bt_type = skill.name + std::string("_H");
BTActionHandlers handlers;
handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode & node) {
auto & info = per_node_exec_[bt_type];
const std::string instance_name = node.name();
// If this specific BT node instance already finished successfully earlier in this tree run,
// short-circuit to SUCCESS so that retries only re-execute the failed node.
if (info.done_instances.find(instance_name) != info.done_instances.end()) {
RCLCPP_INFO(this->get_logger(), "[%s/%s] already succeeded, skip", bt_type.c_str(), instance_name.c_str());
return BT::NodeStatus::SUCCESS;
const std::string key = bt_type + std::string("|") + instance_name;
{
std::lock_guard<std::mutex> lk(exec_mutex_);
if (done_instances_epoch_.find(key) != done_instances_epoch_.end()) {
// RCLCPP_INFO(this->get_logger(), "[%s/%s] skipped: already SUCCESS in current epoch", bt_type.c_str(), instance_name.c_str());
return BT::NodeStatus::SUCCESS;
}
}
// If a previous instance for this skill type is still in flight, keep running.
if (info.in_flight) {
auto & inst = per_instance_exec_[key];
if (inst.in_flight.load(std::memory_order_acquire) && !inst.finished.load(std::memory_order_acquire)) {
robot_work_info_.bt_node_status = "RUNNING";
return BT::NodeStatus::RUNNING;
}
info.in_flight = true;
info.result = false;
info.result_code = ExecResultCode::UNKNOWN;
info.start_time = this->now();
info.skill_name = skill.name;
info.last_progress = 0.0;
info.cancel_state = PerNodeExecInfo::CancelState::NONE;
info.current_instance = instance_name;
if (!DeclareBtActionParamsForSkillInstance(skill.name, instance_name) ||
!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
// Init per-instance state and launch worker
inst.in_flight = true;
inst.finished = false;
inst.success = false;
inst.result_code = ExecResultCode::UNKNOWN;
inst.start_time = this->now();
if (!DeclareBtActionParamsForSkillInstance(skill.name, instance_name) ||
!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
RCLCPP_ERROR(this->get_logger(), "ExecuteBtAction server unavailable");
info.in_flight = false;
info.result = false;
info.result_code = ExecResultCode::FAILURE;
single_skill_goal_override_.reset();
info.current_instance.reset();
inst.in_flight = false;
inst.finished = true;
inst.success = false;
inst.result_code = ExecResultCode::FAILURE;
return BT::NodeStatus::FAILURE;
}
// Override goal builder to send only this skill.
single_skill_goal_override_ = skill.name;
single_skill_instance_override_ = instance_name;
action_registry_->send(brain::kExecuteBtActionName, this->get_logger());
RCLCPP_INFO(this->get_logger(), "[%s/%s] BTAction on_start", bt_type.c_str(), instance_name.c_str());
// Clear override after send to avoid affecting other triggers.
single_skill_goal_override_.reset();
single_skill_instance_override_.reset();
interfaces::action::ExecuteBtAction::Goal goal;
goal.epoch = static_cast<uint32_t>(epoch_filter_.epoch());
goal.action_name = skill.name;
goal.calls.clear();
goal.calls.push_back(BuildSkillCallForSkill(skill.name, instance_name));
auto logger = this->get_logger();
std::thread([this, key, goal, logger]() {
using EBA = interfaces::action::ExecuteBtAction;
auto res = action_registry_->send_and_wait_with_goal<EBA>(
brain::kExecuteBtActionName, logger, goal,
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(node_timeout_sec_)));
auto it = per_instance_exec_.find(key);
if (it == per_instance_exec_.end()) { return; }
auto & inst = it->second;
if (!res.has_value()) {
inst.success = false;
inst.result_code = ExecResultCode::FAILURE;
} else {
const auto & wrapped = res.value();
const bool ok = (wrapped.code == rclcpp_action::ResultCode::SUCCEEDED && wrapped.result && wrapped.result->success);
inst.success = ok;
inst.result_code = ok ? ExecResultCode::SUCCESS : ExecResultCode::FAILURE;
}
inst.in_flight = false;
inst.finished = true;
}).detach();
RCLCPP_INFO(this->get_logger(), "[%s/%s] launched per-instance ExecuteBtAction goal", bt_type.c_str(), instance_name.c_str());
robot_work_info_.bt_node_status = "RUNNING";
return BT::NodeStatus::RUNNING;
};
handlers.on_running = [this, bt_type](rclcpp::Node *, BT::TreeNode & node) {
auto it = per_node_exec_.find(bt_type);
if (it == per_node_exec_.end()) {return BT::NodeStatus::FAILURE;}
auto & info = it->second;
const std::string instance_name = node.name();
// If this instance is already marked done (from a previous retry cycle), succeed immediately.
if (info.done_instances.find(instance_name) != info.done_instances.end()) {
return BT::NodeStatus::SUCCESS;
}
if (info.in_flight) {
double elapsed = (this->now() - info.start_time).seconds();
if (elapsed > node_timeout_sec_) {
info.in_flight = false;
info.result = false;
info.result_code = ExecResultCode::FAILURE;
RCLCPP_WARN(this->get_logger(), "[%s/%s] BTAction on_running timeout after %.2f sec", bt_type.c_str(), instance_name.c_str(), elapsed);
action_registry_->cancel(brain::kExecuteBtActionName, this->get_logger());
// return BT::NodeStatus::FAILURE;
}
const std::string key = bt_type + std::string("|") + instance_name;
auto it = per_instance_exec_.find(key);
if (it == per_instance_exec_.end()) { return BT::NodeStatus::FAILURE; }
auto & inst = it->second;
if (!inst.finished.load(std::memory_order_acquire)) {
return BT::NodeStatus::RUNNING;
}
if (info.awaiting_result) {
double wait_elapsed = (this->now() - info.end_feedback_time).seconds();
if (wait_elapsed > result_wait_warn_sec_) {
RCLCPP_WARN_THROTTLE(
this->get_logger(), *this->get_clock(), 2000,
"Skill %s awaiting final result %.2fs after END feedback",
bt_type.c_str(), wait_elapsed);
}
return BT::NodeStatus::RUNNING;
}
// Terminal state for this instance
if (info.result) {
// Mark this instance as done so future retries skip it
if (!instance_name.empty()) {
info.done_instances.insert(instance_name);
}
info.current_instance.reset();
// Set a fixed cooldown between actions on SUCCESS
g_bt_resume_time_steady = std::chrono::steady_clock::now() + kInterActionDelayMs;
return BT::NodeStatus::SUCCESS;
}
info.current_instance.reset();
// Set a fixed cooldown between actions on FAILURE as well
g_bt_resume_time_steady = std::chrono::steady_clock::now() + kInterActionDelayMs;
if (inst.success) {
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.insert(key);
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
};
handlers.on_halted = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
@@ -601,7 +550,7 @@ bool CerebrumNode::UpdateBtActionParamsForSkillInstance(
RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill instance %s", instance_name.c_str());
try {
this->set_parameter(rclcpp::Parameter(p_payload, instance_params));
RCLCPP_INFO(this->get_logger(), "Seeded sample payload for %s instance %s", skill_name.c_str(), instance_name.c_str());
// RCLCPP_INFO(this->get_logger(), "Seeded sample payload for %s instance %s", skill_name.c_str(), instance_name.c_str());
} catch (const std::exception & e) {
RCLCPP_ERROR(this->get_logger(), "[%s] seeding sample payload failed: %s", skill_name.c_str(), e.what());
return false;
@@ -647,8 +596,8 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
return false;
} else {
instance_params = *params;
RCLCPP_INFO(this->get_logger(), "Loaded BT params for %s, instance_params: %s",
instance_name.c_str(), instance_params.c_str());
// RCLCPP_INFO(this->get_logger(), "Loaded BT params for %s, instance_params: %s",
// instance_name.c_str(), instance_params.c_str());
}
} catch (const std::exception & e) {
RCLCPP_ERROR(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
@@ -705,7 +654,7 @@ void CerebrumNode::SetupDynamicParameterHandling()
break;
}
const std::string field = name.substr(last_dot + 1);
if (field != "topic" && field != "payload_yaml") {
if (field != "topic" && field != "payload_yaml" && field != "timeout_ms") {
// Reject unknown fields under this namespace to avoid typos
result.successful = false;
result.reason = "unsupported field for cerebrum.bt.*";
@@ -735,6 +684,8 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::strin
{
interfaces::msg::SkillCall call;
call.name = skill_name;
call.instance_name = "";
call.timeout_ms = 0;
// Interface type/kind inference from SkillSpec
if (!skill_manager_) {return call;}
auto it = skill_manager_->skills().find(skill_name);
@@ -752,6 +703,7 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::strin
const std::string ns = std::string("cerebrum.bt.") + skill_name + ".";
const std::string p_topic = ns + "topic";
const std::string p_payload = ns + "payload_yaml";
const std::string p_timeout = ns + "timeout_ms";
try {
if (this->has_parameter(p_topic)) {
call.topic = this->get_parameter(p_topic).as_string();
@@ -759,6 +711,15 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::strin
if (this->has_parameter(p_payload)) {
call.payload_yaml = this->get_parameter(p_payload).as_string();
}
if (this->has_parameter(p_timeout)) {
auto v = this->get_parameter(p_timeout).as_string();
try {
long parsed = std::stol(v);
if (parsed > 0) call.timeout_ms = static_cast<uint32_t>(parsed);
} catch (...) {
RCLCPP_WARN(this->get_logger(), "[%s] invalid timeout_ms value '%s'", skill_name.c_str(), v.c_str());
}
}
} catch (const std::exception & e) {
RCLCPP_WARN(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
}
@@ -784,6 +745,7 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
const std::string ns = std::string("cerebrum.bt.") + skill_name + "." + *instance_name + ".";
const std::string p_topic = ns + "topic";
const std::string p_payload = ns + "payload_yaml";
const std::string p_timeout = ns + "timeout_ms";
try {
// topic override
if (this->has_parameter(p_topic)) {
@@ -795,6 +757,21 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
auto v = this->get_parameter(p_payload).as_string();
if (!v.empty()) base.payload_yaml = v;
}
base.instance_name = *instance_name;
if (this->has_parameter(p_timeout)) {
auto v = this->get_parameter(p_timeout).as_string();
try {
long parsed = std::stol(v);
if (parsed > 0) base.timeout_ms = static_cast<uint32_t>(parsed);
} catch (...) {
RCLCPP_WARN(this->get_logger(), "[%s/%s] invalid timeout_ms value '%s'", skill_name.c_str(), instance_name->c_str(), v.c_str());
}
}
if (base.instance_name == "s4_arm_grasp_box" || base.instance_name == "s5_arm_pickup_box" ||
base.instance_name == "s16_arm_grasp_box" || base.instance_name == "s17_arm_pickup_box") {
base.timeout_ms = 500;
}
} catch (const std::exception & e) {
RCLCPP_WARN(this->get_logger(), "[%s/%s] read instance params failed: %s", skill_name.c_str(), instance_name->c_str(), e.what());
}
@@ -963,6 +940,58 @@ void CerebrumNode::CancelActiveExecuteBtGoal()
}
}
void CerebrumNode::StopBehaviorTree(bool cancel_goals)
{
// 1) Cancel any active action goals
if (cancel_goals) {
try {
CancelActiveExecuteBtGoal();
} catch (...) {
RCLCPP_ERROR(this->get_logger(), "Exception while cancelling ExecuteBtAction goals");
}
}
// 2) Invalidate in-flight feedback/results by bumping epoch
const uint64_t new_epoch = epoch_filter_.epoch() + 1;
epoch_filter_.reset_epoch(new_epoch, {});
// 3) Halt the BT (forces nodes to IDLE)
try {
tree_.haltTree();
} catch (...) {
RCLCPP_ERROR(this->get_logger(), "Exception halting Behavior Tree");
}
// 4) Stop periodic ticking and clear cooldown gate
bt_pending_run_ = false;
g_bt_resume_time_steady = std::chrono::steady_clock::time_point{};
// 5) Mark any per-instance execution as cancelled and finished
for (auto & kv : per_instance_exec_) {
auto & inst = kv.second;
if (inst.in_flight.load(std::memory_order_acquire) && !inst.finished.load(std::memory_order_acquire)) {
inst.in_flight.store(false, std::memory_order_release);
inst.finished.store(true, std::memory_order_release);
inst.success = false;
inst.result_code = ExecResultCode::CANCELLED;
}
}
per_node_exec_.clear();
{
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
}
// 6) Update work info
if (robot_work_info_.task.size() > 2) {
robot_work_info_.task[2] = "None"; // current
}
robot_work_info_.bt_node_status = "CANCELLED";
robot_work_time_.total_seconds += (rclcpp::Clock().now() - robot_work_time_.start).seconds();
RCLCPP_INFO(this->get_logger(), "Behavior Tree stopped (epoch=%llu)", static_cast<unsigned long long>(new_epoch));
}
/**
* @brief Legacy epoch skill setter (now a no-op; filtering handled elsewhere).
* @param skills Ignored.
@@ -1071,6 +1100,22 @@ void CerebrumNode::CreateServices()
[this](
const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
std::shared_ptr<interfaces::srv::BtRebuild::Response> resp) {
if (req->config == "CancelBTTask") {
if (!bt_pending_run_) {
// Even if not pending, still call StopBehaviorTree to ensure cleanup states.
StopBehaviorTree(true);
resp->success = true;
resp->message = "No active BT (cleanup applied)";
RCLCPP_WARN(this->get_logger(), "No active BT to cancel (cleanup applied)");
return;
}
StopBehaviorTree(true);
resp->success = true;
resp->message = "Cancel BTTask success";
return;
}
// Check if rebuild is allowed
if (bt_pending_run_ && !allow_bt_rebuild_during_execution_) {
resp->success = false;
@@ -1079,7 +1124,7 @@ void CerebrumNode::CreateServices()
}
if (req->type == "Trigger") {
HandleTriggerRebuild(resp);
HandleTriggerRebuild(req, resp);
} else if (req->type == "Remote") {
HandleRemoteRebuild(req, resp);
} else if (req->type == "Local") {
@@ -1107,11 +1152,32 @@ void CerebrumNode::CreateServices()
* @param resp
*/
void CerebrumNode::HandleTriggerRebuild(
const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp)
{
if (req->config == "CancelBTTask") {
try {
tree_.haltTree();
RCLCPP_INFO(this->get_logger(), "halting previous BT ok, CancelBTTask");
} catch (...) {
// Swallow halt exceptions.
RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT CancelBTTask");
}
resp->success = true;
resp->message = "Rebuild triggered";
return;
}
for (const auto & path_param : bt_config_params_paths_) {
// path_param is a pair {config, param}; compare the config path string against the current path
if (path_param.config == current_bt_config_params_path_.config) {
// if (path_param.config == current_bt_config_params_path_.config) {
// continue;
// }
std::string sch_filename = ExtractFilenameWithoutExtension(path_param.config);
if (sch_filename != req->config) {
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service INVALID config: %s, local: %s", req->config.c_str(), sch_filename.c_str());
continue;
}
@@ -1119,9 +1185,9 @@ void CerebrumNode::HandleTriggerRebuild(
current_bt_config_params_path_ = path_param;
// Update working info
std::string scheduled = ExtractFilenameWithoutExtension(path_param.config);
// std::string scheduled = ExtractFilenameWithoutExtension(path_param.config);
if (robot_work_info_.task.size() > 2) {
robot_work_info_.task[2] = scheduled; // current
robot_work_info_.task[2] = sch_filename; // current
}
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Switching to BT config file path: %s", path_param.config.c_str());
@@ -1288,9 +1354,9 @@ void CerebrumNode::RobotWorkInfoPublish()
for (auto & task : msg.task) {
if (task == "bt_carry_boxes_sch1") {
task = "StockIn";
} else if (task == "bt_carry_boxes_sch2") {
task = "StockOut";
} else if (task == "bt_carry_boxes_sch2") {
task = "StockIn";
}
}

56
src/brain_interfaces/CMakeLists.txt
View File

@@ -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()

5
src/brain_interfaces/msg/Movej.msg
View File

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

23
src/brain_interfaces/package.xml
View File

@@ -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>

8
src/brain_interfaces/srv/VisionDemo.srv
View File

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

62
src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp
View File

@@ -44,64 +44,26 @@ 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 = "bottle";
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;
//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);
//target_frame: bottle, grab_width: 0.046393, target_pose: -0.0067, -0.0278, 0.2976, -0.1379, -0.0965, 0.5650, 0.8078
pose1.position.x = -0.0067;
pose1.position.y = -0.0278;
pose1.position.z = 0.2976;
pose1.orientation.x = -0.1379;
pose1.orientation.y = -0.0965;
pose1.orientation.z = 0.5650;
pose1.orientation.w = 0.8078;
obj1.pose = pose1;
obj1.grab_width = 0.046393;
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;

4
src/scripts/build.sh
View File

@@ -1,4 +1,4 @@
#!/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
colcon build --packages-select smacc2_msgs smacc2 brain
#colcon build --packages-select smacc2_msgs smacc2 brain arm_control leg_control vision_object_recg hand_control

0
src/scripts/euler_to_equa.py Normal file → Executable file
View File

0
src/scripts/euler_to_quaternion.py Normal file → Executable file
View File

16
src/scripts/gen_bt_params.py Normal file → Executable file
View File

@@ -116,25 +116,25 @@ 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 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
@@ -239,7 +239,9 @@ def main():
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)
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)
results.append({'name': instance_name, 'params': yaml_params})

0
src/scripts/gen_payload_converters.py Normal file → Executable file
View File

0
src/scripts/gen_robot_config.py Normal file → Executable file
View File

0
src/scripts/hand_eye_cali.py Normal file → Executable file
View File

0
src/scripts/images_to_quaternions.py Normal file → Executable file
View File

0
src/scripts/rotate_pose.py Normal file → Executable file
View File

65
src/thirdparty/rm_arm/include/arm_define.h vendored Normal file
View File

@@ -0,0 +1,65 @@
#ifndef ARM_DEFINE_H
#define ARM_DEFINE_H
#include "arm_action_define.h"
#define MAX_ARM_GOAL_COUNT 16
#define MAX_TRAJECTORY_HISTORY 64
#define ARM_FOLLOWING_PERIOD (5e6) //ns == 5ms
#define ARM_FOLLOWING_CHECKING_STEP (4)
#define ARM_FOLLOWING_CHECKING (ARM_FOLLOWING_PERIOD * ARM_FOLLOWING_CHECKING_STEP) //ns == 20ms
#define GET_FUNC_LINE_STR() \
(std::string(__func__ ) + ":" + std::to_string(__LINE__))
#define LEFT_ARM_IP_ADDRESS "192.168.2.2"
#define RIGHT_ARM_IP_ADDRESS "192.168.2.18"
#define ARM_IP_PORT (8080)
#define TRAJECTORY_COMPLETE 1
#define TRAJECTORY_ONGOING 0
#define TRAJECTORY_ERROR -1
/*typedef enum {
ARM_COMMAND_TYPE_ANGLE_STEP_ON = 0,
ARM_COMMAND_TYPE_POSE_STEP_ON,
ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE,
ARM_COMMAND_TYPE_POSE_DIRECT_MOVE,
ARM_COMMAND_TYPE_ERR
} ArmCommandTypeE;
typedef enum {
POSE_POSITION_X = 0,
POSE_POSITION_Y,
POSE_POSITION_Z,
POSE_QUATERNION_X,
POSE_QUATERNION_Y,
POSE_QUATERNION_Z,
POSE_QUATERNION_W,
POSE_DIMENSION, // should be 7
} PoseDimensionE;
typedef enum {
LEFT_ARM = 0,
RIGHT_ARM,
ARM_ID_ERR
} ArmIdE;
typedef enum {
OK = 0,
UNKNOWN_ERR = -1,
ARM_NOW_FORCE_MOVING = -2,
ARM_COLLISION = -3,
ARM_AIM_CANNOT_REACH = -4,
ARM_NOW_NO_GOAL = -5,
ARM_GOAL_CANCELLED = -6,
} ErrCodeE;*/
typedef enum {
GOAL_TYPE_MOVING = 0,
GOAL_TYPE_STEPPING,
GOAL_TYPE_POSE_STEPPING,
GOAL_TYPE_ERROR
} GoalTypeE;
#endif // ARM_DEFINE_H

1035
src/thirdparty/rm_arm/include/rm_define.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

4533
src/thirdparty/rm_arm/include/rm_interface.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

16
src/thirdparty/rm_arm/include/rm_interface_global.h vendored Normal file
View File

@@ -0,0 +1,16 @@
#ifndef RM_INTERFACE_GLOBAL_H
#define RM_INTERFACE_GLOBAL_H
#ifdef __linux
#define RM_INTERFACE_EXPORT
#endif
#if _WIN32
#if defined(RM_INTERFACE_LIBRARY)
# define RM_INTERFACE_EXPORT __declspec(dllexport)
#else
# define RM_INTERFACE_EXPORT __declspec(dllexport)
#endif
#endif
#endif // RM_INTERFACE_GLOBAL_H

4553
src/thirdparty/rm_arm/include/rm_service.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

16
src/thirdparty/rm_arm/include/rm_version.h vendored Normal file
View File

@@ -0,0 +1,16 @@
#ifndef RM_VERSION_H
#define RM_VERSION_H
#ifdef __cplusplus
extern "C" {
#endif
#define SDK_VERSION ("1.1.1")
#ifdef __cplusplus
}
#endif
#endif

BIN
src/thirdparty/rm_arm/lib/api_cpp.dll vendored Normal file
View File

Binary file not shown.

BIN
src/thirdparty/rm_arm/lib/api_cpp.lib vendored Normal file
View File

Binary file not shown.

BIN
src/thirdparty/rm_arm/lib/libapi_cpp.so vendored Normal file
View File

Binary file not shown.