Compare commits
13 Commits
eed410e776
...
e4684bbe70
| Author | SHA1 | Date | |
|---|---|---|---|
| e4684bbe70 | |||
| 9fe000911f | |||
| b4b3c3b80c | |||
| 1b69632c7b | |||
| f9992d8437 | |||
| 0992caae72 | |||
| 4f679c77ae | |||
| 1951dd8276 | |||
| e8f1021af1 | |||
| 79ade2d741 | |||
| 53e62e2714 | |||
| 3d79cf2eb8 | |||
| 9ecdb1be60 |
@@ -34,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
|
||||
|
||||
@@ -66,6 +71,7 @@ ament_target_dependencies(brain_node
|
||||
target_link_libraries(brain_node
|
||||
Boost::thread
|
||||
yaml-cpp
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so
|
||||
)
|
||||
|
||||
install(TARGETS brain_node
|
||||
@@ -161,9 +167,12 @@ if(BUILD_TESTING)
|
||||
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 ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
|
||||
endif()
|
||||
|
||||
ament_add_gtest(test_cerebrum_node
|
||||
@@ -188,9 +197,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 ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
|
||||
endif()
|
||||
|
||||
ament_add_gtest(test_sm_cerebellum
|
||||
@@ -209,7 +221,11 @@ if(BUILD_TESTING)
|
||||
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 ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
|
||||
endif()
|
||||
|
||||
# ExecuteBtAction end-to-end single skill test
|
||||
@@ -234,8 +250,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 ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
|
||||
endif()
|
||||
|
||||
# Extended ExecuteBtAction scenario tests (multi-skill, failure, RUN interpolation, timeout, stats, cancel)
|
||||
@@ -260,8 +279,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 ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
|
||||
endif()
|
||||
|
||||
# Additional isolated ExecuteBtAction scenario tests
|
||||
@@ -276,8 +298,11 @@ if(BUILD_TESTING)
|
||||
target_compile_definitions(${test_name} PRIVATE BRAIN_DISABLE_SM=1)
|
||||
ament_target_dependencies(${test_name}
|
||||
rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp 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)
|
||||
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 ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
|
||||
endif()
|
||||
endforeach()
|
||||
|
||||
@@ -291,8 +316,11 @@ if(BUILD_TESTING)
|
||||
target_compile_definitions(test_cerebrum_utils PRIVATE BRAIN_DISABLE_SM=1)
|
||||
ament_target_dependencies(test_cerebrum_utils
|
||||
rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp 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)
|
||||
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 ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
|
||||
Binary file not shown.
@@ -11,7 +11,7 @@
|
||||
|
||||
'
|
||||
- name: s1_arm_move_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -25,7 +25,7 @@
|
||||
|
||||
'
|
||||
- name: s2_arm_move_to_snapshot_pose
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
|
||||
'
|
||||
- name: s3_snapshot_action1
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -59,7 +59,7 @@
|
||||
|
||||
'
|
||||
- name: s3_snapshot_action2
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -73,7 +73,7 @@
|
||||
|
||||
'
|
||||
- name: s3_snapshot_top_action1
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -87,7 +87,7 @@
|
||||
|
||||
'
|
||||
- name: s3_snapshot_top_action2
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -101,7 +101,7 @@
|
||||
|
||||
'
|
||||
- name: s3_snapshot_top_action3
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -115,7 +115,7 @@
|
||||
|
||||
'
|
||||
- name: s4_arm_prepare_grasp_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -129,7 +129,7 @@
|
||||
|
||||
'
|
||||
- name: s4_arm_grasp_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -143,7 +143,7 @@
|
||||
|
||||
'
|
||||
- name: s5_arm_pickup_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -175,7 +175,7 @@
|
||||
|
||||
'
|
||||
- name: s9_arm_put_down_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -189,7 +189,7 @@
|
||||
|
||||
'
|
||||
- name: s10_arm_release
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -203,7 +203,7 @@
|
||||
|
||||
'
|
||||
- name: s10_arm_raise_up
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -229,7 +229,7 @@
|
||||
|
||||
'
|
||||
- name: s13_arm_move_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -243,7 +243,7 @@
|
||||
|
||||
'
|
||||
- name: s14_arm_move_to_snapshot_pose
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -257,7 +257,7 @@
|
||||
|
||||
'
|
||||
- name: s14_arm_move_pre_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -271,7 +271,7 @@
|
||||
|
||||
'
|
||||
- name: s14_arm_move_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -291,7 +291,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_action1
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -305,7 +305,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_action2
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -319,7 +319,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_top_action1
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -333,7 +333,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_top_action2
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -347,7 +347,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_top_action3
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -361,7 +361,7 @@
|
||||
|
||||
'
|
||||
- name: s16_arm_prepare_grasp_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -375,7 +375,7 @@
|
||||
|
||||
'
|
||||
- name: s16_arm_grasp_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -389,7 +389,7 @@
|
||||
|
||||
'
|
||||
- name: s17_arm_pickup_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -421,7 +421,7 @@
|
||||
|
||||
'
|
||||
- name: s21_arm_put_down_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -435,7 +435,7 @@
|
||||
|
||||
'
|
||||
- name: s22_arm_release
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -449,7 +449,7 @@
|
||||
|
||||
'
|
||||
- name: s22_arm_raise_up
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -475,7 +475,7 @@
|
||||
|
||||
'
|
||||
- name: s25_arm_move_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -495,7 +495,7 @@
|
||||
|
||||
'
|
||||
- name: s26_arm_move_pre_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -509,7 +509,7 @@
|
||||
|
||||
'
|
||||
- name: s26_arm_move_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
|
||||
'
|
||||
- name: s1_arm_move_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -39,7 +39,7 @@
|
||||
|
||||
'
|
||||
- name: s4_arm_move_pre_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -53,7 +53,7 @@
|
||||
|
||||
'
|
||||
- name: s4_arm_move_to_snapshot_pose
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -67,7 +67,7 @@
|
||||
|
||||
'
|
||||
- name: s4_snapshot_action1
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -81,7 +81,7 @@
|
||||
|
||||
'
|
||||
- name: s4_snapshot_action2
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -95,7 +95,7 @@
|
||||
|
||||
'
|
||||
- name: s4_snapshot_top_action1
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -109,7 +109,7 @@
|
||||
|
||||
'
|
||||
- name: s4_snapshot_top_action2
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -123,7 +123,7 @@
|
||||
|
||||
'
|
||||
- name: s4_snapshot_top_action3
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -137,7 +137,7 @@
|
||||
|
||||
'
|
||||
- name: s5_arm_prepare_to_grasp
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -151,7 +151,7 @@
|
||||
|
||||
'
|
||||
- name: s6_arm_grasp_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -165,7 +165,7 @@
|
||||
|
||||
'
|
||||
- name: s7_arm_pickup_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -197,7 +197,7 @@
|
||||
|
||||
'
|
||||
- name: s11_arm_putdown_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -211,7 +211,7 @@
|
||||
|
||||
'
|
||||
- name: s12_arm_release_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -225,7 +225,7 @@
|
||||
|
||||
'
|
||||
- name: s12_arm_raise_up
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -239,7 +239,7 @@
|
||||
|
||||
'
|
||||
- name: s13_arm_move_to_snapshot_pose
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -253,7 +253,7 @@
|
||||
|
||||
'
|
||||
- name: s13_arm_move_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -291,7 +291,7 @@
|
||||
|
||||
'
|
||||
- name: s15_arm_move_pre_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -305,7 +305,7 @@
|
||||
|
||||
'
|
||||
- name: s15_arm_move_to_snapshot_pose
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -319,7 +319,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_action1
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -333,7 +333,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_action2
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -347,7 +347,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_top_action1
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -361,7 +361,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_top_action2
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -375,7 +375,7 @@
|
||||
|
||||
'
|
||||
- name: s15_snapshot_top_action3
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -389,7 +389,7 @@
|
||||
|
||||
'
|
||||
- name: s16_arm_prepare_to_grasp
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -403,7 +403,7 @@
|
||||
|
||||
'
|
||||
- name: s17_arm_grasp_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -417,7 +417,7 @@
|
||||
|
||||
'
|
||||
- name: s18_arm_pickup_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -449,7 +449,7 @@
|
||||
|
||||
'
|
||||
- name: s22_arm_putdown_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -463,7 +463,7 @@
|
||||
|
||||
'
|
||||
- name: s23_arm_release_box
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -477,7 +477,7 @@
|
||||
|
||||
'
|
||||
- name: s23_arm_raise_up
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -491,7 +491,7 @@
|
||||
|
||||
'
|
||||
- name: s23_arm_move_to_snapshot_pose
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -505,7 +505,7 @@
|
||||
|
||||
'
|
||||
- name: s23_arm_move_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
@@ -525,7 +525,7 @@
|
||||
|
||||
'
|
||||
- name: s25_arm_move_origin_position
|
||||
params: 'body_id: 0
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
|
||||
@@ -6,12 +6,22 @@
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: VisionObjectRecognition_H
|
||||
params: '{}
|
||||
- name: gripper_open
|
||||
params: 'loc: 0
|
||||
|
||||
speed: 20
|
||||
|
||||
torque: 20
|
||||
|
||||
mode: 2
|
||||
|
||||
'
|
||||
- name: Arm_H
|
||||
params: 'body_id: 0
|
||||
- name: VisionObjectRecognition_H
|
||||
params: 'camera_position: right
|
||||
|
||||
'
|
||||
- name: right_arm_vision_grasp
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 0
|
||||
|
||||
@@ -24,9 +34,13 @@
|
||||
data_array: []
|
||||
|
||||
'
|
||||
- name: HandControl_H
|
||||
params: 'mode: 0
|
||||
- name: gripper_close
|
||||
params: 'loc: 30
|
||||
|
||||
effort: 0.0
|
||||
speed: 20
|
||||
|
||||
torque: 20
|
||||
|
||||
mode: 2
|
||||
|
||||
'
|
||||
|
||||
@@ -1,15 +1,17 @@
|
||||
<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">
|
||||
<!-- Retry vision + hand until GripperControl_H succeeds -->
|
||||
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
|
||||
<Sequence>
|
||||
<!-- <GripperCmd_H name="gripper_open" /> -->
|
||||
<VisionObjectRecognition_H name="VisionObjectRecognition_H" />
|
||||
<Arm_H name="Arm_H" />
|
||||
<!-- <Arm_H name="right_arm_vision_grasp" /> -->
|
||||
<!-- <GripperCmd_H name="gripper_close" /> -->
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
<!-- After Arm_H eventually succeeds, run the hand -->
|
||||
<HandControl_H name="HandControl_H" />
|
||||
<!-- <GripperCmd_H name="GripperCmd_H" /> -->
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
|
||||
@@ -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}
|
||||
]
|
||||
@@ -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"
|
||||
@@ -379,16 +379,16 @@ public:
|
||||
}
|
||||
|
||||
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;
|
||||
// }
|
||||
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();
|
||||
@@ -460,16 +460,16 @@ public:
|
||||
}
|
||||
|
||||
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;
|
||||
// }
|
||||
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();
|
||||
|
||||
66
src/brain/include/brain/calc_grasp_pose.hpp
Normal file
66
src/brain/include/brain/calc_grasp_pose.hpp
Normal 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
35
src/brain/include/brain/cerebellum_hooks.hpp
Normal 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_
|
||||
@@ -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,11 +43,19 @@ public:
|
||||
*/
|
||||
explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
||||
|
||||
std::vector<geometry_msgs::msg::Pose> pre_grasp_poses_;
|
||||
std::vector<geometry_msgs::msg::Pose> grasp_poses_;
|
||||
|
||||
std::vector<double> left_arm_joint_angles_;
|
||||
std::vector<double> right_arm_joint_angles_;
|
||||
std::mutex joint_state_mutex_;
|
||||
|
||||
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_;
|
||||
@@ -86,10 +96,17 @@ private:
|
||||
std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
|
||||
std::string target_frame_{"bottle"};
|
||||
int64_t command_id_{0};
|
||||
int64_t command_id_left_arm_ {0};
|
||||
int64_t command_id_right_arm_{0};
|
||||
std::unordered_map<std::string, geometry_msgs::msg::Pose> final_arm_pose_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
std::string arm_target_frame_{"base_link_leftarm"};
|
||||
double arm_transform_timeout_sec_{0.2};
|
||||
std::string arm_target_frame_{"base_link_rightarm"};
|
||||
double arm_transform_timeout_sec_{1};
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
|
||||
void JointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
|
||||
double grab_width_{0.0};
|
||||
|
||||
// Current accepted sequence epoch (from client) used to filter/reject stale goals
|
||||
std::atomic<uint32_t> current_epoch_{0};
|
||||
@@ -107,6 +124,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.
|
||||
@@ -136,26 +155,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.
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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>
|
||||
{
|
||||
|
||||
@@ -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());
|
||||
|
||||
252
src/brain/src/calc_grasp_pose.cpp
Normal file
252
src/brain/src/calc_grasp_pose.cpp
Normal file
@@ -0,0 +1,252 @@
|
||||
#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);
|
||||
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;
|
||||
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);
|
||||
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) {
|
||||
RCLCPP_ERROR(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
|
||||
793
src/brain/src/cerebellum_hooks.cpp
Normal file
793
src/brain/src/cerebellum_hooks.cpp
Normal file
@@ -0,0 +1,793 @@
|
||||
#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 {
|
||||
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);
|
||||
}
|
||||
|
||||
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 {
|
||||
if (node->target_frame_ == obj.class_name) {
|
||||
node->target_pose_[node->target_frame_].header = header;
|
||||
node->target_pose_[node->target_frame_].header.frame_id = "RightLink6";
|
||||
node->target_pose_[node->target_frame_].pose = obj.pose;
|
||||
// node->target_pose_[node->target_frame_].pose.position.x -= obj.grab_width / 2;
|
||||
// node->target_pose_[node->target_frame_].pose.position.z -= 0.175;
|
||||
node->grab_width_ = obj.grab_width;
|
||||
RCLCPP_INFO(node->get_logger(), "target_frame_: %s, grab_width_: %f, updated target_pose", node->target_frame_.c_str(), node->grab_width_);
|
||||
return true;
|
||||
} else {
|
||||
RCLCPP_WARN(node->get_logger(), "object class_name[%s] not match target_frame_[%s]",
|
||||
obj.class_name.c_str(), node->target_frame_.c_str());
|
||||
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);
|
||||
|
||||
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();
|
||||
best_angles.push_back(0);
|
||||
|
||||
{
|
||||
std::vector<std::string> palm_facings = {"up"}; //{"up", "down"};
|
||||
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_;
|
||||
}
|
||||
}
|
||||
|
||||
for (double angle : best_angles) {
|
||||
for (const auto& palm : palm_facings) {
|
||||
auto result = brain::GraspPoseCalculator::calculate(target_pos,
|
||||
target_quat, "side", angle, palm, 0.06, 0.175, node->arm_target_frame_);
|
||||
|
||||
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();
|
||||
|
||||
std::vector<double> pre_grasp_joint_angles = current_joint_angles;
|
||||
std::vector<double> grasp_joint_angles = current_joint_angles;
|
||||
|
||||
#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());
|
||||
#endif
|
||||
|
||||
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.pre_grasp_pose.position,
|
||||
result.pre_grasp_pose.orientation, pre_grasp_joint_angles) != 0) {
|
||||
RCLCPP_WARN(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;
|
||||
}
|
||||
|
||||
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();
|
||||
|
||||
#ifdef DEBUG_GRASP_POSE_CALCULATION
|
||||
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
|
||||
|
||||
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.grasp_pose.position,
|
||||
result.grasp_pose.orientation, grasp_joint_angles) != 0) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
|
||||
skill_name.c_str(), angle, palm.c_str());
|
||||
continue;
|
||||
}
|
||||
|
||||
node->pre_grasp_poses_.push_back(pre_grasp_msg);
|
||||
node->grasp_poses_.push_back(grasp_msg);
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Generated Pose (Angle: %.1f, Palm: %s)", angle, palm.c_str());
|
||||
RCLCPP_INFO(node->get_logger(), " Pre: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
|
||||
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(), " Grasp: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
|
||||
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());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
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();
|
||||
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();
|
||||
|
||||
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;
|
||||
}
|
||||
if (!CalculateGraspPoses(skill_name)) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
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
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
// Own header first.
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "brain/cerebellum_hooks.hpp"
|
||||
|
||||
// C / C++ standard library.
|
||||
#include <chrono>
|
||||
@@ -12,6 +13,7 @@
|
||||
#include <cmath>
|
||||
#include <future>
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <cstdint>
|
||||
@@ -54,7 +56,7 @@
|
||||
|
||||
namespace brain
|
||||
{
|
||||
static thread_local int tls_arm_body_id = -1;
|
||||
thread_local int CerebellumNode::tls_arm_body_id = -1;
|
||||
// 线程局部：当前ExecuteBtAction goal的calls（用于payload解析与topic解析）
|
||||
thread_local const std::vector<interfaces::msg::SkillCall>* CerebellumNode::tls_current_calls_ = nullptr;
|
||||
|
||||
@@ -86,14 +88,17 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
|
||||
tf_buffer_->setUsingDedicatedThread(true);
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
|
||||
joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/joint_states", 10, std::bind(&CerebellumNode::JointStateCallback, this, std::placeholders::_1));
|
||||
|
||||
action_registry_ = std::make_unique<ActionServerRegistry>(this);
|
||||
action_clients_ = std::make_unique<ActionClientRegistry>(this);
|
||||
skill_manager_ = std::make_unique<SkillManager>(this, action_clients_.get(), nullptr);
|
||||
|
||||
DeclareAndLoadParameters();
|
||||
// Register action/service hook overrides first so SkillManager will pick them up
|
||||
ConfigureActionHooks();
|
||||
ConfigureServiceHooks();
|
||||
CerebellumHooks::ConfigureActionHooks(this);
|
||||
CerebellumHooks::ConfigureServiceHooks(this);
|
||||
// Then load skills which will register action/service clients and honor the overrides
|
||||
LoadSkillsFile();
|
||||
SetupStatsTimerAndPublisher();
|
||||
@@ -110,589 +115,6 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for arm control.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureArmHooks()
|
||||
{
|
||||
// Arm
|
||||
SkillManager::ActionHookOptions<interfaces::action::Arm> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::Arm::Goal goal{};
|
||||
if (!TryParseCallPayload<interfaces::action::Arm::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "[%s]Received Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=",
|
||||
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);
|
||||
|
||||
goal.body_id = (tls_arm_body_id == LEFT_ARM ||
|
||||
tls_arm_body_id == RIGHT_ARM) ? tls_arm_body_id : LEFT_ARM;
|
||||
goal.command_id = ++command_id_;
|
||||
goal.frame_time_stamp = this->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;
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->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]);
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
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(this->get_logger(), "[%s] feedback is empty", skill_name.c_str());
|
||||
return;
|
||||
}
|
||||
(void)feedback;
|
||||
//TODO :
|
||||
// RCLCPP_INFO(this->get_logger(), "[%s] command_id: %ld, current progress: %d",
|
||||
// skill_name.c_str(), feedback->command_id, feedback->int_lenth);
|
||||
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
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;
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] Final position: %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f",
|
||||
skill_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(this->get_logger(), "[%s] command_id %ld completed: %s", skill_name.c_str(), res.result->command_id, message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::Arm>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::Arm>("Arm", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for hand control.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureHandControlHooks()
|
||||
{
|
||||
// HandControl
|
||||
SkillManager::ActionHookOptions<interfaces::action::HandControl> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::HandControl::Goal goal{};
|
||||
// Plan A: per-call YAML overrides
|
||||
if (!TryParseCallPayload<interfaces::action::HandControl::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(this->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 (!this->has_parameter(param)) {
|
||||
this->declare_parameter<std::vector<double>>(param, {0.0, 0.0, 0.0});
|
||||
}
|
||||
const auto values = this->get_parameter(param).as_double_array();
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Target parameters size=%zu", skill_name.c_str(), values.size());
|
||||
(void)values;
|
||||
goal.mode = 1;
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
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;
|
||||
//TODO :
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Current progress: %.2f", skill_name.c_str(), feedback->progress);
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
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(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::HandControl>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::HandControl>("HandControl", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for waist movement.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureMoveWaistHooks()
|
||||
{
|
||||
// MoveWaist
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveWaist> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::MoveWaist::Goal goal{};
|
||||
// Plan A: load per-call YAML payload into goal if present
|
||||
if (!TryParseCallPayload<interfaces::action::MoveWaist::Goal>(skill_name, goal)) {
|
||||
RCLCPP_INFO(this->get_logger(), "Loaded MoveWaist goal from YAML payload");
|
||||
return goal;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->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 = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::MoveWaist::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
//TODO
|
||||
(void)skill_name;
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
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(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::MoveWaist>("MoveWaist", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for leg movement.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureMoveLegHooks()
|
||||
{
|
||||
// MoveLeg
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveLeg> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::MoveLeg::Goal goal{};
|
||||
if (!TryParseCallPayload<interfaces::action::MoveLeg::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::MoveLeg::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
//TODO
|
||||
(void)skill_name;
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
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(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::MoveLeg>("MoveLeg", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for wheel movement.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureMoveWheelHooks()
|
||||
{
|
||||
// MoveWheel
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveWheel> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::MoveWheel::Goal goal{};
|
||||
// Plan A: per-call YAML overrides
|
||||
if (!TryParseCallPayload<interfaces::action::MoveWheel::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::MoveWheel::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
//TODO
|
||||
(void)skill_name;
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
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(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::MoveWheel>("MoveWheel", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for home movement.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureMoveHomeHooks()
|
||||
{
|
||||
// MoveHome
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveHome> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::MoveHome::Goal goal{};
|
||||
// Plan A: per-call YAML overrides (even though goal is empty)
|
||||
if (!TryParseCallPayload<interfaces::action::MoveHome::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::MoveHome::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
//TODO
|
||||
(void)skill_name;
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
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(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::MoveHome>("MoveHome", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for various movements.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureActionHooks()
|
||||
{
|
||||
if (!skill_manager_) {
|
||||
RCLCPP_WARN(this->get_logger(), "SkillManager unavailable, skipping action hook configuration");
|
||||
return;
|
||||
}
|
||||
ConfigureArmHooks();
|
||||
ConfigureHandControlHooks();
|
||||
ConfigureCameraTakePhotoHooks();
|
||||
ConfigureMoveWaistHooks();
|
||||
ConfigureMoveLegHooks();
|
||||
ConfigureMoveWheelHooks();
|
||||
ConfigureMoveHomeHooks();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for vision object recognition.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureVisionObjectRecognitionHooks()
|
||||
{
|
||||
using VisionSrv = interfaces::srv::VisionObjectRecognition;
|
||||
{
|
||||
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 = [this](const std::string & skill_name) {
|
||||
auto req = std::make_shared<VisionSrv::Request>();
|
||||
// Prefer payload_yaml from calls
|
||||
if (TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
|
||||
return req;
|
||||
}
|
||||
// fallback to parameter
|
||||
const std::string param = "vision_object_recognition.camera_position";
|
||||
if (!this->has_parameter(param)) {
|
||||
this->declare_parameter<std::string>(param, std::string("left"));
|
||||
}
|
||||
req->camera_position = this->get_parameter(param).as_string();
|
||||
return req;
|
||||
};
|
||||
hooks.on_response = [this](
|
||||
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(this->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(
|
||||
this->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());
|
||||
for (size_t i = 0; i < resp->objects.size(); ++i) {
|
||||
const auto & obj = resp->objects[i];
|
||||
|
||||
if (target_frame_ == obj.class_name) {
|
||||
target_pose_[target_frame_].header = resp->header;
|
||||
target_pose_[target_frame_].pose = obj.pose_list[0];
|
||||
RCLCPP_INFO(this->get_logger(), "target_frame_: %s, updated target_pose", target_frame_.c_str());
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), " [%zu] class='%s' id=%d poses=%zu", i,
|
||||
obj.class_name.c_str(), obj.class_id, obj.pose_list.size());
|
||||
for (size_t j = 0; j < obj.pose_list.size() && j < 2; ++j) {
|
||||
const auto & p = obj.pose_list[j];
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
" pose[%zu]: pos(%.3f, %.3f, %.3f) quat(%.3f, %.3f, %.3f, %.3f)", j,
|
||||
p.position.x, p.position.y, p.position.z,
|
||||
p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
|
||||
}
|
||||
if (obj.pose_list.size() > 2) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), " ... (%zu more poses omitted)", obj.pose_list.size() - 2);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
};
|
||||
skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));
|
||||
}
|
||||
}
|
||||
|
||||
void CerebellumNode::ConfigureCameraTakePhotoHooks()
|
||||
{
|
||||
// CameraTakePhoto
|
||||
SkillManager::ActionHookOptions<interfaces::action::CameraTakePhoto> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::CameraTakePhoto::Goal goal{};
|
||||
if (!TryParseCallPayload<interfaces::action::CameraTakePhoto::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
return goal;
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
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(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::CameraTakePhoto>("CameraTakePhoto", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumNode::ConfigureMapLoadHooks()
|
||||
{
|
||||
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 = [this](const std::string & skill_name) {
|
||||
auto req = std::make_shared<MapLoadSrv::Request>();
|
||||
if (TryParseCallPayload<MapLoadSrv::Request>(skill_name, *req)) { return req; }
|
||||
// Fallback to parameter
|
||||
if (!this->has_parameter("map_load_url")) {
|
||||
this->declare_parameter<std::string>("map_load_url", std::string(""));
|
||||
}
|
||||
req->map_url = this->get_parameter("map_load_url").as_string();
|
||||
return req;
|
||||
};
|
||||
hooks.on_response = [this](
|
||||
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(this->get_logger(), "[%s] MapLoad failed: %s", skill_name.c_str(), detail.c_str());
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] MapLoad: %s", skill_name.c_str(), detail.c_str());
|
||||
}
|
||||
return resp->success;
|
||||
};
|
||||
skill_manager_->configure_service_hooks<MapLoadSrv>("MapLoad", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumNode::ConfigureMapSaveHooks()
|
||||
{
|
||||
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 = [this](const std::string & skill_name) {
|
||||
auto req = std::make_shared<MapSaveSrv::Request>();
|
||||
if (TryParseCallPayload<MapSaveSrv::Request>(skill_name, *req)) { return req; }
|
||||
// Fallback to parameter group
|
||||
if (!this->has_parameter("map_save_url")) {
|
||||
this->declare_parameter<std::string>("map_save_url", std::string("/tmp/humanoid_map"));
|
||||
}
|
||||
req->map_url = this->get_parameter("map_save_url").as_string();
|
||||
return req;
|
||||
};
|
||||
hooks.on_response = [this](
|
||||
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(this->get_logger(), "[%s] MapSave failed: %s", skill_name.c_str(), detail.c_str());
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] MapSave: %s", skill_name.c_str(), detail.c_str());
|
||||
}
|
||||
return resp->success;
|
||||
};
|
||||
skill_manager_->configure_service_hooks<MapSaveSrv>("MapSave", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure Service hooks for various movements.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureServiceHooks()
|
||||
{
|
||||
if (!skill_manager_) {
|
||||
RCLCPP_WARN(this->get_logger(), "SkillManager unavailable, skipping service hook configuration");
|
||||
return;
|
||||
}
|
||||
ConfigureVisionObjectRecognitionHooks();
|
||||
ConfigureMapLoadHooks();
|
||||
ConfigureMapSaveHooks();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Declare all configurable ROS parameters and load their initial values.
|
||||
*
|
||||
@@ -1179,9 +601,32 @@ bool CerebellumNode::ExecuteActionSkill(
|
||||
if (timeout_ms_override > 0) {
|
||||
effective_timeout = std::chrono::milliseconds(timeout_ms_override);
|
||||
}
|
||||
return (skill == "Arm") ?
|
||||
ExecuteBilateralArmAction(skill, topic, effective_timeout, instance_name, timeout_ms_override, index, total_skills, goal_handle, detail) :
|
||||
RunActionSkillWithProgress(skill, topic, effective_timeout, instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
|
||||
|
||||
if (skill == "Arm") {
|
||||
interfaces::action::Arm::Goal goal{};
|
||||
if (!TryParseCallPayload<interfaces::action::Arm::Goal>(skill, goal)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (goal.body_id >= 2) { //left and right
|
||||
return ExecuteBilateralArmAction(skill, topic, effective_timeout,
|
||||
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
|
||||
} else if (goal.body_id == 0 || goal.body_id == 1) { //left or right
|
||||
tls_arm_body_id = goal.body_id;
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid body_id: %d for skill %s", goal.body_id, skill.c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return RunActionSkillWithProgress(skill, topic, effective_timeout,
|
||||
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
|
||||
|
||||
// return (skill == "Arm") ?
|
||||
// ExecuteBilateralArmAction(skill, topic, effective_timeout,
|
||||
// instance_name, timeout_ms_override, index, total_skills, goal_handle, detail) :
|
||||
// RunActionSkillWithProgress(skill, topic, effective_timeout,
|
||||
// instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -1275,15 +720,14 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
RCLCPP_INFO(this->get_logger(), "[Arm] send two goals: body_id=1 and body_id=0, index=%d, total_skills=%d", index, total_skills);
|
||||
RCLCPP_INFO(this->get_logger(), "[Arm] instance='%s' timeout=%ld ms", instance_name.c_str(), static_cast<long>(timeout_ms_override));
|
||||
|
||||
std::atomic<bool> finished1{false}, finished2{false};
|
||||
bool ok1 = false, ok2 = false;
|
||||
std::string d1, d2;
|
||||
|
||||
std::atomic<bool> right_arm_finished{false}, left_arm_finished{false};
|
||||
bool right_arm_ok = false, left_arm_ok = false;
|
||||
std::string right_arm_d, left_arm_d;
|
||||
auto left_arm_time_steady = std::chrono::steady_clock::now();
|
||||
auto right_arm_time_steady = std::chrono::steady_clock::now();
|
||||
|
||||
auto worker = [this, &ok_out = ok1, &done_flag = finished1, &d_out = d1,
|
||||
skill, topic, timeout, &goal_handle, &time_steady = left_arm_time_steady](int body_id) {
|
||||
auto left_arm_worker = [this, &ok_out = left_arm_ok, &done_flag = left_arm_finished, &d_out = left_arm_d,
|
||||
skill, topic, timeout, &goal_handle, &time_steady = left_arm_time_steady](int body_id) {
|
||||
tls_arm_body_id = body_id;
|
||||
const auto * prev_calls = CerebellumNode::tls_current_calls_;
|
||||
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
|
||||
@@ -1294,10 +738,10 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
time_steady = std::chrono::steady_clock::now();
|
||||
RCLCPP_WARN(this->get_logger(), "left_arm_time_steady2");
|
||||
};
|
||||
std::thread left_arm_thread(left_arm_worker, LEFT_ARM); // LEFT_ARM=0
|
||||
|
||||
std::thread t1(worker, RIGHT_ARM); // RIGHT_ARM=1
|
||||
auto worker2 = [this, &ok_out = ok2, &done_flag = finished2, &d_out = d2,
|
||||
skill, topic, timeout, &goal_handle, &time_steady = right_arm_time_steady](int body_id) {
|
||||
auto right_arm_worker = [this, &ok_out = right_arm_ok, &done_flag = right_arm_finished, &d_out = right_arm_d,
|
||||
skill, topic, timeout, &goal_handle, &time_steady = right_arm_time_steady](int body_id) {
|
||||
tls_arm_body_id = body_id;
|
||||
const auto * prev_calls = CerebellumNode::tls_current_calls_;
|
||||
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
|
||||
@@ -1308,11 +752,11 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
time_steady = std::chrono::steady_clock::now();
|
||||
RCLCPP_WARN(this->get_logger(), "right_arm_time_steady2");
|
||||
};
|
||||
std::thread t2(worker2, LEFT_ARM); // LEFT_ARM=0
|
||||
std::thread right_arm_thread(right_arm_worker, RIGHT_ARM); // RIGHT_ARM=1
|
||||
|
||||
const auto start_steady = std::chrono::steady_clock::now();
|
||||
|
||||
while (!(finished1.load(std::memory_order_acquire) && finished2.load(std::memory_order_acquire))) {
|
||||
while (!(right_arm_finished.load(std::memory_order_acquire) && left_arm_finished.load(std::memory_order_acquire))) {
|
||||
double elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start_steady).count();
|
||||
PublishFeedbackStage(goal_handle, "RUN", skill, elapsed_ms, "");
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
@@ -1322,30 +766,30 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
RCLCPP_WARN(this->get_logger(), "arm_exe_bias: %f", arm_exe_bias);
|
||||
|
||||
if (timeout_ms_override > 0 && std::abs(arm_exe_bias) > timeout_ms_override) {
|
||||
ok1 = false;
|
||||
ok2 = false;
|
||||
right_arm_ok = false;
|
||||
left_arm_ok = false;
|
||||
RCLCPP_ERROR(this->get_logger(), "Fail in Arm arm_exe_bias: %f, timeout_ms_override=%ld", arm_exe_bias, static_cast<long>(timeout_ms_override));
|
||||
}
|
||||
|
||||
if (t1.joinable()) {
|
||||
if (right_arm_thread.joinable()) {
|
||||
try {
|
||||
t1.join();
|
||||
right_arm_thread.join();
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception in Arm thread 1 join: %s", e.what());
|
||||
}
|
||||
}
|
||||
if (t2.joinable()) {
|
||||
if (left_arm_thread.joinable()) {
|
||||
try {
|
||||
t2.join();
|
||||
left_arm_thread.join();
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception in Arm thread 2 join: %s", e.what());
|
||||
}
|
||||
}
|
||||
|
||||
if (!ok1 || !ok2) {
|
||||
if (!right_arm_ok || !left_arm_ok) {
|
||||
std::ostringstream oss;
|
||||
if (!ok1) { oss << "Arm(body_id=1) failed"; if (!d1.empty()) oss << ": " << d1; }
|
||||
if (!ok2) { if (!oss.str().empty()) oss << "; "; oss << "Arm(body_id=0) failed"; if (!d2.empty()) oss << ": " << d2; }
|
||||
if (!right_arm_ok) { oss << "Arm(body_id=1) failed"; if (!right_arm_d.empty()) oss << ": " << right_arm_d; }
|
||||
if (!left_arm_ok) { if (!oss.str().empty()) oss << "; "; oss << "Arm(body_id=0) failed"; if (!left_arm_d.empty()) oss << ": " << left_arm_d; }
|
||||
detail = oss.str();
|
||||
return false;
|
||||
}
|
||||
@@ -1383,7 +827,7 @@ bool CerebellumNode::ExecuteServiceSkill(
|
||||
for (const auto & iface : spec.interfaces) {
|
||||
const auto pos = iface.rfind('.');
|
||||
if (pos == std::string::npos || pos + 1 >= iface.size()) {continue;}
|
||||
if (to_lower_str(iface.substr(pos + 1)) == "service") {
|
||||
if (to_lower_str(iface.substr(pos + 1)) == "srv") {
|
||||
service_base = iface.substr(0, pos);
|
||||
break;
|
||||
}
|
||||
@@ -1433,7 +877,7 @@ void CerebellumNode::LogStatsPeriodic()
|
||||
for (const auto & kv : skill_success_counts_) {
|
||||
size_t fails = skill_failure_counts_[kv.first];
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), " skill %s: OK=%zu FAIL=%zu", kv.first.c_str(), kv.second,
|
||||
this->get_logger(), "skill %s: OK=%zu FAIL=%zu", kv.first.c_str(), kv.second,
|
||||
fails);
|
||||
}
|
||||
if (stats_pub_) {
|
||||
@@ -1648,6 +1092,23 @@ std::string CerebellumNode::ResolveTopicForSkill(const std::string & skill) cons
|
||||
return c.topic;
|
||||
}
|
||||
}
|
||||
|
||||
if (skill_manager_) {
|
||||
const auto & skills = skill_manager_->skills();
|
||||
auto it = skills.find(skill);
|
||||
if (it != skills.end()) {
|
||||
const auto & spec = it->second;
|
||||
for (const auto & iface : spec.interfaces) {
|
||||
if (iface.find(".action") != std::string::npos || iface.find(".srv") != std::string::npos) {
|
||||
auto topic_it = spec.interface_default_topics.find(iface);
|
||||
if (topic_it != spec.interface_default_topics.end()) {
|
||||
return topic_it->second;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const std::string snake = ToSnake(skill);
|
||||
if (action_clients_->has_client(snake)) {return snake;}
|
||||
if (action_clients_->has_client(skill)) {return skill;}
|
||||
@@ -1811,4 +1272,31 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequenceConcurrent(
|
||||
return result;
|
||||
}
|
||||
|
||||
void CerebellumNode::JointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(joint_state_mutex_);
|
||||
if (msg->position.size() >= 21) {
|
||||
left_arm_joint_angles_.clear();
|
||||
// left_arm_subset = positions[9:15] -> indices 9 to 14
|
||||
for (size_t i = 9; i < 15; ++i) {
|
||||
double angle_deg = msg->position[i] * 180.0 / 3.14;
|
||||
left_arm_joint_angles_.push_back(angle_deg);
|
||||
}
|
||||
right_arm_joint_angles_.clear();
|
||||
// right_arm_subset = positions[15:21] -> indices 15 to 20
|
||||
for (size_t i = 15; i < 21; ++i) {
|
||||
double angle_deg = msg->position[i] * 180.0 / 3.14;
|
||||
right_arm_joint_angles_.push_back(angle_deg);
|
||||
}
|
||||
#ifdef DEBUG_JOINT_STATE
|
||||
RCLCPP_INFO(this->get_logger(), "Received left arm joint angle[%f, %f, %f, %f, %f, %f] ",
|
||||
left_arm_joint_angles_[0], left_arm_joint_angles_[1], left_arm_joint_angles_[2],
|
||||
left_arm_joint_angles_[3], left_arm_joint_angles_[4], left_arm_joint_angles_[5]);
|
||||
RCLCPP_INFO(this->get_logger(), "Received right arm joint angle[%f, %f, %f, %f, %f, %f] ",
|
||||
right_arm_joint_angles_[0], right_arm_joint_angles_[1], right_arm_joint_angles_[2],
|
||||
right_arm_joint_angles_[3], right_arm_joint_angles_[4], right_arm_joint_angles_[5]);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace brain
|
||||
|
||||
@@ -78,12 +78,12 @@ private:
|
||||
pose1.orientation.z = -0.19539139;
|
||||
pose1.orientation.w = 0.73153153;
|
||||
|
||||
obj1.pose_list.push_back(pose1);
|
||||
// obj1.pose_list.push_back(pose1);
|
||||
|
||||
// 第二个同类实例(示例)
|
||||
geometry_msgs::msg::Pose pose1b = pose1;
|
||||
pose1b.position.y += 0.05;
|
||||
obj1.pose_list.push_back(pose1b);
|
||||
// obj1.pose_list.push_back(pose1b);
|
||||
|
||||
interfaces::msg::PoseClassAndID obj2;
|
||||
obj2.class_name = "cup";
|
||||
@@ -93,15 +93,15 @@ private:
|
||||
pose2.position.y = -0.2;
|
||||
pose2.position.z = 0.78;
|
||||
pose2.orientation.w = 1.0;
|
||||
obj2.pose_list.push_back(pose2);
|
||||
// obj2.pose_list.push_back(pose2);
|
||||
|
||||
res->objects.clear();
|
||||
res->objects.emplace_back(obj1);
|
||||
res->objects.emplace_back(obj2);
|
||||
|
||||
std::ostringstream oss;
|
||||
oss << "Recognized " << res->objects.size() << " object classes (total instances: "
|
||||
<< (obj1.pose_list.size() + obj2.pose_list.size()) << ")";
|
||||
// oss << "Recognized " << res->objects.size() << " object classes (total instances: "
|
||||
// << (obj1.pose_list.size() + obj2.pose_list.size()) << ")";
|
||||
res->info = oss.str();
|
||||
} else {
|
||||
res->success = false;
|
||||
|
||||
0
src/scripts/euler_to_equa.py
Normal file → Executable file
0
src/scripts/euler_to_equa.py
Normal file → Executable file
0
src/scripts/euler_to_quaternion.py
Normal file → Executable file
0
src/scripts/euler_to_quaternion.py
Normal file → Executable file
16
src/scripts/gen_bt_params.py
Normal file → Executable file
16
src/scripts/gen_bt_params.py
Normal file → Executable 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
0
src/scripts/gen_payload_converters.py
Normal file → Executable file
0
src/scripts/gen_robot_config.py
Normal file → Executable file
0
src/scripts/gen_robot_config.py
Normal file → Executable file
0
src/scripts/hand_eye_cali.py
Normal file → Executable file
0
src/scripts/hand_eye_cali.py
Normal file → Executable file
0
src/scripts/images_to_quaternions.py
Normal file → Executable file
0
src/scripts/images_to_quaternions.py
Normal file → Executable file
0
src/scripts/rotate_pose.py
Normal file → Executable file
0
src/scripts/rotate_pose.py
Normal file → Executable file
65
src/thirdparty/rm_arm/include/arm_define.h
vendored
Normal file
65
src/thirdparty/rm_arm/include/arm_define.h
vendored
Normal 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
1035
src/thirdparty/rm_arm/include/rm_define.h
vendored
Normal file
File diff suppressed because it is too large
Load Diff
4533
src/thirdparty/rm_arm/include/rm_interface.h
vendored
Normal file
4533
src/thirdparty/rm_arm/include/rm_interface.h
vendored
Normal file
File diff suppressed because it is too large
Load Diff
16
src/thirdparty/rm_arm/include/rm_interface_global.h
vendored
Normal file
16
src/thirdparty/rm_arm/include/rm_interface_global.h
vendored
Normal 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
4553
src/thirdparty/rm_arm/include/rm_service.h
vendored
Normal file
File diff suppressed because it is too large
Load Diff
16
src/thirdparty/rm_arm/include/rm_version.h
vendored
Normal file
16
src/thirdparty/rm_arm/include/rm_version.h
vendored
Normal 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
BIN
src/thirdparty/rm_arm/lib/api_cpp.dll
vendored
Normal file
Binary file not shown.
BIN
src/thirdparty/rm_arm/lib/api_cpp.lib
vendored
Normal file
BIN
src/thirdparty/rm_arm/lib/api_cpp.lib
vendored
Normal file
Binary file not shown.
BIN
src/thirdparty/rm_arm/lib/libapi_cpp.so
vendored
Normal file
BIN
src/thirdparty/rm_arm/lib/libapi_cpp.so
vendored
Normal file
Binary file not shown.
Reference in New Issue
Block a user