Compare commits
14 Commits
feature-vi
...
develop
| Author | SHA1 | Date | |
|---|---|---|---|
| b728f050fc | |||
| 13f235c682 | |||
| 80217bbae7 | |||
| a39f6a6d2e | |||
| 6709fd510c | |||
| ce03b2c6c5 | |||
|
a02bc399b7 | ||
|
|
0e7b8570fd | ||
|
|
527c243617 | ||
|
|
32e15a7d9a | ||
|
|
aa74fc2397 | ||
|
|
97363f1e32 | ||
|
|
742e5ec67e | ||
|
|
7d7f602a16 |
@@ -8,6 +8,10 @@ endif()
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
|
||||
# hivecore logger SDK
|
||||
set(hivecore_logger_cpp_DIR "/opt/hivecore/logger-sdk/1.0.1/lib/cmake/hivecore_logger_cpp")
|
||||
find_package(hivecore_logger_cpp REQUIRED)
|
||||
find_package(rclcpp_components REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
@@ -44,6 +48,7 @@ target_include_directories(brain_node PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
interfaces/include
|
||||
/opt/hivecore/logger-sdk/1.0.1/include
|
||||
)
|
||||
|
||||
target_compile_features(brain_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
@@ -70,6 +75,7 @@ ament_target_dependencies(brain_node
|
||||
target_link_libraries(brain_node
|
||||
Boost::thread
|
||||
yaml-cpp
|
||||
hivecore_logger_cpp::hivecore_logger_cpp
|
||||
)
|
||||
|
||||
install(TARGETS brain_node
|
||||
|
||||
@@ -82,9 +82,9 @@ cerebellum_node:
|
||||
top_cam_left_arm_x_offset: 0.10 #左臂相机视觉识别时的x坐标偏移
|
||||
top_cam_left_arm_y: -0.33 #左臂相机视觉识别时的y坐标
|
||||
top_cam_left_arm_orientation: [0.7071, 0.0, 0.0, 0.7071] #左臂相机视觉识别时的四元数
|
||||
side_cam_right_arm_y_offset: 0.0 # 补偿右臂y轴方向的抓取偏差（前后）
|
||||
side_cam_right_arm_y_offset: 0.02 # 补偿右臂y轴方向的抓取偏差（前后）
|
||||
side_cam_right_arm_z_offset: 0.02 # 补偿右臂z轴方向的抓取偏差（左右）
|
||||
side_cam_left_arm_y_offset: 0.02 # 补偿左臂y轴方向的抓取偏差（前后）
|
||||
side_cam_left_arm_y_offset: -0.02 # 补偿左臂y轴方向的抓取偏差（前后）
|
||||
side_cam_left_arm_z_offset: 0.01 # 补偿左臂z轴方向的抓取偏差（左右）
|
||||
take_object_arm_x: -0.05 # 抓取物体时x坐标
|
||||
left_camera_frame: "LeftLink6" # 左臂相机坐标系
|
||||
@@ -104,6 +104,7 @@ cerebellum_node:
|
||||
arm_cmd_type_take_object: 110 # 拿取动作
|
||||
arm_cmd_type_custom_min: 100 # 自定义动作最小值
|
||||
arm_cmd_type_custom_max: 120 # 自定义动作最大值
|
||||
dual_arm_motion_type: "MOVEJ" # DualArm motion_type: MOVEJ or MOVEL
|
||||
gripper_default_speed: 0 # 夹爪默认速度
|
||||
gripper_default_torque: 0 # 夹爪默认力矩
|
||||
gripper_default_mode: 0 # 夹爪默认模式
|
||||
|
||||
@@ -521,4 +521,8 @@
|
||||
|
||||
data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
|
||||
|
||||
'
|
||||
- name: clear_done_instances_retry
|
||||
params: '{}
|
||||
|
||||
'
|
||||
@@ -1,8 +1,13 @@
|
||||
<!--
|
||||
功能描述: 机器人搬运箱子流程1（单臂+底盘，3次重试）。
|
||||
流程: 清理完成标记 -> 机械臂回零 -> 底盘到抓取点 -> 多组拍照位与抓取位动作 -> 抓取抬起
|
||||
-> 底盘到放置点 -> 腰部下俯与放置并行 -> 松爪 -> 抬臂与腰部上仰并行 -> 回初始位
|
||||
-> 第二次抓取放置循环 -> 回原点。
|
||||
功能描述: 搬运箱子流程1（单臂 + 底盘；无视觉；基于预设位抓取/放置；整体3次重试）。
|
||||
运行概览:
|
||||
1) 每次重试先清理已完成标记（ClearDoneInstances_H），随后机械臂回零/回初始位。
|
||||
2) 第一次搬运循环：底盘前往抓取点 -> 依次执行多组“拍照/定位”预设位（s2/s3/s3_top）->
|
||||
机械臂准备抓取/抓取/抬起 -> 底盘回到中间放置位 -> 腰部下俯与放置动作并行 -> 松爪 ->
|
||||
抬臂与腰部上仰并行 -> 机械臂回预备位/回初始位。
|
||||
3) 第二次搬运循环：重复“拍照位 -> 抓取/抬起 -> 回放置位 -> 放置/松爪 -> 抬臂/上仰”流程，
|
||||
最后底盘回原点、机械臂回初始位。
|
||||
说明: 全流程仅使用 Arm_H 与 MoveWheel_H/MoveWaist_H 的预设动作序列，无相机识别与条件分支。
|
||||
-->
|
||||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="MainTree">
|
||||
|
||||
@@ -537,4 +537,8 @@
|
||||
|
||||
data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
|
||||
|
||||
'
|
||||
- name: clear_done_instances_retry
|
||||
params: '{}
|
||||
|
||||
'
|
||||
@@ -1,8 +1,13 @@
|
||||
<!--
|
||||
功能描述: 机器人搬运箱子流程2（单臂+底盘+腰部俯仰，3次重试，适配高低位箱体）。
|
||||
流程: 回零 -> 底盘到抓取点 -> 预抓取位 -> 腰部下俯拍照 -> 多角度拍照识别 -> 抓取
|
||||
-> 抬起与腰部上仰并行 -> 底盘到放置点 -> 放置/松爪 -> 抬臂 -> 返回拍照位与回零
|
||||
-> 第二次抓取放置循环。
|
||||
功能描述: 搬运箱子流程2（单臂 + 底盘 + 腰部俯仰；无视觉；适配高低位；整体3次重试）。
|
||||
运行概览:
|
||||
1) 每次重试先清理已完成标记（ClearDoneInstances_H），机械臂回零后底盘移动到抓取点。
|
||||
2) 第一次抓取：机械臂到预抓取位与拍照位 -> 腰部下俯 -> 依次执行多组“拍照位”预设动作
|
||||
-> 抓取准备/抓取 -> 抬起与腰部上仰并行 -> 底盘移动到放置位 -> 放置/松爪/抬臂
|
||||
-> 机械臂回拍照位并回初始位。
|
||||
3) 第二次抓取：底盘移动到第二抓取点 -> 重复“预抓取位 -> 下俯 -> 拍照位序列 -> 抓取/抬起”
|
||||
-> 底盘到放置位 -> 放置/松爪/抬臂 -> 回拍照位与初始位 -> 底盘回原点。
|
||||
说明: 全流程动作由 Arm_H、MoveWheel_H、MoveWaist_H 的预设位驱动，无相机识别与条件分支。
|
||||
-->
|
||||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="MainTree">
|
||||
|
||||
@@ -15,157 +15,73 @@
|
||||
- name: s1_left_hand_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s1_right_arm_initial
|
||||
params: &arm_inittial_right 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 80, -5, -112, -12, -62, 175]
|
||||
|
||||
'
|
||||
params: &arm_inittial_right "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_left_arm_initial
|
||||
params: &arm_inittial_left 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [100, 5, 112, 12, 62, 5, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: &arm_inittial_left "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_right_arm_pre_cam_take_photo
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 14.29, -34.07, -122.53, -76.89, 84.40, 112.89]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_left_arm_pre_cam_take_photo
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_right_arm_cam_take_photo
|
||||
params: &right_arm_cam_take_photo 'body_id: 1
|
||||
|
||||
data_type: 100
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &right_arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_left_arm_cam_take_photo
|
||||
params: &left_arm_cam_take_photo 'body_id: 0
|
||||
|
||||
data_type: 100
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &left_arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_top_camera_vision_recg
|
||||
params: 'camera_position: top
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_right_camera_vision_recg
|
||||
params: 'camera_position: right
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_left_camera_vision_recg
|
||||
params: 'camera_position: left
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_right_arm_vision_pre_grasp
|
||||
params: &pre_grasp_right 'body_id: 1
|
||||
|
||||
data_type: 101
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s3_left_arm_vision_pre_grasp
|
||||
params: &pre_grasp_left 'body_id: 0
|
||||
|
||||
data_type: 101
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &pre_grasp_left "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_right_arm_vision_grasp
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 102
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 102\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_left_arm_vision_grasp
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 102
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 102\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_right_hand_gripper_close
|
||||
params: &gripper_close 'loc: 200
|
||||
|
||||
@@ -179,117 +95,45 @@
|
||||
- name: s4_left_hand_gripper_close
|
||||
params: *gripper_close
|
||||
- name: s4_right_arm_take_box
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 110
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 110\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_left_arm_take_box
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 110
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_right_arm_take_box_off
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_left_arm_take_box_off
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_grasp_box
|
||||
params: &grasp_box_s7_s9_right 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
|
||||
|
||||
'
|
||||
params: &grasp_box_s7_s9_right "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_left_arm_grasp_box
|
||||
params: &grasp_box_s7_s9_left 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: &grasp_box_s7_s9_left "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_put_down_box
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 85.28, -103.86, 19.46, 40.34, -7.37, 49.94]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85.28, -103.86, 19.46, 40.34, -7.37, 49.94]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_left_arm_put_down_box
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s8_right_hand_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s8_left_hand_gripper_open
|
||||
|
||||
@@ -1,7 +1,19 @@
|
||||
<!--
|
||||
功能描述: 双臂协同视觉抓取流程（不含底盘，含分阶段恢复与完成标记清理）。
|
||||
流程: 初始化状态变量 -> 头部相机视觉定位（失败进入视觉恢复）-> 双臂同步到手眼拍照位
|
||||
-> 串行左右手眼视觉识别 -> 双臂异步抓取（各自含抓取恢复）-> 双臂放置 -> 双臂回初始位。
|
||||
功能描述: 双臂协同视觉抓取流程（不含底盘；顶视+双手眼；分阶段恢复与完成标记清理）。
|
||||
运行概览:
|
||||
1) 变量初始化：通过 Script 初始化右/左臂拍照状态、视觉成功标记与完成标记。
|
||||
2) 顶视视觉阶段：双爪打开、双臂回初始 -> 顶视相机识别（最多5次）；失败则进入视觉恢复子树。
|
||||
3) 双臂拍照位同步：左右臂并行到手眼拍照位，失败则回初始并允许单臂成功，确保至少一臂可用。
|
||||
4) 串行手眼识别：右臂手眼视觉（最多5次）-> 左臂手眼视觉（最多5次）；
|
||||
仅对尚未完成且拍照成功的手臂执行识别。
|
||||
5) 异步抓取：左右臂并行抓取，各自包含“抓取动作 + 抓取恢复”分支；任一臂成功即可继续。
|
||||
6) 放置阶段：左右臂并行放置，各自独立执行放置子树；任一臂成功即可继续。
|
||||
7) 结束归位：双臂并行回初始位。
|
||||
恢复机制:
|
||||
- 视觉恢复：双臂回初始并清理视觉完成标记。
|
||||
- 抓取恢复：左右臂各自执行“松爪/预抓取/拍照位/回初始”，并清理对应完成标记（match=left/right）。
|
||||
- 手眼视觉恢复：左右臂各自回初始并清理对应视觉标记。
|
||||
- 总体恢复：双爪打开、双臂回初始并清理整体完成标记。
|
||||
-->
|
||||
<root BTCPP_format="4" main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
@@ -33,8 +45,8 @@
|
||||
|
||||
<!-- Phase 5: Dual Arm Hand Vision -->
|
||||
<Parallel name="dual_arm_init_action">
|
||||
<Arm_H name="s11_right_arm_initial" />
|
||||
<Arm_H name="s11_left_arm_initial" />
|
||||
<DualArm_H name="s11_right_arm_initial" />
|
||||
<DualArm_H name="s11_left_arm_initial" />
|
||||
</Parallel>
|
||||
|
||||
</Sequence>
|
||||
@@ -55,14 +67,14 @@
|
||||
<Parallel name="parallel_arm_init_action">
|
||||
<GripperCmd1_H name="s0_right_hand_gripper_open" />
|
||||
<GripperCmd0_H name="s0_left_hand_gripper_open" />
|
||||
<Arm_H name="s1_right_arm_initial" />
|
||||
<Arm_H name="s1_left_arm_initial" />
|
||||
<DualArm_H name="s1_right_arm_initial" />
|
||||
<DualArm_H name="s1_left_arm_initial" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_init_action">
|
||||
<GripperCmd1_H name="s1_right_hand_gripper_open" />
|
||||
<GripperCmd0_H name="s1_left_hand_gripper_open" />
|
||||
<!-- <Arm_H name="s1_right_arm_pre_cam_take_photo" />
|
||||
<Arm_H name="s1_left_arm_pre_cam_take_photo" /> -->
|
||||
<!-- <DualArm_H name="s1_right_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s1_left_arm_pre_cam_take_photo" /> -->
|
||||
</Parallel>
|
||||
<RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
|
||||
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
|
||||
@@ -71,13 +83,13 @@
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Right Arm Specific Subtrees -->
|
||||
<BehaviorTree ID="right_arm_hand_vision_subtree">
|
||||
<BehaviorTree ID="right_DualArm_Hand_vision_subtree">
|
||||
<Fallback name="right_hand_vision_fallback">
|
||||
<RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
|
||||
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
|
||||
</RetryUntilSuccessful>
|
||||
<Sequence>
|
||||
<Arm_H name="s15_right_arm_initial_vision" />
|
||||
<DualArm_H name="s15_right_arm_initial_vision" />
|
||||
<AlwaysFailure />
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
@@ -87,11 +99,11 @@
|
||||
<Sequence name="right_arm_grasp_root">
|
||||
<RetryUntilSuccessful name="retry_right_arm_grasp_action" num_attempts="1">
|
||||
<Sequence>
|
||||
<Arm_H name="s3_right_arm_vision_pre_grasp" />
|
||||
<Arm_H name="s4_right_arm_vision_grasp" />
|
||||
<DualArm_H name="s3_right_arm_vision_pre_grasp" />
|
||||
<DualArm_H name="s4_right_arm_vision_grasp" />
|
||||
<GripperCmd1_H name="s4_right_hand_gripper_close" />
|
||||
<Arm_H name="s4_right_arm_take_box" />
|
||||
<Arm_H name="s5_right_arm_take_box_off" />
|
||||
<DualArm_H name="s4_right_arm_take_box" />
|
||||
<DualArm_H name="s5_right_arm_take_box_off" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
@@ -101,24 +113,24 @@
|
||||
<Sequence name="right_arm_putdown_root">
|
||||
<RetryUntilSuccessful name="retry_right_arm_putdown" num_attempts="1">
|
||||
<Sequence>
|
||||
<!-- <Arm_H name="s7_right_arm_grasp_box" /> -->
|
||||
<Arm_H name="s7_right_arm_put_down_box" />
|
||||
<!-- <DualArm_H name="s7_right_arm_grasp_box" /> -->
|
||||
<DualArm_H name="s7_right_arm_put_down_box" />
|
||||
<GripperCmd1_H name="s8_right_hand_gripper_open" />
|
||||
<Arm_H name="s9_right_arm_grasp_box" />
|
||||
<!-- <Arm_H name="s11_right_arm_initial" /> -->
|
||||
<DualArm_H name="s9_right_arm_grasp_box" />
|
||||
<!-- <DualArm_H name="s11_right_arm_initial" /> -->
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Left Arm Specific Subtrees -->
|
||||
<BehaviorTree ID="left_arm_hand_vision_subtree">
|
||||
<BehaviorTree ID="left_DualArm_Hand_vision_subtree">
|
||||
<Fallback name="left_hand_vision_fallback">
|
||||
<RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
|
||||
<VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
|
||||
</RetryUntilSuccessful>
|
||||
<Sequence>
|
||||
<Arm_H name="s15_left_arm_initial_vision" />
|
||||
<DualArm_H name="s15_left_arm_initial_vision" />
|
||||
<AlwaysFailure />
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
@@ -128,11 +140,11 @@
|
||||
<Sequence name="left_arm_grasp_root">
|
||||
<RetryUntilSuccessful name="retry_left_arm_grasp_action" num_attempts="1">
|
||||
<Sequence>
|
||||
<Arm_H name="s3_left_arm_vision_pre_grasp" />
|
||||
<Arm_H name="s4_left_arm_vision_grasp" />
|
||||
<DualArm_H name="s3_left_arm_vision_pre_grasp" />
|
||||
<DualArm_H name="s4_left_arm_vision_grasp" />
|
||||
<GripperCmd0_H name="s4_left_hand_gripper_close" />
|
||||
<Arm_H name="s4_left_arm_take_box" />
|
||||
<Arm_H name="s5_left_arm_take_box_off" />
|
||||
<DualArm_H name="s4_left_arm_take_box" />
|
||||
<DualArm_H name="s5_left_arm_take_box_off" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
@@ -142,11 +154,11 @@
|
||||
<Sequence name="left_arm_putdown_root">
|
||||
<RetryUntilSuccessful name="retry_left_arm_putdown" num_attempts="1">
|
||||
<Sequence>
|
||||
<!-- <Arm_H name="s7_left_arm_grasp_box" /> -->
|
||||
<Arm_H name="s7_left_arm_put_down_box" />
|
||||
<!-- <DualArm_H name="s7_left_arm_grasp_box" /> -->
|
||||
<DualArm_H name="s7_left_arm_put_down_box" />
|
||||
<GripperCmd0_H name="s8_left_hand_gripper_open" />
|
||||
<Arm_H name="s9_left_arm_grasp_box" />
|
||||
<!-- <Arm_H name="s11_left_arm_initial" /> -->
|
||||
<DualArm_H name="s9_left_arm_grasp_box" />
|
||||
<!-- <DualArm_H name="s11_left_arm_initial" /> -->
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
@@ -158,8 +170,8 @@
|
||||
<Parallel name="overall_recovery_parallel">
|
||||
<GripperCmd1_H name="s12_right_hand_gripper_open" />
|
||||
<GripperCmd0_H name="s12_left_hand_gripper_open" />
|
||||
<Arm_H name="s13_right_arm_initial" />
|
||||
<Arm_H name="s13_left_arm_initial" />
|
||||
<DualArm_H name="s13_right_arm_initial" />
|
||||
<DualArm_H name="s13_left_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_overall" />
|
||||
</Sequence>
|
||||
@@ -168,8 +180,8 @@
|
||||
<BehaviorTree ID="vision_recovery_subtree">
|
||||
<Sequence name="vision_recovery_root">
|
||||
<Parallel name="arm_initial2">
|
||||
<Arm_H name="s14_right_arm_initial" />
|
||||
<Arm_H name="s14_left_arm_initial" />
|
||||
<DualArm_H name="s14_right_arm_initial" />
|
||||
<DualArm_H name="s14_left_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_vision" />
|
||||
</Sequence>
|
||||
@@ -178,9 +190,9 @@
|
||||
<BehaviorTree ID="right_arm_grasp_recovery_subtree">
|
||||
<Sequence name="right_grasp_recovery_root">
|
||||
<GripperCmd1_H name="s15_right_hand_gripper_open" />
|
||||
<Arm_H name="s15_right_arm_pre_grasp" />
|
||||
<Arm_H name="s15_right_arm_cam_take_photo" />
|
||||
<Arm_H name="s15_right_arm_initial_grasp" />
|
||||
<DualArm_H name="s15_right_arm_pre_grasp" />
|
||||
<DualArm_H name="s15_right_arm_cam_take_photo" />
|
||||
<DualArm_H name="s15_right_arm_initial_grasp" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_right_grasp" match="right" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
@@ -188,9 +200,9 @@
|
||||
<BehaviorTree ID="left_arm_grasp_recovery_subtree">
|
||||
<Sequence name="left_grasp_recovery_root">
|
||||
<GripperCmd0_H name="s15_left_hand_gripper_open" />
|
||||
<Arm_H name="s15_left_arm_pre_grasp" />
|
||||
<Arm_H name="s15_left_arm_cam_take_photo" />
|
||||
<Arm_H name="s15_left_arm_initial_grasp" />
|
||||
<DualArm_H name="s15_left_arm_pre_grasp" />
|
||||
<DualArm_H name="s15_left_arm_cam_take_photo" />
|
||||
<DualArm_H name="s15_left_arm_initial_grasp" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_left_grasp" match="left" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
@@ -198,8 +210,8 @@
|
||||
<BehaviorTree ID="arm_vision_recovery_subtree">
|
||||
<Sequence name="arm_vision_recovery_root">
|
||||
<Parallel name="arm_initial3">
|
||||
<Arm_H name="s16_right_arm_initial" />
|
||||
<Arm_H name="s16_left_arm_initial" />
|
||||
<DualArm_H name="s16_right_arm_initial" />
|
||||
<DualArm_H name="s16_left_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_arm_vision" />
|
||||
</Sequence>
|
||||
@@ -208,7 +220,7 @@
|
||||
<BehaviorTree ID="left_arm_vision_recovery_subtree">
|
||||
<Sequence name="left_arm_vision_recovery_root">
|
||||
<Parallel name="left_arm_initial">
|
||||
<Arm_H name="s17_left_arm_initial" />
|
||||
<DualArm_H name="s17_left_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_left_arm_vision" match="left" />
|
||||
</Sequence>
|
||||
@@ -217,7 +229,7 @@
|
||||
<BehaviorTree ID="right_arm_vision_recovery_subtree">
|
||||
<Sequence name="right_arm_vision_recovery_root">
|
||||
<Parallel name="right_arm_initial">
|
||||
<Arm_H name="s17_right_arm_initial" />
|
||||
<DualArm_H name="s17_right_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" match="right" />
|
||||
</Sequence>
|
||||
@@ -232,19 +244,19 @@
|
||||
<!-- 右臂 -->
|
||||
<Fallback>
|
||||
<Sequence>
|
||||
<Arm_H name="s2_right_arm_cam_take_photo" />
|
||||
<DualArm_H name="s2_right_arm_cam_take_photo" />
|
||||
<Script code="right_cam_status:=true" />
|
||||
</Sequence>
|
||||
<Arm_H name="s15_right_arm_initial_cam" />
|
||||
<DualArm_H name="s15_right_arm_initial_cam" />
|
||||
<AlwaysSuccess/>
|
||||
</Fallback>
|
||||
<!-- 左臂 -->
|
||||
<Fallback>
|
||||
<Sequence>
|
||||
<Arm_H name="s2_left_arm_cam_take_photo" />
|
||||
<DualArm_H name="s2_left_arm_cam_take_photo" />
|
||||
<Script code="left_cam_status:=true" />
|
||||
</Sequence>
|
||||
<Arm_H name="s15_left_arm_initial_cam" />
|
||||
<DualArm_H name="s15_left_arm_initial_cam" />
|
||||
<AlwaysSuccess/>
|
||||
</Fallback>
|
||||
</Parallel>
|
||||
@@ -262,7 +274,7 @@
|
||||
<ScriptCondition code="right_done==false && right_cam_ok==true" />
|
||||
<Fallback>
|
||||
<Sequence>
|
||||
<SubTree ID="right_arm_hand_vision_subtree"/>
|
||||
<SubTree ID="right_DualArm_Hand_vision_subtree"/>
|
||||
<Script code="right_v_ok:=true" />
|
||||
</Sequence>
|
||||
<AlwaysFailure/>
|
||||
@@ -276,7 +288,7 @@
|
||||
<ScriptCondition code="left_done==false && left_cam_ok==true" />
|
||||
<Fallback>
|
||||
<Sequence>
|
||||
<SubTree ID="left_arm_hand_vision_subtree"/>
|
||||
<SubTree ID="left_DualArm_Hand_vision_subtree"/>
|
||||
<Script code="left_v_ok:=true" />
|
||||
</Sequence>
|
||||
<AlwaysFailure/>
|
||||
|
||||
@@ -15,157 +15,73 @@
|
||||
- name: s1_left_hand_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s1_right_arm_initial
|
||||
params: &arm_inittial_right 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 80, -5, -112, -12, -62, 175]
|
||||
|
||||
'
|
||||
params: &arm_inittial_right "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_left_arm_initial
|
||||
params: &arm_inittial_left 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [100, 5, 112, 12, 62, 5, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: &arm_inittial_left "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_right_arm_pre_cam_take_photo
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 14.29, -34.07, -122.53, -76.89, 84.40, 112.89]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_left_arm_pre_cam_take_photo
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_right_arm_cam_take_photo
|
||||
params: &right_arm_cam_take_photo 'body_id: 1
|
||||
|
||||
data_type: 100
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &right_arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_left_arm_cam_take_photo
|
||||
params: &left_arm_cam_take_photo 'body_id: 0
|
||||
|
||||
data_type: 100
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &left_arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_top_camera_vision_recg
|
||||
params: 'camera_position: top
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_right_camera_vision_recg
|
||||
params: 'camera_position: right
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_left_camera_vision_recg
|
||||
params: 'camera_position: left
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_right_arm_vision_pre_grasp
|
||||
params: &pre_grasp_right 'body_id: 1
|
||||
|
||||
data_type: 101
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 50\nvelocity_scaling: 40\n"
|
||||
- name: s3_left_arm_vision_pre_grasp
|
||||
params: &pre_grasp_left 'body_id: 0
|
||||
|
||||
data_type: 101
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &pre_grasp_left "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 50\nvelocity_scaling: 40\n"
|
||||
- name: s4_right_arm_vision_grasp
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 102
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 102\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_left_arm_vision_grasp
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 102
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 102\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_right_hand_gripper_close
|
||||
params: &gripper_close 'loc: 200
|
||||
|
||||
@@ -179,117 +95,45 @@
|
||||
- name: s4_left_hand_gripper_close
|
||||
params: *gripper_close
|
||||
- name: s4_right_arm_take_box
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 110
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 110\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_left_arm_take_box
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 110
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_right_arm_take_box_off
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_left_arm_take_box_off
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_grasp_box
|
||||
params: &grasp_box_s7_s9_right 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
|
||||
|
||||
'
|
||||
params: &grasp_box_s7_s9_right "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_left_arm_grasp_box
|
||||
params: &grasp_box_s7_s9_left 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: &grasp_box_s7_s9_left "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_put_down_box
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [0, 0, 0, 0, 0, 0, 85.28, -103.86, 19.46, 40.34, -7.37, 49.94]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85.28, -103.86, 19.46, 40.34, -7.37, 49.94]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_left_arm_put_down_box
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15, 0, 0, 0, 0, 0, 0]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s8_right_hand_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s8_left_hand_gripper_open
|
||||
@@ -338,10 +182,6 @@
|
||||
params: *pre_grasp_right
|
||||
- name: s15_left_arm_pre_grasp
|
||||
params: *pre_grasp_left
|
||||
- name: s15_right_arm_initial
|
||||
params: *arm_inittial_right
|
||||
- name: s15_left_arm_initial
|
||||
params: *arm_inittial_left
|
||||
- name: s16_right_arm_initial
|
||||
params: *arm_inittial_right
|
||||
- name: s16_left_arm_initial
|
||||
@@ -367,19 +207,19 @@
|
||||
- name: s0_left_hand_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s1_wheel_move_to_origin_pickup_position
|
||||
params: &origin_pickup_position 'move_distance: 0.0
|
||||
params: &origin_pickup_position 'move_distance: 0.5
|
||||
|
||||
move_angle: 0.0
|
||||
|
||||
'
|
||||
- name: s6_wheel_move_to_putdown_position
|
||||
params: 'move_distance: -0.5
|
||||
params: 'move_distance: 0.0
|
||||
|
||||
move_angle: 0.0
|
||||
|
||||
'
|
||||
- name: s6_move_waist_turn_left_90
|
||||
params: 'move_pitch_degree: 0.0
|
||||
params: &waist_turn_left_90 'move_pitch_degree: 0.0
|
||||
|
||||
move_yaw_degree: 90.0
|
||||
|
||||
@@ -395,4 +235,20 @@
|
||||
- name: s15_left_arm_cam_take_photo
|
||||
params: *left_arm_cam_take_photo
|
||||
- name: s15_right_arm_cam_take_photo
|
||||
params: *right_arm_cam_take_photo
|
||||
params: *right_arm_cam_take_photo
|
||||
- name: s15_right_arm_initial_vision
|
||||
params: *arm_inittial_right
|
||||
- name: s15_left_arm_initial_vision
|
||||
params: *arm_inittial_left
|
||||
- name: s15_right_arm_initial_grasp
|
||||
params: *arm_inittial_right
|
||||
- name: s15_left_arm_initial_grasp
|
||||
params: *arm_inittial_left
|
||||
- name: s15_right_arm_initial_cam
|
||||
params: *arm_inittial_right
|
||||
- name: s15_left_arm_initial_cam
|
||||
params: *arm_inittial_left
|
||||
- name: s6_move_waist_turn_right_90
|
||||
params: *waist_turn_right_90
|
||||
- name: s10_move_waist_turn_left_90
|
||||
params: *waist_turn_left_90
|
||||
@@ -1,7 +1,15 @@
|
||||
<!--
|
||||
功能描述: 双臂协同视觉抓取演示流程1（全机身协作，含失败恢复与重试）。
|
||||
流程: 底盘移动到抓取工位 -> 头部相机定位 + 双臂手眼识别 -> 双臂异步抓取
|
||||
-> 底盘移动到放置工位并腰部旋转 -> 双臂放置 -> 腰部复位 -> 底盘回抓取位 -> 双臂回初始位。
|
||||
功能描述: 双臂协同视觉抓取演示流程1（全机身协作；顶视+双手眼；含恢复与重试）。
|
||||
运行概览:
|
||||
1) 起始移动：底盘移动到抓取工位。
|
||||
2) 顶视视觉阶段：双爪打开、双臂回初始 -> 顶视相机识别（最多5次）；失败则进入视觉恢复。
|
||||
3) 双臂拍照位同步：左右臂并行到手眼拍照位，允许单臂成功。
|
||||
4) 串行手眼识别：右臂手眼 -> 左臂手眼（各最多5次），仅对可用且未完成的手臂执行。
|
||||
5) 异步抓取：左右臂并行抓取，各自含抓取恢复；任一臂成功即可继续。
|
||||
6) 放置移动：底盘到放置工位 + 腰部右转并行。
|
||||
7) 放置阶段：左右臂并行放置，各自独立完成。
|
||||
8) 复位：腰部左转复位；底盘回抓取位；双臂回初始位。
|
||||
恢复机制: 视觉/抓取/手眼各自有恢复子树，失败时触发重试；外层总流程最多重试3次。
|
||||
-->
|
||||
<root BTCPP_format="4" main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
@@ -33,19 +41,19 @@
|
||||
|
||||
<Parallel name="parallel_action_2">
|
||||
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
|
||||
<MoveWaist_H name="s6_move_waist_turn_left_90" />
|
||||
<MoveWaist_H name="s6_move_waist_turn_right_90" />
|
||||
</Parallel>
|
||||
|
||||
<!-- Phase 4: Dual Arm Putdown -->
|
||||
<SubTree ID="dual_arm_putdown_subtree_wrapper" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
|
||||
|
||||
<MoveWaist_H name="s10_move_waist_turn_right_90" />
|
||||
<MoveWaist_H name="s10_move_waist_turn_left_90" />
|
||||
|
||||
<!-- Phase 5: Dual Arm Initialization -->
|
||||
<Parallel name="dual_arm_init_action">
|
||||
<MoveWheel_H name="s10_wheel_move_to_origin_pickup_position" />
|
||||
<Arm_H name="s11_right_arm_initial" />
|
||||
<Arm_H name="s11_left_arm_initial" />
|
||||
<!-- <MoveWheel_H name="s10_wheel_move_to_origin_pickup_position" /> -->
|
||||
<DualArm_H name="s11_right_arm_initial" />
|
||||
<DualArm_H name="s11_left_arm_initial" />
|
||||
</Parallel>
|
||||
|
||||
</Sequence>
|
||||
@@ -66,14 +74,14 @@
|
||||
<Parallel name="parallel_arm_init_action">
|
||||
<GripperCmd1_H name="s0_right_hand_gripper_open" />
|
||||
<GripperCmd0_H name="s0_left_hand_gripper_open" />
|
||||
<Arm_H name="s1_right_arm_initial" />
|
||||
<Arm_H name="s1_left_arm_initial" />
|
||||
<DualArm_H name="s1_right_arm_initial" />
|
||||
<DualArm_H name="s1_left_arm_initial" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_init_action">
|
||||
<GripperCmd1_H name="s1_right_hand_gripper_open" />
|
||||
<GripperCmd0_H name="s1_left_hand_gripper_open" />
|
||||
<!-- <Arm_H name="s1_right_arm_pre_cam_take_photo" />
|
||||
<Arm_H name="s1_left_arm_pre_cam_take_photo" /> -->
|
||||
<!-- <DualArm_H name="s1_right_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s1_left_arm_pre_cam_take_photo" /> -->
|
||||
</Parallel>
|
||||
<RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
|
||||
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
|
||||
@@ -82,23 +90,27 @@
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Right Arm Specific Subtrees -->
|
||||
<BehaviorTree ID="right_arm_hand_vision_subtree">
|
||||
<Sequence name="right_hand_vision_root">
|
||||
<BehaviorTree ID="right_DualArm_Hand_vision_subtree">
|
||||
<Fallback name="right_hand_vision_fallback">
|
||||
<RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
|
||||
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
<Sequence>
|
||||
<DualArm_H name="s15_right_arm_initial_vision" />
|
||||
<AlwaysFailure />
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="right_arm_grasp_subtree">
|
||||
<Sequence name="right_arm_grasp_root">
|
||||
<RetryUntilSuccessful name="retry_right_arm_grasp_action" num_attempts="1">
|
||||
<Sequence>
|
||||
<Arm_H name="s3_right_arm_vision_pre_grasp" />
|
||||
<Arm_H name="s4_right_arm_vision_grasp" />
|
||||
<DualArm_H name="s3_right_arm_vision_pre_grasp" />
|
||||
<DualArm_H name="s4_right_arm_vision_grasp" />
|
||||
<GripperCmd1_H name="s4_right_hand_gripper_close" />
|
||||
<Arm_H name="s4_right_arm_take_box" />
|
||||
<Arm_H name="s5_right_arm_take_box_off" />
|
||||
<DualArm_H name="s4_right_arm_take_box" />
|
||||
<DualArm_H name="s5_right_arm_take_box_off" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
@@ -108,34 +120,38 @@
|
||||
<Sequence name="right_arm_putdown_root">
|
||||
<RetryUntilSuccessful name="retry_right_arm_putdown" num_attempts="1">
|
||||
<Sequence>
|
||||
<!-- <Arm_H name="s7_right_arm_grasp_box" /> -->
|
||||
<Arm_H name="s7_right_arm_put_down_box" />
|
||||
<!-- <DualArm_H name="s7_right_arm_grasp_box" /> -->
|
||||
<DualArm_H name="s7_right_arm_put_down_box" />
|
||||
<GripperCmd1_H name="s8_right_hand_gripper_open" />
|
||||
<Arm_H name="s9_right_arm_grasp_box" />
|
||||
<!-- <Arm_H name="s11_right_arm_initial" /> -->
|
||||
<DualArm_H name="s9_right_arm_grasp_box" />
|
||||
<!-- <DualArm_H name="s11_right_arm_initial" /> -->
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<!-- Left Arm Specific Subtrees -->
|
||||
<BehaviorTree ID="left_arm_hand_vision_subtree">
|
||||
<Sequence name="left_hand_vision_root">
|
||||
<BehaviorTree ID="left_DualArm_Hand_vision_subtree">
|
||||
<Fallback name="left_hand_vision_fallback">
|
||||
<RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
|
||||
<VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
<Sequence>
|
||||
<DualArm_H name="s15_left_arm_initial_vision" />
|
||||
<AlwaysFailure />
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="left_arm_grasp_subtree">
|
||||
<Sequence name="left_arm_grasp_root">
|
||||
<RetryUntilSuccessful name="retry_left_arm_grasp_action" num_attempts="1">
|
||||
<Sequence>
|
||||
<Arm_H name="s3_left_arm_vision_pre_grasp" />
|
||||
<Arm_H name="s4_left_arm_vision_grasp" />
|
||||
<DualArm_H name="s3_left_arm_vision_pre_grasp" />
|
||||
<DualArm_H name="s4_left_arm_vision_grasp" />
|
||||
<GripperCmd0_H name="s4_left_hand_gripper_close" />
|
||||
<Arm_H name="s4_left_arm_take_box" />
|
||||
<Arm_H name="s5_left_arm_take_box_off" />
|
||||
<DualArm_H name="s4_left_arm_take_box" />
|
||||
<DualArm_H name="s5_left_arm_take_box_off" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
@@ -145,11 +161,11 @@
|
||||
<Sequence name="left_arm_putdown_root">
|
||||
<RetryUntilSuccessful name="retry_left_arm_putdown" num_attempts="1">
|
||||
<Sequence>
|
||||
<!-- <Arm_H name="s7_left_arm_grasp_box" /> -->
|
||||
<Arm_H name="s7_left_arm_put_down_box" />
|
||||
<!-- <DualArm_H name="s7_left_arm_grasp_box" /> -->
|
||||
<DualArm_H name="s7_left_arm_put_down_box" />
|
||||
<GripperCmd0_H name="s8_left_hand_gripper_open" />
|
||||
<Arm_H name="s9_left_arm_grasp_box" />
|
||||
<!-- <Arm_H name="s11_left_arm_initial" /> -->
|
||||
<DualArm_H name="s9_left_arm_grasp_box" />
|
||||
<!-- <DualArm_H name="s11_left_arm_initial" /> -->
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
@@ -161,8 +177,8 @@
|
||||
<Parallel name="overall_recovery_parallel">
|
||||
<GripperCmd1_H name="s12_right_hand_gripper_open" />
|
||||
<GripperCmd0_H name="s12_left_hand_gripper_open" />
|
||||
<Arm_H name="s13_right_arm_initial" />
|
||||
<Arm_H name="s13_left_arm_initial" />
|
||||
<DualArm_H name="s13_right_arm_initial" />
|
||||
<DualArm_H name="s13_left_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_overall" />
|
||||
</Sequence>
|
||||
@@ -171,8 +187,8 @@
|
||||
<BehaviorTree ID="vision_recovery_subtree">
|
||||
<Sequence name="vision_recovery_root">
|
||||
<Parallel name="arm_initial2">
|
||||
<Arm_H name="s14_right_arm_initial" />
|
||||
<Arm_H name="s14_left_arm_initial" />
|
||||
<DualArm_H name="s14_right_arm_initial" />
|
||||
<DualArm_H name="s14_left_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_vision" />
|
||||
</Sequence>
|
||||
@@ -181,28 +197,28 @@
|
||||
<BehaviorTree ID="right_arm_grasp_recovery_subtree">
|
||||
<Sequence name="right_grasp_recovery_root">
|
||||
<GripperCmd1_H name="s15_right_hand_gripper_open" />
|
||||
<Arm_H name="s15_right_arm_pre_grasp" />
|
||||
<Arm_H name="s15_right_arm_cam_take_photo" />
|
||||
<Arm_H name="s15_right_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_right_grasp" />
|
||||
<DualArm_H name="s15_right_arm_pre_grasp" />
|
||||
<DualArm_H name="s15_right_arm_cam_take_photo" />
|
||||
<DualArm_H name="s15_right_arm_initial_grasp" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_right_grasp" match="right" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="left_arm_grasp_recovery_subtree">
|
||||
<Sequence name="left_grasp_recovery_root">
|
||||
<GripperCmd0_H name="s15_left_hand_gripper_open" />
|
||||
<Arm_H name="s15_left_arm_pre_grasp" />
|
||||
<Arm_H name="s15_left_arm_cam_take_photo" />
|
||||
<Arm_H name="s15_left_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_left_grasp" />
|
||||
<DualArm_H name="s15_left_arm_pre_grasp" />
|
||||
<DualArm_H name="s15_left_arm_cam_take_photo" />
|
||||
<DualArm_H name="s15_left_arm_initial_grasp" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_left_grasp" match="left" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="arm_vision_recovery_subtree">
|
||||
<Sequence name="arm_vision_recovery_root">
|
||||
<Parallel name="arm_initial3">
|
||||
<Arm_H name="s16_right_arm_initial" />
|
||||
<Arm_H name="s16_left_arm_initial" />
|
||||
<DualArm_H name="s16_right_arm_initial" />
|
||||
<DualArm_H name="s16_left_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_arm_vision" />
|
||||
</Sequence>
|
||||
@@ -211,18 +227,18 @@
|
||||
<BehaviorTree ID="left_arm_vision_recovery_subtree">
|
||||
<Sequence name="left_arm_vision_recovery_root">
|
||||
<Parallel name="left_arm_initial">
|
||||
<Arm_H name="s17_left_arm_initial" />
|
||||
<DualArm_H name="s17_left_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_left_arm_vision" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_left_arm_vision" match="left" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="right_arm_vision_recovery_subtree">
|
||||
<Sequence name="right_arm_vision_recovery_root">
|
||||
<Parallel name="right_arm_initial">
|
||||
<Arm_H name="s17_right_arm_initial" />
|
||||
<DualArm_H name="s17_right_arm_initial" />
|
||||
</Parallel>
|
||||
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" match="right" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
@@ -235,19 +251,19 @@
|
||||
<!-- 右臂 -->
|
||||
<Fallback>
|
||||
<Sequence>
|
||||
<Arm_H name="s2_right_arm_cam_take_photo" />
|
||||
<DualArm_H name="s2_right_arm_cam_take_photo" />
|
||||
<Script code="right_cam_status:=true" />
|
||||
</Sequence>
|
||||
<Arm_H name="s15_right_arm_initial" />
|
||||
<DualArm_H name="s15_right_arm_initial_cam" />
|
||||
<AlwaysSuccess/>
|
||||
</Fallback>
|
||||
<!-- 左臂 -->
|
||||
<Fallback>
|
||||
<Sequence>
|
||||
<Arm_H name="s2_left_arm_cam_take_photo" />
|
||||
<DualArm_H name="s2_left_arm_cam_take_photo" />
|
||||
<Script code="left_cam_status:=true" />
|
||||
</Sequence>
|
||||
<Arm_H name="s15_left_arm_initial" />
|
||||
<DualArm_H name="s15_left_arm_initial_cam" />
|
||||
<AlwaysSuccess/>
|
||||
</Fallback>
|
||||
</Parallel>
|
||||
@@ -265,7 +281,7 @@
|
||||
<ScriptCondition code="right_done==false && right_cam_ok==true" />
|
||||
<Fallback>
|
||||
<Sequence>
|
||||
<SubTree ID="right_arm_hand_vision_subtree"/>
|
||||
<SubTree ID="right_DualArm_Hand_vision_subtree"/>
|
||||
<Script code="right_v_ok:=true" />
|
||||
</Sequence>
|
||||
<AlwaysFailure/>
|
||||
@@ -279,7 +295,7 @@
|
||||
<ScriptCondition code="left_done==false && left_cam_ok==true" />
|
||||
<Fallback>
|
||||
<Sequence>
|
||||
<SubTree ID="left_arm_hand_vision_subtree"/>
|
||||
<SubTree ID="left_DualArm_Hand_vision_subtree"/>
|
||||
<Script code="left_v_ok:=true" />
|
||||
</Sequence>
|
||||
<AlwaysFailure/>
|
||||
|
||||
@@ -22,20 +22,16 @@
|
||||
mode: 2
|
||||
|
||||
'
|
||||
- name: s1_left_arm_pre_cam_take_photo
|
||||
params: &pre_cam_take_photo 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [85, 25, 90, 0, 68, 90, 95, -25, -90, 0, -68, 90]
|
||||
|
||||
'
|
||||
- name: s1_left_arm_initial
|
||||
params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_right_arm_initial
|
||||
params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_wheel_move_to_origin_pickup_position
|
||||
params: 'move_distance: 0.5
|
||||
|
||||
@@ -43,69 +39,42 @@
|
||||
|
||||
'
|
||||
- name: s2_left_arm_pre_cam_take_photo
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [120, 20, 80, 0, 80, 120, 60, -20, -80, 0, -80, 60]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_right_arm_pre_cam_take_photo
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_top_camera_vision_recg
|
||||
params: 'camera_position: top
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s2_left_arm_cam_take_photo
|
||||
params: &arm_cam_take_photo 'body_id: 0
|
||||
|
||||
data_type: 100
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s3_left_camera_vision_recg
|
||||
params: 'camera_position: left
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_left_arm_vision_pre_grasp
|
||||
params: &pre_grasp 'body_id: 0
|
||||
|
||||
data_type: 101
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &pre_grasp "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_left_arm_vision_grasp
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 102
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 102\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_gripper_close
|
||||
params: 'loc: 200
|
||||
|
||||
@@ -117,33 +86,20 @@
|
||||
|
||||
'
|
||||
- name: s4_left_arm_take_box
|
||||
params: 'body_id: 0
|
||||
|
||||
data_type: 110
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_left_arm_grasp_box
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [84, 88, 45, 22, 16, 68, 95, -25, -90, 0, -68, 90]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [84, 88, 45, 22, 16, 68]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_right_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s6_move_waist_turn_left_90
|
||||
params: 'move_pitch_degree: 0.0
|
||||
|
||||
@@ -157,51 +113,53 @@
|
||||
|
||||
'
|
||||
- name: s7_left_arm_grasp_box
|
||||
params: &grasp_box_s7_s9 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [85, 88, -14, 22, 16, 68, 95, -25, -90, 0, -68, 90]
|
||||
|
||||
'
|
||||
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 88, -14, 22, 16, 68]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_left_arm_put_down_box
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [95, 88, -14, -22, 16, 112, 95, -25, -90, 0, -68, 90]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, 88, -14, -22, 16, 112]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_put_down_box
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s8_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s9_left_arm_grasp_box
|
||||
params: *grasp_box_s7_s9
|
||||
- name: s10_move_waist_turn_right_90
|
||||
params: *waist_turn_right_90
|
||||
- name: s11_left_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s11_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s11_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s12_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s13_left_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s14_left_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s15_left_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s16_left_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s13_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s13_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s14_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s14_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s15_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s15_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s16_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s16_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s14_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s15_gripper_open
|
||||
@@ -215,4 +173,20 @@
|
||||
- name: s15_left_arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
- name: s16_left_arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
- name: clear_done_instances_overall
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_vision
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_grasp
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_putdown
|
||||
params: '{}
|
||||
|
||||
'
|
||||
@@ -1,6 +1,15 @@
|
||||
<!--
|
||||
功能描述: 左臂独立视觉抓取流程（视觉/抓取/放置均带恢复与重试）。
|
||||
流程: 左臂到手眼拍照位 -> 相机识别目标 -> 预抓取/抓取/抬起 -> 送到放置位并松爪 -> 回到初始位。
|
||||
功能描述: 左臂视觉抓取流程（顶视+手眼识别；抓取/放置含恢复；外层3次重试）。
|
||||
运行概览:
|
||||
1) 视觉阶段（vision_subtree）：双臂回初始并打开左爪；进行顶视相机识别（最多10次）；
|
||||
左臂到手眼拍照位后进行左手眼识别（最多10次）。
|
||||
2) 抓取阶段（grasp_subtree）：左臂预抓取 -> 左臂抓取 -> 关闭左爪 -> 左臂抬起；
|
||||
随后左右臂并行保持抓箱位（parallel_action_grasp_box）。
|
||||
3) 放置阶段（putdown_subtree）：左右臂并行抓箱位/放箱位 -> 左爪松开 -> 左臂回抓箱位；
|
||||
双臂并行回初始位。
|
||||
4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
|
||||
并通过 AlwaysFailure 触发阶段重试；外层总流程最多重试3次。
|
||||
说明: 底盘与腰部动作在此版本中被注释，流程主要为视觉+双臂姿态与左爪控制。
|
||||
-->
|
||||
<root BTCPP_format="4" main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
@@ -50,15 +59,20 @@
|
||||
<Sequence name="vision_root">
|
||||
<RetryUntilSuccessful name="retry_vision" num_attempts="1">
|
||||
<Sequence>
|
||||
<Arm_H name="s1_left_arm_pre_cam_take_photo" />
|
||||
<Parallel name="parallel_action_1">
|
||||
<!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
|
||||
<DualArm_H name="s1_left_arm_initial" />
|
||||
<DualArm_H name="s1_right_arm_initial" />
|
||||
<GripperCmd0_H name="s1_gripper_open" />
|
||||
<Arm_H name="s2_left_arm_pre_cam_take_photo" />
|
||||
</Parallel>
|
||||
<!-- <Parallel name="parallel_action_2"> -->
|
||||
<!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
|
||||
<!-- <GripperCmd0_H name="s1_gripper_open" /> -->
|
||||
<!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
|
||||
<!-- </Parallel> -->
|
||||
<SubTree ID="vision_top_cam_subtree"/>
|
||||
|
||||
<Arm_H name="s2_left_arm_cam_take_photo" />
|
||||
<DualArm_H name="s2_left_arm_cam_take_photo" />
|
||||
<SubTree ID="vision_hand_cam_subtree"/>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -85,11 +99,14 @@
|
||||
<Sequence name="grasp_root">
|
||||
<RetryUntilSuccessful name="retry_grasp" num_attempts="1">
|
||||
<Sequence>
|
||||
<Arm_H name="s3_left_arm_vision_pre_grasp" />
|
||||
<Arm_H name="s4_left_arm_vision_grasp" />
|
||||
<DualArm_H name="s3_left_arm_vision_pre_grasp" />
|
||||
<DualArm_H name="s4_left_arm_vision_grasp" />
|
||||
<GripperCmd0_H name="s4_gripper_close" />
|
||||
<Arm_H name="s4_left_arm_take_box" />
|
||||
<Arm_H name="s5_left_arm_grasp_box" />
|
||||
<DualArm_H name="s4_left_arm_take_box" />
|
||||
<Parallel name="parallel_action_grasp_box">
|
||||
<DualArm_H name="s5_left_arm_grasp_box" />
|
||||
<DualArm_H name="s5_right_arm_grasp_box" />
|
||||
</Parallel>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
@@ -103,14 +120,21 @@
|
||||
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
|
||||
<MoveWaist_H name="s6_move_waist_turn_left_90" />
|
||||
</Parallel> -->
|
||||
<Arm_H name="s7_left_arm_grasp_box" />
|
||||
<Arm_H name="s7_left_arm_put_down_box" />
|
||||
<Parallel name="parallel_action_s7_grasp_box">
|
||||
<DualArm_H name="s7_left_arm_grasp_box" />
|
||||
<DualArm_H name="s7_right_arm_grasp_box" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_action_s7_put_down_box">
|
||||
<DualArm_H name="s7_left_arm_put_down_box" />
|
||||
<DualArm_H name="s7_right_arm_put_down_box" />
|
||||
</Parallel>
|
||||
<GripperCmd0_H name="s8_gripper_open" />
|
||||
<Arm_H name="s9_left_arm_grasp_box" />
|
||||
<DualArm_H name="s9_left_arm_grasp_box" />
|
||||
|
||||
<Parallel name="parallel_action_3">
|
||||
<!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
|
||||
<Arm_H name="s11_left_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s11_left_arm_initial" />
|
||||
<DualArm_H name="s11_right_arm_initial" />
|
||||
</Parallel>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -122,7 +146,8 @@
|
||||
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd0_H name="s12_gripper_open" />
|
||||
<Arm_H name="s13_left_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s13_left_arm_initial" />
|
||||
<DualArm_H name="s13_right_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_overall" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -134,7 +159,8 @@
|
||||
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<!-- <GripperCmd0_H name="s14_gripper_open" /> -->
|
||||
<Arm_H name="s14_left_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s14_left_arm_initial" />
|
||||
<DualArm_H name="s14_right_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_vision" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -146,9 +172,10 @@
|
||||
<RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd0_H name="s15_gripper_open" />
|
||||
<Arm_H name="s15_arm_pre_grasp" />
|
||||
<Arm_H name="s15_left_arm_cam_take_photo" />
|
||||
<Arm_H name="s15_left_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s15_arm_pre_grasp" />
|
||||
<DualArm_H name="s15_left_arm_cam_take_photo" />
|
||||
<DualArm_H name="s15_left_arm_initial" />
|
||||
<DualArm_H name="s15_right_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_grasp" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -160,9 +187,10 @@
|
||||
<RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd0_H name="s16_gripper_open" />
|
||||
<Arm_H name="s16_arm_pre_grasp" />
|
||||
<Arm_H name="s16_left_arm_cam_take_photo" />
|
||||
<Arm_H name="s16_left_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s16_arm_pre_grasp" />
|
||||
<DualArm_H name="s16_left_arm_cam_take_photo" />
|
||||
<DualArm_H name="s16_left_arm_initial" />
|
||||
<DualArm_H name="s16_right_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_putdown" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
|
||||
196
src/brain/config/bt_vision_grasp_left_arm_demo1.params.yaml
Normal file
196
src/brain/config/bt_vision_grasp_left_arm_demo1.params.yaml
Normal file
@@ -0,0 +1,196 @@
|
||||
- name: root
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: retry_vision_hand
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: s1_move_waist_turn_right_90
|
||||
params: &waist_turn_right_90 'move_pitch_degree: 0.0
|
||||
|
||||
move_yaw_degree: -90.0
|
||||
|
||||
'
|
||||
- name: s1_gripper_open
|
||||
params: &gripper_open 'loc: 0
|
||||
|
||||
speed: 150
|
||||
|
||||
torque: 20
|
||||
|
||||
mode: 2
|
||||
|
||||
'
|
||||
- name: s1_left_arm_initial
|
||||
params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_right_arm_initial
|
||||
params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_wheel_move_to_origin_pickup_position
|
||||
params: 'move_distance: 0.5
|
||||
|
||||
move_angle: 0.0
|
||||
|
||||
'
|
||||
- name: s2_left_arm_pre_cam_take_photo
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_right_arm_pre_cam_take_photo
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_top_camera_vision_recg
|
||||
params: 'camera_position: top
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s2_left_arm_cam_take_photo
|
||||
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s3_left_camera_vision_recg
|
||||
params: 'camera_position: left
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_left_arm_vision_pre_grasp
|
||||
params: &pre_grasp "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_left_arm_vision_grasp
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 102\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_gripper_close
|
||||
params: 'loc: 200
|
||||
|
||||
speed: 150
|
||||
|
||||
torque: 100
|
||||
|
||||
mode: 2
|
||||
|
||||
'
|
||||
- name: s4_left_arm_take_box
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_left_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [84, 88, 45, 22, 16, 68]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_right_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s6_move_waist_turn_left_90
|
||||
params: &waist_turn_left_90 'move_pitch_degree: 0.0
|
||||
|
||||
move_yaw_degree: 90.0
|
||||
|
||||
'
|
||||
- name: s6_wheel_move_to_putdown_position
|
||||
params: 'move_distance: 0.0
|
||||
|
||||
move_angle: 0.0
|
||||
|
||||
'
|
||||
- name: s7_left_arm_grasp_box
|
||||
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 88, -14, 22, 16, 68]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_left_arm_put_down_box
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, 88, -14, -22, 16, 112]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_put_down_box
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s8_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s9_left_arm_grasp_box
|
||||
params: *grasp_box_s7_s9
|
||||
- name: s10_move_waist_turn_right_90
|
||||
params: *waist_turn_right_90
|
||||
- name: s11_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s11_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s12_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s13_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s13_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s14_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s14_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s15_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s15_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s16_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s16_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s14_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s15_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s16_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s15_arm_pre_grasp
|
||||
params: *pre_grasp
|
||||
- name: s16_arm_pre_grasp
|
||||
params: *pre_grasp
|
||||
- name: s15_left_arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
- name: s16_left_arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
- name: s6_move_waist_turn_right_90
|
||||
params: *waist_turn_right_90
|
||||
- name: s10_move_waist_turn_left_90
|
||||
params: *waist_turn_left_90
|
||||
- name: clear_done_instances_overall
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_vision
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_grasp
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_putdown
|
||||
params: '{}
|
||||
|
||||
'
|
||||
196
src/brain/config/bt_vision_grasp_left_arm_demo1.xml
Normal file
196
src/brain/config/bt_vision_grasp_left_arm_demo1.xml
Normal file
@@ -0,0 +1,196 @@
|
||||
<!--
|
||||
功能描述: 左臂视觉抓取演示流程1（顶视 + 左手眼识别；含底盘与腰部转向；外层3次重试）。
|
||||
运行概览:
|
||||
1) 视觉阶段：双臂回初始并打开左爪（GripperCmd0）；底盘移动到取物位；顶视相机识别（最多10次）；
|
||||
左臂到手眼拍照位后进行左手眼识别（最多10次）。
|
||||
2) 抓取阶段：左臂预抓取 -> 左臂抓取 -> 关闭左爪 -> 左臂抬起 -> 左右臂并行保持抓箱位。
|
||||
3) 放置阶段：底盘移动到放置位并腰部右转；左右臂并行抓箱位/放箱位 -> 左爪松开 -> 左臂回抓箱位；
|
||||
腰部左转复位，同时双臂回初始位。
|
||||
4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
|
||||
并通过 AlwaysFailure 触发阶段重试。
|
||||
-->
|
||||
<root BTCPP_format="4" main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Sequence name="root">
|
||||
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
|
||||
<Fallback name="fallback_main_action">
|
||||
<Sequence name="complete_pick_and_place">
|
||||
<!-- Vision with recovery -->
|
||||
<Fallback name="vision_with_recovery">
|
||||
<SubTree ID="vision_subtree"/>
|
||||
<Sequence>
|
||||
<SubTree ID="vision_recovery_subtree"/>
|
||||
<AlwaysFailure/>
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
|
||||
<!-- Grasp with recovery -->
|
||||
<Fallback name="grasp_with_recovery">
|
||||
<SubTree ID="grasp_subtree"/>
|
||||
<Sequence>
|
||||
<SubTree ID="grasp_recovery_subtree"/>
|
||||
<AlwaysFailure/>
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
|
||||
<!-- Putdown with recovery -->
|
||||
<Fallback name="putdown_with_recovery">
|
||||
<SubTree ID="putdown_subtree"/>
|
||||
<Sequence>
|
||||
<SubTree ID="putdown_recovery_subtree"/>
|
||||
<AlwaysFailure/>
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
</Sequence>
|
||||
|
||||
<!-- Overall recovery if the entire sequence fails -->
|
||||
<!-- <Sequence>
|
||||
<SubTree ID="recovery_subtree"/>
|
||||
<AlwaysFailure/>
|
||||
</Sequence> -->
|
||||
</Fallback>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="vision_subtree">
|
||||
<Sequence name="vision_root">
|
||||
<RetryUntilSuccessful name="retry_vision" num_attempts="1">
|
||||
<Sequence>
|
||||
<Parallel name="parallel_action_1">
|
||||
<DualArm_H name="s1_left_arm_initial" />
|
||||
<DualArm_H name="s1_right_arm_initial" />
|
||||
<GripperCmd0_H name="s1_gripper_open" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_action_2">
|
||||
<MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
|
||||
<!-- <GripperCmd0_H name="s1_gripper_open" /> -->
|
||||
<!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
|
||||
</Parallel>
|
||||
<SubTree ID="vision_top_cam_subtree"/>
|
||||
|
||||
<DualArm_H name="s2_left_arm_cam_take_photo" />
|
||||
<SubTree ID="vision_hand_cam_subtree"/>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="vision_top_cam_subtree">
|
||||
<Sequence name="vision_top_cam_root">
|
||||
<RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
|
||||
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="vision_hand_cam_subtree">
|
||||
<Sequence name="vision_hand_cam_root">
|
||||
<RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
|
||||
<VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="grasp_subtree">
|
||||
<Sequence name="grasp_root">
|
||||
<RetryUntilSuccessful name="retry_grasp" num_attempts="1">
|
||||
<Sequence>
|
||||
<DualArm_H name="s3_left_arm_vision_pre_grasp" />
|
||||
<DualArm_H name="s4_left_arm_vision_grasp" />
|
||||
<GripperCmd0_H name="s4_gripper_close" />
|
||||
<DualArm_H name="s4_left_arm_take_box" />
|
||||
<Parallel name="parallel_action_grasp_box">
|
||||
<DualArm_H name="s5_left_arm_grasp_box" />
|
||||
<DualArm_H name="s5_right_arm_grasp_box" />
|
||||
</Parallel>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="putdown_subtree">
|
||||
<Sequence name="putdown_root">
|
||||
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
|
||||
<Sequence>
|
||||
<Parallel name="parallel_action_2">
|
||||
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
|
||||
<MoveWaist_H name="s6_move_waist_turn_right_90" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_action_s7_grasp_box">
|
||||
<DualArm_H name="s7_left_arm_grasp_box" />
|
||||
<DualArm_H name="s7_right_arm_grasp_box" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_action_s7_put_down_box">
|
||||
<DualArm_H name="s7_left_arm_put_down_box" />
|
||||
<DualArm_H name="s7_right_arm_put_down_box" />
|
||||
</Parallel>
|
||||
<GripperCmd0_H name="s8_gripper_open" />
|
||||
<DualArm_H name="s9_left_arm_grasp_box" />
|
||||
<MoveWaist_H name="s10_move_waist_turn_left_90" />
|
||||
<Parallel name="parallel_action_3">
|
||||
<DualArm_H name="s11_left_arm_initial" />
|
||||
<DualArm_H name="s11_right_arm_initial" />
|
||||
</Parallel>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="recovery_subtree">
|
||||
<Sequence name="recovery_root">
|
||||
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd0_H name="s12_gripper_open" />
|
||||
<DualArm_H name="s13_left_arm_initial" />
|
||||
<DualArm_H name="s13_right_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_overall" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="vision_recovery_subtree">
|
||||
<Sequence name="vision_recovery_root">
|
||||
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<!-- <GripperCmd0_H name="s14_gripper_open" /> -->
|
||||
<DualArm_H name="s14_left_arm_initial" />
|
||||
<DualArm_H name="s14_right_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_vision" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="grasp_recovery_subtree">
|
||||
<Sequence name="grasp_recovery_root">
|
||||
<RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd0_H name="s15_gripper_open" />
|
||||
<DualArm_H name="s15_arm_pre_grasp" />
|
||||
<DualArm_H name="s15_left_arm_cam_take_photo" />
|
||||
<DualArm_H name="s15_left_arm_initial" />
|
||||
<DualArm_H name="s15_right_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_grasp" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="putdown_recovery_subtree">
|
||||
<Sequence name="putdown_recovery_root">
|
||||
<RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd0_H name="s16_gripper_open" />
|
||||
<DualArm_H name="s16_arm_pre_grasp" />
|
||||
<DualArm_H name="s16_left_arm_cam_take_photo" />
|
||||
<DualArm_H name="s16_left_arm_initial" />
|
||||
<DualArm_H name="s16_right_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_putdown" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -22,90 +22,59 @@
|
||||
mode: 2
|
||||
|
||||
'
|
||||
- name: s1_right_arm_pre_cam_take_photo
|
||||
params: &pre_cam_take_photo 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [85, 25, 90, 0, 68, 90, 95, -25, -90, 0, -68, 90]
|
||||
|
||||
'
|
||||
- name: s1_left_arm_initial
|
||||
params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_right_arm_initial
|
||||
params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_wheel_move_to_origin_pickup_position
|
||||
params: 'move_distance: 0.5
|
||||
|
||||
move_angle: 0.0
|
||||
|
||||
'
|
||||
- name: s2_left_arm_pre_cam_take_photo
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_right_arm_pre_cam_take_photo
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [120, 20, 80, 0, 80, 120, 60, -20, -80, 0, -80, 60]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_top_camera_vision_recg
|
||||
params: 'camera_position: top
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s2_right_arm_cam_take_photo
|
||||
params: &arm_cam_take_photo 'body_id: 1
|
||||
|
||||
data_type: 100
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s3_right_camera_vision_recg
|
||||
params: 'camera_position: right
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_right_arm_vision_pre_grasp
|
||||
params: &pre_grasp 'body_id: 1
|
||||
|
||||
data_type: 101
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: &pre_grasp "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_right_arm_vision_grasp
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 102
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 102\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_gripper_close
|
||||
params: 'loc: 200
|
||||
|
||||
@@ -117,33 +86,20 @@
|
||||
|
||||
'
|
||||
- name: s4_right_arm_take_box
|
||||
params: 'body_id: 1
|
||||
|
||||
data_type: 110
|
||||
|
||||
data_length: 0
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: []
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 110\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_left_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_right_arm_grasp_box
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [85, 25, 90, 0, 68, 90, 96.30, -88.95, -45, -22.88, -16.30, 112.10]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.30, -88.95, -45, -22.88, -16.30, 112.10]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s6_move_waist_turn_left_90
|
||||
params: 'move_pitch_degree: 0.0
|
||||
|
||||
@@ -156,52 +112,54 @@
|
||||
move_angle: 0.0
|
||||
|
||||
'
|
||||
- name: s7_left_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_grasp_box
|
||||
params: &grasp_box_s7_s9 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [85, 25, 90, 0, 68, 90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
|
||||
|
||||
'
|
||||
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.30, -88.95, 13.98, -22.88, -16.30, 112.10]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_left_arm_put_down_box
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_put_down_box
|
||||
params: 'body_id: 2
|
||||
|
||||
data_type: 2
|
||||
|
||||
data_length: 12
|
||||
|
||||
command_id: 0
|
||||
|
||||
frame_time_stamp: 0
|
||||
|
||||
data_array: [85, 25, 90, 0, 68, 90, 85.78, -88.95, 13.97, 22.88, -16.29, 67.99]
|
||||
|
||||
'
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85.78, -88.95, 13.97, 22.88, -16.29, 67.99]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s8_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s9_right_arm_grasp_box
|
||||
params: *grasp_box_s7_s9
|
||||
- name: s10_move_waist_turn_right_90
|
||||
params: *waist_turn_right_90
|
||||
- name: s11_right_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s11_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s11_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s12_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s13_right_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s14_right_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s15_right_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s16_right_arm_pre_cam_take_photo
|
||||
params: *pre_cam_take_photo
|
||||
- name: s13_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s13_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s14_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s14_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s15_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s15_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s16_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s16_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s14_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s15_gripper_open
|
||||
@@ -215,4 +173,20 @@
|
||||
- name: s15_right_arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
- name: s16_right_arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
- name: clear_done_instances_overall
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_vision
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_grasp
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_putdown
|
||||
params: '{}
|
||||
|
||||
'
|
||||
@@ -1,6 +1,15 @@
|
||||
<!--
|
||||
功能描述: 右臂独立视觉抓取流程（视觉/抓取/放置均带恢复与重试）。
|
||||
流程: 右臂到手眼拍照位 -> 相机识别目标 -> 预抓取/抓取/抬起 -> 送到放置位并松爪 -> 回到初始位。
|
||||
功能描述: 右臂视觉抓取流程（顶视+手眼识别；抓取/放置含恢复；外层3次重试）。
|
||||
运行概览:
|
||||
1) 视觉阶段（vision_subtree）：双臂回初始并打开右爪；进行顶视相机识别（最多10次）；
|
||||
右臂到手眼拍照位后进行右手眼识别（最多10次）。
|
||||
2) 抓取阶段（grasp_subtree）：右臂预抓取 -> 右臂抓取 -> 关闭右爪 -> 右臂抬起；
|
||||
随后左右臂并行保持抓箱位（parallel_action_grasp_box）。
|
||||
3) 放置阶段（putdown_subtree）：左右臂并行抓箱位/放箱位 -> 右爪松开 -> 右臂回抓箱位；
|
||||
双臂并行回初始位。
|
||||
4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
|
||||
并通过 AlwaysFailure 触发阶段重试；外层总流程最多重试3次。
|
||||
说明: 底盘与腰部动作在此版本中被注释，流程主要为视觉+双臂姿态与右爪控制。
|
||||
-->
|
||||
<root BTCPP_format="4" main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
@@ -50,15 +59,20 @@
|
||||
<Sequence name="vision_root">
|
||||
<RetryUntilSuccessful name="retry_vision" num_attempts="1">
|
||||
<Sequence>
|
||||
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
|
||||
<Parallel name="parallel_action_1">
|
||||
<!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
|
||||
<DualArm_H name="s1_left_arm_initial" />
|
||||
<DualArm_H name="s1_right_arm_initial" />
|
||||
<GripperCmd1_H name="s1_gripper_open" />
|
||||
<Arm_H name="s2_right_arm_pre_cam_take_photo" />
|
||||
</Parallel>
|
||||
<!-- <Parallel name="parallel_action_2"> -->
|
||||
<!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
|
||||
<!-- <GripperCmd1_H name="s1_gripper_open" /> -->
|
||||
<!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
|
||||
<!-- </Parallel> -->
|
||||
<SubTree ID="vision_top_cam_subtree"/>
|
||||
|
||||
<Arm_H name="s2_right_arm_cam_take_photo" />
|
||||
<DualArm_H name="s2_right_arm_cam_take_photo" />
|
||||
<SubTree ID="vision_hand_cam_subtree"/>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -85,11 +99,14 @@
|
||||
<Sequence name="grasp_root">
|
||||
<RetryUntilSuccessful name="retry_grasp" num_attempts="1">
|
||||
<Sequence>
|
||||
<Arm_H name="s3_right_arm_vision_pre_grasp" />
|
||||
<Arm_H name="s4_right_arm_vision_grasp" />
|
||||
<DualArm_H name="s3_right_arm_vision_pre_grasp" />
|
||||
<DualArm_H name="s4_right_arm_vision_grasp" />
|
||||
<GripperCmd1_H name="s4_gripper_close" />
|
||||
<Arm_H name="s4_right_arm_take_box" />
|
||||
<Arm_H name="s5_right_arm_grasp_box" />
|
||||
<DualArm_H name="s4_right_arm_take_box" />
|
||||
<Parallel name="parallel_action_grasp_box">
|
||||
<DualArm_H name="s5_left_arm_grasp_box" />
|
||||
<DualArm_H name="s5_right_arm_grasp_box" />
|
||||
</Parallel>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
@@ -103,14 +120,21 @@
|
||||
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
|
||||
<MoveWaist_H name="s6_move_waist_turn_left_90" />
|
||||
</Parallel> -->
|
||||
<Arm_H name="s7_right_arm_grasp_box" />
|
||||
<Arm_H name="s7_right_arm_put_down_box" />
|
||||
<Parallel name="parallel_action_s7_grasp_box">
|
||||
<DualArm_H name="s7_left_arm_grasp_box" />
|
||||
<DualArm_H name="s7_right_arm_grasp_box" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_action_s7_put_down_box">
|
||||
<DualArm_H name="s7_left_arm_put_down_box" />
|
||||
<DualArm_H name="s7_right_arm_put_down_box" />
|
||||
</Parallel>
|
||||
<GripperCmd1_H name="s8_gripper_open" />
|
||||
<Arm_H name="s9_right_arm_grasp_box" />
|
||||
<DualArm_H name="s9_right_arm_grasp_box" />
|
||||
|
||||
<Parallel name="parallel_action_3">
|
||||
<!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
|
||||
<Arm_H name="s11_right_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s11_right_arm_initial" />
|
||||
<DualArm_H name="s11_left_arm_initial" />
|
||||
</Parallel>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -122,7 +146,8 @@
|
||||
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd1_H name="s12_gripper_open" />
|
||||
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s13_right_arm_initial" />
|
||||
<DualArm_H name="s13_left_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_overall" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -134,7 +159,8 @@
|
||||
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<!-- <GripperCmd1_H name="s14_gripper_open" /> -->
|
||||
<Arm_H name="s14_right_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s14_right_arm_initial" />
|
||||
<DualArm_H name="s14_left_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_vision" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -146,9 +172,10 @@
|
||||
<RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd1_H name="s15_gripper_open" />
|
||||
<Arm_H name="s15_arm_pre_grasp" />
|
||||
<Arm_H name="s15_right_arm_cam_take_photo" />
|
||||
<Arm_H name="s15_right_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s15_arm_pre_grasp" />
|
||||
<DualArm_H name="s15_right_arm_cam_take_photo" />
|
||||
<DualArm_H name="s15_right_arm_initial" />
|
||||
<DualArm_H name="s15_left_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_grasp" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
@@ -160,9 +187,10 @@
|
||||
<RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd1_H name="s16_gripper_open" />
|
||||
<Arm_H name="s16_arm_pre_grasp" />
|
||||
<Arm_H name="s16_right_arm_cam_take_photo" />
|
||||
<Arm_H name="s16_right_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s16_arm_pre_grasp" />
|
||||
<DualArm_H name="s16_right_arm_cam_take_photo" />
|
||||
<DualArm_H name="s16_right_arm_initial" />
|
||||
<DualArm_H name="s16_left_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_putdown" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
|
||||
196
src/brain/config/bt_vision_grasp_right_arm_demo1.params.yaml
Normal file
196
src/brain/config/bt_vision_grasp_right_arm_demo1.params.yaml
Normal file
@@ -0,0 +1,196 @@
|
||||
- name: root
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: retry_vision_hand
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: s1_move_waist_turn_right_90
|
||||
params: &waist_turn_right_90 'move_pitch_degree: 0.0
|
||||
|
||||
move_yaw_degree: -90.0
|
||||
|
||||
'
|
||||
- name: s1_gripper_open
|
||||
params: &gripper_open 'loc: 0
|
||||
|
||||
speed: 150
|
||||
|
||||
torque: 20
|
||||
|
||||
mode: 2
|
||||
|
||||
'
|
||||
- name: s1_left_arm_initial
|
||||
params: &left_arm_initial "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_right_arm_initial
|
||||
params: &right_arm_initial "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s1_wheel_move_to_origin_pickup_position
|
||||
params: 'move_distance: 0.5
|
||||
|
||||
move_angle: 0.0
|
||||
|
||||
'
|
||||
- name: s2_left_arm_pre_cam_take_photo
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_right_arm_pre_cam_take_photo
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s2_top_camera_vision_recg
|
||||
params: 'camera_position: top
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s2_right_arm_cam_take_photo
|
||||
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s3_right_camera_vision_recg
|
||||
params: 'camera_position: right
|
||||
|
||||
classes: ["medicine_box"]
|
||||
|
||||
'
|
||||
- name: s3_right_arm_vision_pre_grasp
|
||||
params: &pre_grasp "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_right_arm_vision_grasp
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 102\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s4_gripper_close
|
||||
params: 'loc: 200
|
||||
|
||||
speed: 150
|
||||
|
||||
torque: 100
|
||||
|
||||
mode: 2
|
||||
|
||||
'
|
||||
- name: s4_right_arm_take_box
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 110\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_left_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s5_right_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.30, -88.95, -45, -22.88, -16.30, 112.10]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s6_move_waist_turn_left_90
|
||||
params: &waist_turn_left_90 'move_pitch_degree: 0.0
|
||||
|
||||
move_yaw_degree: 90.0
|
||||
|
||||
'
|
||||
- name: s6_wheel_move_to_putdown_position
|
||||
params: 'move_distance: 0.0
|
||||
|
||||
move_angle: 0.0
|
||||
|
||||
'
|
||||
- name: s7_left_arm_grasp_box
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_grasp_box
|
||||
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.30, -88.95, 13.98, -22.88, -16.30, 112.10]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_left_arm_put_down_box
|
||||
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s7_right_arm_put_down_box
|
||||
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
|
||||
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
|
||||
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85.78, -88.95, 13.97, 22.88, -16.29, 67.99]\n blend_radius:\
|
||||
\ 0\nvelocity_scaling: 40\n"
|
||||
- name: s8_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s9_right_arm_grasp_box
|
||||
params: *grasp_box_s7_s9
|
||||
- name: s10_move_waist_turn_right_90
|
||||
params: *waist_turn_right_90
|
||||
- name: s11_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s11_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s12_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s13_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s13_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s14_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s14_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s15_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s15_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s16_left_arm_initial
|
||||
params: *left_arm_initial
|
||||
- name: s16_right_arm_initial
|
||||
params: *right_arm_initial
|
||||
- name: s14_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s15_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s16_gripper_open
|
||||
params: *gripper_open
|
||||
- name: s15_arm_pre_grasp
|
||||
params: *pre_grasp
|
||||
- name: s16_arm_pre_grasp
|
||||
params: *pre_grasp
|
||||
- name: s15_right_arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
- name: s16_right_arm_cam_take_photo
|
||||
params: *arm_cam_take_photo
|
||||
- name: s6_move_waist_turn_right_90
|
||||
params: *waist_turn_right_90
|
||||
- name: s10_move_waist_turn_left_90
|
||||
params: *waist_turn_left_90
|
||||
- name: clear_done_instances_overall
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_vision
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_grasp
|
||||
params: '{}
|
||||
|
||||
'
|
||||
- name: clear_done_instances_putdown
|
||||
params: '{}
|
||||
|
||||
'
|
||||
196
src/brain/config/bt_vision_grasp_right_arm_demo1.xml
Normal file
196
src/brain/config/bt_vision_grasp_right_arm_demo1.xml
Normal file
@@ -0,0 +1,196 @@
|
||||
<!--
|
||||
功能描述: 右臂视觉抓取演示流程1（顶视 + 右手眼识别；含底盘与腰部转向；外层3次重试）。
|
||||
运行概览:
|
||||
1) 视觉阶段：双臂回初始并打开右爪（GripperCmd1）；底盘移动到取物位；顶视相机识别（最多10次）；
|
||||
右臂到手眼拍照位后进行右手眼识别（最多10次）。
|
||||
2) 抓取阶段：右臂预抓取 -> 右臂抓取 -> 关闭右爪 -> 右臂抬起 -> 左右臂并行保持抓箱位。
|
||||
3) 放置阶段：底盘移动到放置位并腰部右转；左右臂并行抓箱位/放箱位 -> 右爪松开 -> 右臂回抓箱位；
|
||||
腰部左转复位，同时双臂回初始位。
|
||||
4) 恢复机制：视觉/抓取/放置各自失败时进入对应恢复子树（复位/重新拍照/清理完成标记），
|
||||
并通过 AlwaysFailure 触发阶段重试。
|
||||
-->
|
||||
<root BTCPP_format="4" main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Sequence name="root">
|
||||
<RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
|
||||
<Fallback name="fallback_main_action">
|
||||
<Sequence name="complete_pick_and_place">
|
||||
<!-- Vision with recovery -->
|
||||
<Fallback name="vision_with_recovery">
|
||||
<SubTree ID="vision_subtree"/>
|
||||
<Sequence>
|
||||
<SubTree ID="vision_recovery_subtree"/>
|
||||
<AlwaysFailure/>
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
|
||||
<!-- Grasp with recovery -->
|
||||
<Fallback name="grasp_with_recovery">
|
||||
<SubTree ID="grasp_subtree"/>
|
||||
<Sequence>
|
||||
<SubTree ID="grasp_recovery_subtree"/>
|
||||
<AlwaysFailure/>
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
|
||||
<!-- Putdown with recovery -->
|
||||
<Fallback name="putdown_with_recovery">
|
||||
<SubTree ID="putdown_subtree"/>
|
||||
<Sequence>
|
||||
<SubTree ID="putdown_recovery_subtree"/>
|
||||
<AlwaysFailure/>
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
</Sequence>
|
||||
|
||||
<!-- Overall recovery if the entire sequence fails -->
|
||||
<!-- <Sequence>
|
||||
<SubTree ID="recovery_subtree"/>
|
||||
<AlwaysFailure/>
|
||||
</Sequence> -->
|
||||
</Fallback>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="vision_subtree">
|
||||
<Sequence name="vision_root">
|
||||
<RetryUntilSuccessful name="retry_vision" num_attempts="1">
|
||||
<Sequence>
|
||||
<Parallel name="parallel_action_1">
|
||||
<DualArm_H name="s1_left_arm_initial" />
|
||||
<DualArm_H name="s1_right_arm_initial" />
|
||||
<GripperCmd1_H name="s1_gripper_open" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_action_2">
|
||||
<MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
|
||||
<!-- <GripperCmd1_H name="s1_gripper_open" /> -->
|
||||
<!-- <DualArm_H name="s2_left_arm_pre_cam_take_photo" />
|
||||
<DualArm_H name="s2_right_arm_pre_cam_take_photo" /> -->
|
||||
</Parallel>
|
||||
<SubTree ID="vision_top_cam_subtree"/>
|
||||
|
||||
<DualArm_H name="s2_right_arm_cam_take_photo" />
|
||||
<SubTree ID="vision_hand_cam_subtree"/>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="vision_top_cam_subtree">
|
||||
<Sequence name="vision_top_cam_root">
|
||||
<RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
|
||||
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="vision_hand_cam_subtree">
|
||||
<Sequence name="vision_hand_cam_root">
|
||||
<RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
|
||||
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="grasp_subtree">
|
||||
<Sequence name="grasp_root">
|
||||
<RetryUntilSuccessful name="retry_grasp" num_attempts="1">
|
||||
<Sequence>
|
||||
<DualArm_H name="s3_right_arm_vision_pre_grasp" />
|
||||
<DualArm_H name="s4_right_arm_vision_grasp" />
|
||||
<GripperCmd1_H name="s4_gripper_close" />
|
||||
<DualArm_H name="s4_right_arm_take_box" />
|
||||
<Parallel name="parallel_action_grasp_box">
|
||||
<DualArm_H name="s5_left_arm_grasp_box" />
|
||||
<DualArm_H name="s5_right_arm_grasp_box" />
|
||||
</Parallel>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="putdown_subtree">
|
||||
<Sequence name="putdown_root">
|
||||
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
|
||||
<Sequence>
|
||||
<Parallel name="parallel_action_2">
|
||||
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
|
||||
<MoveWaist_H name="s6_move_waist_turn_right_90" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_action_s7_grasp_box">
|
||||
<DualArm_H name="s7_left_arm_grasp_box" />
|
||||
<DualArm_H name="s7_right_arm_grasp_box" />
|
||||
</Parallel>
|
||||
<Parallel name="parallel_action_s7_put_down_box">
|
||||
<DualArm_H name="s7_left_arm_put_down_box" />
|
||||
<DualArm_H name="s7_right_arm_put_down_box" />
|
||||
</Parallel>
|
||||
<GripperCmd1_H name="s8_gripper_open" />
|
||||
<DualArm_H name="s9_right_arm_grasp_box" />
|
||||
<MoveWaist_H name="s10_move_waist_turn_left_90" />
|
||||
<Parallel name="parallel_action_3">
|
||||
<DualArm_H name="s11_right_arm_initial" />
|
||||
<DualArm_H name="s11_left_arm_initial" />
|
||||
</Parallel>
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="recovery_subtree">
|
||||
<Sequence name="recovery_root">
|
||||
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd1_H name="s12_gripper_open" />
|
||||
<DualArm_H name="s13_right_arm_initial" />
|
||||
<DualArm_H name="s13_left_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_overall" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="vision_recovery_subtree">
|
||||
<Sequence name="vision_recovery_root">
|
||||
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<!-- <GripperCmd1_H name="s14_gripper_open" /> -->
|
||||
<DualArm_H name="s14_right_arm_initial" />
|
||||
<DualArm_H name="s14_left_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_vision" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="grasp_recovery_subtree">
|
||||
<Sequence name="grasp_recovery_root">
|
||||
<RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd1_H name="s15_gripper_open" />
|
||||
<DualArm_H name="s15_arm_pre_grasp" />
|
||||
<DualArm_H name="s15_right_arm_cam_take_photo" />
|
||||
<DualArm_H name="s15_right_arm_initial" />
|
||||
<DualArm_H name="s15_left_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_grasp" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<BehaviorTree ID="putdown_recovery_subtree">
|
||||
<Sequence name="putdown_recovery_root">
|
||||
<RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
|
||||
<Sequence>
|
||||
<GripperCmd1_H name="s16_gripper_open" />
|
||||
<DualArm_H name="s16_arm_pre_grasp" />
|
||||
<DualArm_H name="s16_right_arm_cam_take_photo" />
|
||||
<DualArm_H name="s16_right_arm_initial" />
|
||||
<DualArm_H name="s16_left_arm_initial" />
|
||||
<ClearDoneInstances_H name="clear_done_instances_putdown" />
|
||||
</Sequence>
|
||||
</RetryUntilSuccessful>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
@@ -1,55 +0,0 @@
|
||||
# Example configuration file showing how to configure skill-specific parameters
|
||||
# This can be used as an additional parameters file in addition to the main brain_node_params.yaml
|
||||
|
||||
cerebrum_node:
|
||||
ros__parameters:
|
||||
# Skill-specific parameters
|
||||
cerebrum:
|
||||
bt:
|
||||
VisionObjectRecognition:
|
||||
topic: "vision_object_recognition"
|
||||
payload_yaml: ""
|
||||
timeout_ms: "10000"
|
||||
Arm:
|
||||
topic: "arm_control"
|
||||
payload_yaml: ""
|
||||
timeout_ms: "30000"
|
||||
CameraTakePhoto:
|
||||
topic: "camera_take_photo"
|
||||
payload_yaml: ""
|
||||
timeout_ms: "5000"
|
||||
HandControl:
|
||||
topic: "hand_control"
|
||||
payload_yaml: ""
|
||||
timeout_ms: "5000"
|
||||
MoveWaist:
|
||||
topic: "move_waist"
|
||||
payload_yaml: ""
|
||||
timeout_ms: "10000"
|
||||
MoveLeg:
|
||||
topic: "move_leg"
|
||||
payload_yaml: ""
|
||||
timeout_ms: "15000"
|
||||
MoveWheel:
|
||||
topic: "move_wheel"
|
||||
payload_yaml: ""
|
||||
timeout_ms: "20000"
|
||||
MoveHome:
|
||||
topic: "move_home"
|
||||
payload_yaml: ""
|
||||
timeout_ms: "30000"
|
||||
GripperCmd:
|
||||
topic: "gripper_cmd0"
|
||||
payload_yaml: ""
|
||||
timeout_ms: "5000"
|
||||
|
||||
# Example instance-specific overrides
|
||||
Arm:
|
||||
s1_arm_move_to_object:
|
||||
topic: "arm_control_specific"
|
||||
payload_yaml: "{target_pose: {x: 0.1, y: 0.2, z: 0.3}}"
|
||||
timeout_ms: "45000"
|
||||
s2_arm_grasp_object:
|
||||
topic: "arm_control_grasp"
|
||||
payload_yaml: "{target_pose: {x: 0.15, y: 0.25, z: 0.35}, grip_force: 10.0}"
|
||||
timeout_ms: "50000"
|
||||
@@ -9,7 +9,10 @@
|
||||
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
|
||||
{config: /config/bt_vision_grasp_left_arm.xml, param: /config/bt_vision_grasp_left_arm.params.yaml},
|
||||
{config: /config/bt_vision_grasp_right_arm.xml, param: /config/bt_vision_grasp_right_arm.params.yaml},
|
||||
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml}
|
||||
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml},
|
||||
{config: /config/bt_vision_grasp_left_arm_demo1.xml, param: /config/bt_vision_grasp_left_arm_demo1.params.yaml},
|
||||
{config: /config/bt_vision_grasp_right_arm_demo1.xml, param: /config/bt_vision_grasp_right_arm_demo1.params.yaml},
|
||||
{config: /config/bt_vision_grasp_dual_arm_demo1.xml, param: /config/bt_vision_grasp_dual_arm_demo1.params.yaml}
|
||||
]
|
||||
|
||||
- name: cerebellum_node
|
||||
@@ -19,5 +22,8 @@
|
||||
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
|
||||
{config: /config/bt_vision_grasp_left_arm.xml, param: /config/bt_vision_grasp_left_arm.params.yaml},
|
||||
{config: /config/bt_vision_grasp_right_arm.xml, param: /config/bt_vision_grasp_right_arm.params.yaml},
|
||||
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml}
|
||||
{config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml},
|
||||
{config: /config/bt_vision_grasp_left_arm_demo1.xml, param: /config/bt_vision_grasp_left_arm_demo1.params.yaml},
|
||||
{config: /config/bt_vision_grasp_right_arm_demo1.xml, param: /config/bt_vision_grasp_right_arm_demo1.params.yaml},
|
||||
{config: /config/bt_vision_grasp_dual_arm_demo1.xml, param: /config/bt_vision_grasp_dual_arm_demo1.params.yaml}
|
||||
]
|
||||
@@ -2,19 +2,22 @@
|
||||
version: 1.0.0
|
||||
description: "视觉识别指定物体"
|
||||
interfaces:
|
||||
- VisionObjectRecognition.srv
|
||||
- name: VisionObjectRecognition.srv
|
||||
default_topic: "/vision_object_recognition"
|
||||
- name: VisionObjectRecognition.action
|
||||
default_topic: "/vision_object_recognition"
|
||||
|
||||
- name: MapLoad
|
||||
version: 1.0.0
|
||||
description: "加载地图"
|
||||
interfaces:
|
||||
- MapLoad.srv
|
||||
# - name: MapLoad
|
||||
# version: 1.0.0
|
||||
# description: "加载地图"
|
||||
# interfaces:
|
||||
# - MapLoad.srv
|
||||
|
||||
- name: MapSave
|
||||
version: 1.0.0
|
||||
description: "保存地图"
|
||||
interfaces:
|
||||
- MapSave.srv
|
||||
# - name: MapSave
|
||||
# version: 1.0.0
|
||||
# description: "保存地图"
|
||||
# interfaces:
|
||||
# - MapSave.srv
|
||||
|
||||
- name: Arm
|
||||
version: 1.0.0
|
||||
@@ -23,17 +26,24 @@
|
||||
- name: Arm.action
|
||||
default_topic: "ArmAction"
|
||||
|
||||
- name: DualArm
|
||||
version: 1.0.0
|
||||
description: "双臂控制"
|
||||
interfaces:
|
||||
- name: DualArm.action
|
||||
default_topic: "DualArm"
|
||||
|
||||
- name: CameraTakePhoto
|
||||
version: 1.0.0
|
||||
description: "相机拍照"
|
||||
interfaces:
|
||||
- CameraTakePhoto.action
|
||||
|
||||
- name: HandControl
|
||||
version: 1.0.0
|
||||
description: "手部控制"
|
||||
interfaces:
|
||||
- HandControl.action
|
||||
# - name: HandControl
|
||||
# version: 1.0.0
|
||||
# description: "手部控制"
|
||||
# interfaces:
|
||||
# - HandControl.action
|
||||
|
||||
- name: MoveWaist
|
||||
version: 1.0.0
|
||||
|
||||
@@ -11,6 +11,7 @@
|
||||
#include <optional>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
/**
|
||||
@@ -144,11 +145,11 @@ public:
|
||||
{
|
||||
virtual ~EntryBase() = default;
|
||||
/** @brief Send a goal using the entry's callbacks. */
|
||||
virtual void send(rclcpp::Logger logger) = 0;
|
||||
virtual void send() = 0;
|
||||
/** @brief Check if server is available within timeout. */
|
||||
virtual bool available(std::chrono::nanoseconds timeout) const = 0;
|
||||
/** @brief Attempt to cancel last goal if present. */
|
||||
virtual void cancel(rclcpp::Logger logger) = 0;
|
||||
virtual void cancel() = 0;
|
||||
};
|
||||
|
||||
template<typename ActionT>
|
||||
@@ -178,16 +179,16 @@ public:
|
||||
client = rclcpp_action::create_client<ActionT>(node, name);
|
||||
}
|
||||
|
||||
void send(rclcpp::Logger logger) override
|
||||
void send() override
|
||||
{
|
||||
if (!client) {return;}
|
||||
RCLCPP_INFO(logger, "[ActionClientRegistry] waiting for server '%s'", name.c_str());
|
||||
LOG_INFO("[ActionClientRegistry] waiting for server '{}'", name.c_str());
|
||||
if (!client->wait_for_action_server(std::chrono::seconds(10))) {
|
||||
RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
|
||||
LOG_ERROR("Action server '{}' not available", name.c_str());
|
||||
return;
|
||||
}
|
||||
auto goal_msg = make_goal ? make_goal() : Goal{};
|
||||
RCLCPP_INFO(logger, "[ActionClientRegistry] sending goal on '%s'", name.c_str());
|
||||
LOG_INFO("[ActionClientRegistry] sending goal on '{}'", name.c_str());
|
||||
|
||||
auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
|
||||
if (on_goal_response) {opts.goal_response_callback = on_goal_response;}
|
||||
@@ -195,7 +196,7 @@ public:
|
||||
if (on_result) {opts.result_callback = on_result;}
|
||||
auto future_handle = client->async_send_goal(goal_msg, opts);
|
||||
std::thread(
|
||||
[future_handle, this, logger]() mutable {
|
||||
[future_handle, this]() mutable {
|
||||
if (future_handle.wait_for(std::chrono::seconds(0)) != std::future_status::ready) {
|
||||
future_handle.wait();
|
||||
}
|
||||
@@ -203,11 +204,10 @@ public:
|
||||
auto gh = future_handle.get();
|
||||
if (gh) {
|
||||
last_goal_handle = gh;
|
||||
RCLCPP_INFO(logger, "Stored goal handle for '%s'", name.c_str());
|
||||
LOG_INFO("Stored goal handle for '{}'", name.c_str());
|
||||
}
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_WARN(
|
||||
logger, "Failed to obtain goal handle for '%s': %s", name.c_str(),
|
||||
LOG_WARN("Failed to obtain goal handle for '{}': {}", name.c_str(),
|
||||
e.what());
|
||||
}
|
||||
}).detach();
|
||||
@@ -219,14 +219,14 @@ public:
|
||||
return client->wait_for_action_server(timeout);
|
||||
}
|
||||
|
||||
void cancel(rclcpp::Logger logger) override
|
||||
void cancel() override
|
||||
{
|
||||
auto gh = last_goal_handle.lock();
|
||||
if (!gh) {
|
||||
RCLCPP_WARN(logger, "No active goal handle to cancel for '%s'", name.c_str());
|
||||
LOG_WARN("No active goal handle to cancel for '{}'", name.c_str());
|
||||
return;
|
||||
}
|
||||
RCLCPP_INFO(logger, "Cancelling goal on '%s'", name.c_str());
|
||||
LOG_INFO("Cancelling goal on '{}'", name.c_str());
|
||||
client->async_cancel_goal(gh);
|
||||
}
|
||||
};
|
||||
@@ -250,7 +250,7 @@ public:
|
||||
std::function<void(const std::shared_ptr<GoalHandle<ActionT>> &)> on_goal_response = nullptr)
|
||||
{
|
||||
if (entries_.find(name) != entries_.end()) {
|
||||
RCLCPP_WARN(node_->get_logger(), "[ActionClientRegistry] Overwriting existing client for '%s'", name.c_str());
|
||||
LOG_WARN("[ActionClientRegistry] Overwriting existing client for '{}'", name.c_str());
|
||||
}
|
||||
entries_[name] =
|
||||
std::make_unique<Entry<ActionT>>(
|
||||
@@ -263,15 +263,15 @@ public:
|
||||
* @param name Action client key.
|
||||
* @param logger Logger instance.
|
||||
*/
|
||||
void send(const std::string & name, rclcpp::Logger logger)
|
||||
void send(const std::string & name)
|
||||
{
|
||||
auto it = entries_.find(name);
|
||||
if (it == entries_.end()) {
|
||||
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
|
||||
LOG_ERROR("No action client registered for '{}'", name.c_str());
|
||||
return;
|
||||
}
|
||||
RCLCPP_INFO(logger, "Sending action '%s'", name.c_str());
|
||||
it->second->send(logger);
|
||||
LOG_INFO("Sending action '{}'", name.c_str());
|
||||
it->second->send();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -279,14 +279,14 @@ public:
|
||||
* @param name Action client key.
|
||||
* @param logger Logger instance.
|
||||
*/
|
||||
void cancel(const std::string & name, rclcpp::Logger logger)
|
||||
void cancel(const std::string & name)
|
||||
{
|
||||
auto it = entries_.find(name);
|
||||
if (it == entries_.end()) {
|
||||
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
|
||||
LOG_ERROR("No action client registered for '{}'", name.c_str());
|
||||
return;
|
||||
}
|
||||
it->second->cancel(logger);
|
||||
it->second->cancel();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -333,22 +333,21 @@ public:
|
||||
template<typename ActionT>
|
||||
std::optional<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult> send_and_wait(
|
||||
const std::string & name,
|
||||
rclcpp::Logger logger,
|
||||
std::chrono::nanoseconds timeout = std::chrono::seconds(30))
|
||||
{
|
||||
auto it = entries_.find(name);
|
||||
if (it == entries_.end()) {
|
||||
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
|
||||
LOG_ERROR("No action client registered for '{}'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
auto * e = dynamic_cast<Entry<ActionT> *>(it->second.get());
|
||||
if (!e) {
|
||||
RCLCPP_ERROR(logger, "Action client type mismatch for '%s'", name.c_str());
|
||||
LOG_ERROR("Action client type mismatch for '{}'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
if (!e->client) {return std::nullopt;}
|
||||
if (!e->client->wait_for_action_server(std::chrono::seconds(10))) {
|
||||
RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
|
||||
LOG_ERROR("Action server '{}' not available", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
auto goal = e->make_goal ? e->make_goal() : typename ActionT::Goal{};
|
||||
@@ -357,7 +356,7 @@ public:
|
||||
auto goal_future = e->client->async_send_goal(goal, opts);
|
||||
auto goal_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
|
||||
if (goal_future.wait_until(goal_deadline) != std::future_status::ready) {
|
||||
RCLCPP_ERROR(logger, "Timed out waiting for goal response on '%s'", name.c_str());
|
||||
LOG_ERROR("Timed out waiting for goal response on '{}'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
@@ -365,11 +364,11 @@ public:
|
||||
try {
|
||||
goal_handle = goal_future.get();
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(logger, "Failed to get goal handle for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_ERROR("Failed to get goal handle for '{}': {}", name.c_str(), ex.what());
|
||||
return std::nullopt;
|
||||
}
|
||||
if (!goal_handle) {
|
||||
RCLCPP_WARN(logger, "Goal rejected for '%s'", name.c_str());
|
||||
LOG_WARN("Goal rejected for '{}'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
e->last_goal_handle = goal_handle;
|
||||
@@ -377,18 +376,18 @@ public:
|
||||
try {
|
||||
e->on_goal_response(goal_handle);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN(logger, "Goal response callback threw for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_WARN("Goal response callback threw for '{}': {}", name.c_str(), ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
auto result_future = e->client->async_get_result(goal_handle);
|
||||
auto deadline = std::chrono::steady_clock::now() + timeout;
|
||||
if (result_future.wait_until(deadline) != std::future_status::ready) {
|
||||
RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
|
||||
LOG_ERROR("Timed out waiting for result on '{}', sending cancel", name.c_str());
|
||||
try {
|
||||
e->client->async_cancel_goal(goal_handle);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_WARN("Failed cancelling goal for '{}': {}", name.c_str(), ex.what());
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
@@ -396,14 +395,14 @@ public:
|
||||
try {
|
||||
wrapped_result = result_future.get();
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(logger, "Failed to get result for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_ERROR("Failed to get result for '{}': {}", name.c_str(), ex.what());
|
||||
return std::nullopt;
|
||||
}
|
||||
if (e->on_result) {
|
||||
try {
|
||||
e->on_result(wrapped_result);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN(logger, "Result callback threw for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_WARN("Result callback threw for '{}': {}", name.c_str(), ex.what());
|
||||
}
|
||||
}
|
||||
return wrapped_result;
|
||||
@@ -412,23 +411,22 @@ public:
|
||||
template<typename ActionT>
|
||||
std::optional<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult> send_and_wait_with_goal(
|
||||
const std::string & name,
|
||||
rclcpp::Logger logger,
|
||||
const typename ActionT::Goal & goal,
|
||||
std::chrono::nanoseconds timeout = std::chrono::seconds(30))
|
||||
{
|
||||
auto it = entries_.find(name);
|
||||
if (it == entries_.end()) {
|
||||
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
|
||||
LOG_ERROR("No action client registered for '{}'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
auto * e = dynamic_cast<Entry<ActionT> *>(it->second.get());
|
||||
if (!e) {
|
||||
RCLCPP_ERROR(logger, "Action client type mismatch for '%s'", name.c_str());
|
||||
LOG_ERROR("Action client type mismatch for '{}'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
if (!e->client) {return std::nullopt;}
|
||||
if (!e->client->wait_for_action_server(std::chrono::seconds(10))) {
|
||||
RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
|
||||
LOG_ERROR("Action server '{}' not available", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
|
||||
@@ -438,7 +436,7 @@ public:
|
||||
auto goal_future = e->client->async_send_goal(goal, opts);
|
||||
auto goal_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
|
||||
if (goal_future.wait_until(goal_deadline) != std::future_status::ready) {
|
||||
RCLCPP_ERROR(logger, "Timed out waiting for goal response on '%s'", name.c_str());
|
||||
LOG_ERROR("Timed out waiting for goal response on '{}'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
@@ -446,11 +444,11 @@ public:
|
||||
try {
|
||||
goal_handle = goal_future.get();
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(logger, "Failed to get goal handle for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_ERROR("Failed to get goal handle for '{}': {}", name.c_str(), ex.what());
|
||||
return std::nullopt;
|
||||
}
|
||||
if (!goal_handle) {
|
||||
RCLCPP_WARN(logger, "Goal rejected for '%s'", name.c_str());
|
||||
LOG_WARN("Goal rejected for '{}'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
e->last_goal_handle = goal_handle;
|
||||
@@ -458,18 +456,18 @@ public:
|
||||
try {
|
||||
e->on_goal_response(goal_handle);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN(logger, "Goal response callback threw for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_WARN("Goal response callback threw for '{}': {}", name.c_str(), ex.what());
|
||||
}
|
||||
}
|
||||
|
||||
auto result_future = e->client->async_get_result(goal_handle);
|
||||
auto deadline = std::chrono::steady_clock::now() + timeout;
|
||||
if (result_future.wait_until(deadline) != std::future_status::ready) {
|
||||
RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
|
||||
LOG_ERROR("Timed out waiting for result on '{}', sending cancel", name.c_str());
|
||||
try {
|
||||
e->client->async_cancel_goal(goal_handle);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_WARN("Failed cancelling goal for '{}': {}", name.c_str(), ex.what());
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
@@ -477,14 +475,14 @@ public:
|
||||
try {
|
||||
wrapped_result = result_future.get();
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_ERROR(logger, "Failed to get result for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_ERROR("Failed to get result for '{}': {}", name.c_str(), ex.what());
|
||||
return std::nullopt;
|
||||
}
|
||||
if (e->on_result) {
|
||||
try {
|
||||
e->on_result(wrapped_result);
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN(logger, "Result callback threw for '%s': %s", name.c_str(), ex.what());
|
||||
LOG_WARN("Result callback threw for '{}': {}", name.c_str(), ex.what());
|
||||
}
|
||||
}
|
||||
return wrapped_result;
|
||||
|
||||
@@ -25,6 +25,7 @@
|
||||
#include <behaviortree_cpp/bt_factory.h>
|
||||
#include <behaviortree_cpp/action_node.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <string>
|
||||
@@ -203,12 +204,12 @@ public:
|
||||
auto it = bt_sequences_.find(seq_name);
|
||||
if (it == bt_sequences_.end()) {
|
||||
if (node_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "BT sequence '%s' not found", seq_name.c_str());
|
||||
LOG_ERROR("BT sequence '{}' not found", seq_name.c_str());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
const std::string xml_text = build_xml_from_sequence(it->second);
|
||||
RCLCPP_INFO(node_->get_logger(), "Building BT sequence '%s'", xml_text.c_str());
|
||||
LOG_INFO("Building BT sequence '{}'", xml_text.c_str());
|
||||
out_tree = factory_.createTreeFromText(xml_text);
|
||||
return true;
|
||||
}
|
||||
@@ -226,7 +227,7 @@ public:
|
||||
return true;
|
||||
} catch (const std::exception & e) {
|
||||
if (node_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Failed to build BT from XML text: %s", e.what());
|
||||
LOG_ERROR("Failed to build BT from XML text: {}", e.what());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@@ -248,7 +249,7 @@ public:
|
||||
std::ifstream ifs(file_path);
|
||||
if (!ifs.is_open()) {
|
||||
if (node_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Could not open BT XML file: %s", file_path.c_str());
|
||||
LOG_ERROR("Could not open BT XML file: {}", file_path.c_str());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@@ -256,12 +257,12 @@ public:
|
||||
buffer << ifs.rdbuf();
|
||||
if (buffer.str().empty()) {
|
||||
if (node_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "BT XML file empty: %s", file_path.c_str());
|
||||
LOG_ERROR("BT XML file empty: {}", file_path.c_str());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (node_) {
|
||||
RCLCPP_INFO(node_->get_logger(), "Building BT from XML file: %s \n%s", file_path.c_str(), buffer.str().c_str());
|
||||
LOG_INFO("Building BT from XML file: {} \n{}", file_path.c_str(), buffer.str().c_str());
|
||||
}
|
||||
return build_tree_from_xml_text(buffer.str(), out_tree);
|
||||
}
|
||||
|
||||
@@ -12,6 +12,7 @@ public:
|
||||
static void ConfigureActionHooks(CerebellumNode * node);
|
||||
static void ConfigureArmSpaceControlHooks(CerebellumNode * node);
|
||||
static void ConfigureArmHooks(CerebellumNode * node);
|
||||
static void ConfigureDualArmHooks(CerebellumNode * node);
|
||||
static void ConfigureHandControlHooks(CerebellumNode * node);
|
||||
static void ConfigureCameraTakePhotoHooks(CerebellumNode * node);
|
||||
static void ConfigureLegControlHooks(CerebellumNode * node);
|
||||
|
||||
@@ -7,6 +7,7 @@
|
||||
#include <vector>
|
||||
#include <unordered_map>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
@@ -113,7 +114,7 @@ private:
|
||||
std::vector<double> top_cam_left_arm_orientation_{0.7071, 0.0, 0.0, 0.7071};
|
||||
double side_cam_right_arm_y_offset_{0.02};
|
||||
double side_cam_right_arm_z_offset_{0.02};
|
||||
double side_cam_left_arm_y_offset_{0.03};
|
||||
double side_cam_left_arm_y_offset_{-0.02};
|
||||
double side_cam_left_arm_z_offset_{0.01};
|
||||
double take_object_arm_x_{-0.05};
|
||||
double vision_object_recognition_wait_timeout_sec_{2.0};
|
||||
@@ -143,6 +144,7 @@ private:
|
||||
int arm_cmd_type_take_object_{110};
|
||||
int arm_cmd_type_custom_min_{100};
|
||||
int arm_cmd_type_custom_max_{120};
|
||||
std::string dual_arm_motion_type_{"MOVEJ"};
|
||||
|
||||
// Optional per-skill override timeout (skill_name -> seconds)
|
||||
std::unordered_map<std::string, double> skill_timeouts_;
|
||||
@@ -212,10 +214,10 @@ private:
|
||||
try {
|
||||
auto node = YAML::Load(c.payload_yaml);
|
||||
out = brain::from_yaml<T>(node);
|
||||
RCLCPP_INFO(rclcpp::Node::get_logger(), "[%s] payload_yaml parse success", skill_name.c_str());
|
||||
LOG_INFO("[{}] payload_yaml parse success", skill_name.c_str());
|
||||
return true;
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_ERROR(rclcpp::Node::get_logger(), "[%s] payload_yaml parse failed: %s",
|
||||
LOG_ERROR("[{}] payload_yaml parse failed: {}",
|
||||
skill_name.c_str(), e.what());
|
||||
}
|
||||
break;
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
#include "interfaces/action/arm.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
#include "interfaces/action/camera_take_photo.hpp"
|
||||
#include "interfaces/action/dual_arm.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/gripper_cmd.hpp"
|
||||
#include "interfaces/action/hand_control.hpp"
|
||||
@@ -20,12 +21,21 @@
|
||||
#include "interfaces/action/move_wheel.hpp"
|
||||
#include "interfaces/action/slam_mode.hpp"
|
||||
#include "interfaces/action/vision_grasp_object.hpp"
|
||||
#include "interfaces/action/vision_object_recognition.hpp"
|
||||
#include "interfaces/msg/arm_motion_params.hpp"
|
||||
#include "interfaces/msg/skill_call.hpp"
|
||||
#include "interfaces/srv/asr_recognize.hpp"
|
||||
#include "interfaces/srv/audio_data.hpp"
|
||||
#include "interfaces/srv/bt_rebuild.hpp"
|
||||
#include "interfaces/srv/clear_arm_error.hpp"
|
||||
#include "interfaces/srv/gripper_cmd.hpp"
|
||||
#include "interfaces/srv/inverse_kinematics.hpp"
|
||||
#include "interfaces/srv/map_load.hpp"
|
||||
#include "interfaces/srv/map_save.hpp"
|
||||
#include "interfaces/srv/motor_info.hpp"
|
||||
#include "interfaces/srv/motor_param.hpp"
|
||||
#include "interfaces/srv/tts_synthesize.hpp"
|
||||
#include "interfaces/srv/vad_event.hpp"
|
||||
#include "interfaces/srv/vision_object_recognition.hpp"
|
||||
|
||||
|
||||
@@ -34,37 +44,34 @@ namespace brain {
|
||||
template<typename T>
|
||||
T from_yaml(const YAML::Node & n);
|
||||
|
||||
// VisionObjectRecognition::Request: camera_position
|
||||
// VisionObjectRecognition::Request: camera_position, classes
|
||||
template<>
|
||||
inline interfaces::srv::VisionObjectRecognition::Request from_yaml<interfaces::srv::VisionObjectRecognition::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::VisionObjectRecognition::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["camera_position"]) r.camera_position = n["camera_position"].as<std::string>();
|
||||
if (n["classes"]) {
|
||||
auto vec = n["classes"].as<std::vector<std::string>>();
|
||||
r.classes.assign(vec.begin(), vec.end());
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// MapLoad::Request: map_url
|
||||
// VisionObjectRecognition::Goal: camera_position, classes
|
||||
template<>
|
||||
inline interfaces::srv::MapLoad::Request from_yaml<interfaces::srv::MapLoad::Request>(const YAML::Node & n)
|
||||
inline interfaces::action::VisionObjectRecognition::Goal from_yaml<interfaces::action::VisionObjectRecognition::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::MapLoad::Request r;
|
||||
interfaces::action::VisionObjectRecognition::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
|
||||
if (n["camera_position"]) g.camera_position = n["camera_position"].as<std::string>();
|
||||
if (n["classes"]) {
|
||||
auto vec = n["classes"].as<std::vector<std::string>>();
|
||||
g.classes.assign(vec.begin(), vec.end());
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// MapSave::Request: map_url
|
||||
template<>
|
||||
inline interfaces::srv::MapSave::Request from_yaml<interfaces::srv::MapSave::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::MapSave::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
return g;
|
||||
}
|
||||
|
||||
// Arm::Goal: body_id, data_type, data_length, command_id, frame_time_stamp, data_array
|
||||
@@ -90,6 +97,43 @@ inline interfaces::action::Arm::Goal from_yaml<interfaces::action::Arm::Goal>(co
|
||||
return g;
|
||||
}
|
||||
|
||||
// DualArm::Goal: arm_motion_params, velocity_scaling
|
||||
template<>
|
||||
inline interfaces::action::DualArm::Goal from_yaml<interfaces::action::DualArm::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::DualArm::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["arm_motion_params"] && n["arm_motion_params"].IsSequence()) {
|
||||
for (const auto & item : n["arm_motion_params"]) {
|
||||
interfaces::msg::ArmMotionParams tmp;
|
||||
if (item["arm_id"]) tmp.arm_id = item["arm_id"].as<uint8_t>();
|
||||
if (item["motion_type"]) tmp.motion_type = item["motion_type"].as<uint8_t>();
|
||||
if (auto node_1_target_pose = item["target_pose"]) {
|
||||
if (auto node_2_position = node_1_target_pose["position"]) {
|
||||
if (node_2_position["x"]) tmp.target_pose.position.x = node_2_position["x"].as<double>();
|
||||
if (node_2_position["y"]) tmp.target_pose.position.y = node_2_position["y"].as<double>();
|
||||
if (node_2_position["z"]) tmp.target_pose.position.z = node_2_position["z"].as<double>();
|
||||
}
|
||||
if (auto node_2_orientation = node_1_target_pose["orientation"]) {
|
||||
if (node_2_orientation["x"]) tmp.target_pose.orientation.x = node_2_orientation["x"].as<double>();
|
||||
if (node_2_orientation["y"]) tmp.target_pose.orientation.y = node_2_orientation["y"].as<double>();
|
||||
if (node_2_orientation["z"]) tmp.target_pose.orientation.z = node_2_orientation["z"].as<double>();
|
||||
if (node_2_orientation["w"]) tmp.target_pose.orientation.w = node_2_orientation["w"].as<double>();
|
||||
}
|
||||
}
|
||||
if (item["target_joints"]) {
|
||||
auto vec = item["target_joints"].as<std::vector<double>>();
|
||||
tmp.target_joints.assign(vec.begin(), vec.end());
|
||||
}
|
||||
if (item["blend_radius"]) tmp.blend_radius = item["blend_radius"].as<int32_t>();
|
||||
g.arm_motion_params.push_back(tmp);
|
||||
}
|
||||
}
|
||||
if (n["velocity_scaling"]) g.velocity_scaling = n["velocity_scaling"].as<int32_t>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// CameraTakePhoto::Goal: mode, effort
|
||||
template<>
|
||||
inline interfaces::action::CameraTakePhoto::Goal from_yaml<interfaces::action::CameraTakePhoto::Goal>(const YAML::Node & n)
|
||||
@@ -102,18 +146,6 @@ inline interfaces::action::CameraTakePhoto::Goal from_yaml<interfaces::action::C
|
||||
return g;
|
||||
}
|
||||
|
||||
// HandControl::Goal: mode, effort
|
||||
template<>
|
||||
inline interfaces::action::HandControl::Goal from_yaml<interfaces::action::HandControl::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::HandControl::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["mode"]) g.mode = n["mode"].as<int32_t>();
|
||||
if (n["effort"]) g.effort = n["effort"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// MoveWaist::Goal: move_pitch_degree, move_yaw_degree
|
||||
template<>
|
||||
inline interfaces::action::MoveWaist::Goal from_yaml<interfaces::action::MoveWaist::Goal>(const YAML::Node & n)
|
||||
@@ -173,6 +205,108 @@ inline interfaces::action::GripperCmd::Goal from_yaml<interfaces::action::Grippe
|
||||
return g;
|
||||
}
|
||||
|
||||
// LegControl::Goal: target
|
||||
template<>
|
||||
inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::LegControl::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (auto node_0_target = n["target"]) {
|
||||
if (auto node_1_header = node_0_target["header"]) {
|
||||
if (auto node_2_stamp = node_1_header["stamp"]) {
|
||||
if (node_2_stamp["sec"]) g.target.header.stamp.sec = node_2_stamp["sec"].as<int32_t>();
|
||||
if (node_2_stamp["nanosec"]) g.target.header.stamp.nanosec = node_2_stamp["nanosec"].as<uint32_t>();
|
||||
}
|
||||
if (node_1_header["frame_id"]) g.target.header.frame_id = node_1_header["frame_id"].as<std::string>();
|
||||
}
|
||||
if (auto node_1_twist = node_0_target["twist"]) {
|
||||
if (auto node_2_linear = node_1_twist["linear"]) {
|
||||
if (node_2_linear["x"]) g.target.twist.linear.x = node_2_linear["x"].as<double>();
|
||||
if (node_2_linear["y"]) g.target.twist.linear.y = node_2_linear["y"].as<double>();
|
||||
if (node_2_linear["z"]) g.target.twist.linear.z = node_2_linear["z"].as<double>();
|
||||
}
|
||||
if (auto node_2_angular = node_1_twist["angular"]) {
|
||||
if (node_2_angular["x"]) g.target.twist.angular.x = node_2_angular["x"].as<double>();
|
||||
if (node_2_angular["y"]) g.target.twist.angular.y = node_2_angular["y"].as<double>();
|
||||
if (node_2_angular["z"]) g.target.twist.angular.z = node_2_angular["z"].as<double>();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// HandControl::Goal: mode, effort
|
||||
template<>
|
||||
inline interfaces::action::HandControl::Goal from_yaml<interfaces::action::HandControl::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::HandControl::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["mode"]) g.mode = n["mode"].as<int32_t>();
|
||||
if (n["effort"]) g.effort = n["effort"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// SlamMode::Goal: enable_mapping
|
||||
template<>
|
||||
inline interfaces::action::SlamMode::Goal from_yaml<interfaces::action::SlamMode::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::SlamMode::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["enable_mapping"]) g.enable_mapping = n["enable_mapping"].as<bool>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// MoveToPosition::Goal: target_x, target_y, target_z, target_angle_pitch, target_angle_roll, target_angle_yaw
|
||||
template<>
|
||||
inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::MoveToPosition::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["target_x"]) g.target_x = n["target_x"].as<float>();
|
||||
if (n["target_y"]) g.target_y = n["target_y"].as<float>();
|
||||
if (n["target_z"]) g.target_z = n["target_z"].as<float>();
|
||||
if (n["target_angle_pitch"]) g.target_angle_pitch = n["target_angle_pitch"].as<float>();
|
||||
if (n["target_angle_roll"]) g.target_angle_roll = n["target_angle_roll"].as<float>();
|
||||
if (n["target_angle_yaw"]) g.target_angle_yaw = n["target_angle_yaw"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// ArmSpaceControl::Goal: target
|
||||
template<>
|
||||
inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::ArmSpaceControl::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::ArmSpaceControl::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (auto node_0_target = n["target"]) {
|
||||
if (auto node_1_header = node_0_target["header"]) {
|
||||
if (auto node_2_stamp = node_1_header["stamp"]) {
|
||||
if (node_2_stamp["sec"]) g.target.header.stamp.sec = node_2_stamp["sec"].as<int32_t>();
|
||||
if (node_2_stamp["nanosec"]) g.target.header.stamp.nanosec = node_2_stamp["nanosec"].as<uint32_t>();
|
||||
}
|
||||
if (node_1_header["frame_id"]) g.target.header.frame_id = node_1_header["frame_id"].as<std::string>();
|
||||
}
|
||||
if (auto node_1_pose = node_0_target["pose"]) {
|
||||
if (auto node_2_position = node_1_pose["position"]) {
|
||||
if (node_2_position["x"]) g.target.pose.position.x = node_2_position["x"].as<double>();
|
||||
if (node_2_position["y"]) g.target.pose.position.y = node_2_position["y"].as<double>();
|
||||
if (node_2_position["z"]) g.target.pose.position.z = node_2_position["z"].as<double>();
|
||||
}
|
||||
if (auto node_2_orientation = node_1_pose["orientation"]) {
|
||||
if (node_2_orientation["x"]) g.target.pose.orientation.x = node_2_orientation["x"].as<double>();
|
||||
if (node_2_orientation["y"]) g.target.pose.orientation.y = node_2_orientation["y"].as<double>();
|
||||
if (node_2_orientation["z"]) g.target.pose.orientation.z = node_2_orientation["z"].as<double>();
|
||||
if (node_2_orientation["w"]) g.target.pose.orientation.w = node_2_orientation["w"].as<double>();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// ExecuteBtAction::Goal: epoch, action_name, calls
|
||||
template<>
|
||||
inline interfaces::action::ExecuteBtAction::Goal from_yaml<interfaces::action::ExecuteBtAction::Goal>(const YAML::Node & n)
|
||||
@@ -181,31 +315,17 @@ inline interfaces::action::ExecuteBtAction::Goal from_yaml<interfaces::action::E
|
||||
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>();
|
||||
if (n["calls"] && n["calls"].IsSequence()) {
|
||||
for (const auto & item : n["calls"]) {
|
||||
interfaces::msg::SkillCall tmp;
|
||||
if (item["name"]) tmp.name = item["name"].as<std::string>();
|
||||
if (item["interface_type"]) tmp.interface_type = item["interface_type"].as<std::string>();
|
||||
if (item["call_kind"]) tmp.call_kind = item["call_kind"].as<std::string>();
|
||||
if (item["topic"]) tmp.topic = item["topic"].as<std::string>();
|
||||
if (item["payload_yaml"]) tmp.payload_yaml = item["payload_yaml"].as<std::string>();
|
||||
if (item["instance_name"]) tmp.instance_name = item["instance_name"].as<std::string>();
|
||||
if (item["timeout_ms"]) tmp.timeout_ms = item["timeout_ms"].as<int32_t>();
|
||||
g.calls.push_back(tmp);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -223,66 +343,14 @@ inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action:
|
||||
return g;
|
||||
}
|
||||
|
||||
// MoveToPosition::Goal: target_x, target_y
|
||||
// VADEvent::Request: command, timeout_ms
|
||||
template<>
|
||||
inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
|
||||
inline interfaces::srv::VADEvent::Request from_yaml<interfaces::srv::VADEvent::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::MoveToPosition::Goal g;
|
||||
interfaces::srv::VADEvent::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["target_x"]) g.target_x = n["target_x"].as<float>();
|
||||
if (n["target_y"]) g.target_y = n["target_y"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// SlamMode::Goal: enable_mapping
|
||||
template<>
|
||||
inline interfaces::action::SlamMode::Goal from_yaml<interfaces::action::SlamMode::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::SlamMode::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["enable_mapping"]) g.enable_mapping = n["enable_mapping"].as<bool>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// ArmSpaceControl::Goal: target
|
||||
template<>
|
||||
inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::ArmSpaceControl::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::ArmSpaceControl::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
// geometry_msgs::msg::PoseStamped
|
||||
if (n) {
|
||||
if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
|
||||
if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
|
||||
if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
|
||||
if (auto p = n["position"]) {
|
||||
if (p["x"]) g.target.pose.position.x = p["x"].as<double>();
|
||||
if (p["y"]) g.target.pose.position.y = p["y"].as<double>();
|
||||
if (p["z"]) g.target.pose.position.z = p["z"].as<double>();
|
||||
}
|
||||
if (auto q = n["orientation"]) {
|
||||
if (q["x"]) g.target.pose.orientation.x = q["x"].as<double>();
|
||||
if (q["y"]) g.target.pose.orientation.y = q["y"].as<double>();
|
||||
if (q["z"]) g.target.pose.orientation.z = q["z"].as<double>();
|
||||
if (q["w"]) g.target.pose.orientation.w = q["w"].as<double>();
|
||||
}
|
||||
}
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// GripperCmd::Request: devid, loc, speed, torque
|
||||
template<>
|
||||
inline interfaces::srv::GripperCmd::Request from_yaml<interfaces::srv::GripperCmd::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::GripperCmd::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["devid"]) r.devid = n["devid"].as<uint8_t>();
|
||||
if (n["loc"]) r.loc = n["loc"].as<uint8_t>();
|
||||
if (n["speed"]) r.speed = n["speed"].as<uint8_t>();
|
||||
if (n["torque"]) r.torque = n["torque"].as<uint8_t>();
|
||||
if (n["command"]) r.command = n["command"].as<std::string>();
|
||||
if (n["timeout_ms"]) r.timeout_ms = n["timeout_ms"].as<int32_t>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
@@ -303,6 +371,65 @@ inline interfaces::srv::MotorInfo::Request from_yaml<interfaces::srv::MotorInfo:
|
||||
return r;
|
||||
}
|
||||
|
||||
// BtRebuild::Request: type, config, param
|
||||
template<>
|
||||
inline interfaces::srv::BtRebuild::Request from_yaml<interfaces::srv::BtRebuild::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::BtRebuild::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["type"]) r.type = n["type"].as<std::string>();
|
||||
if (n["config"]) r.config = n["config"].as<std::string>();
|
||||
if (n["param"]) r.param = n["param"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// MapLoad::Request: map_url
|
||||
template<>
|
||||
inline interfaces::srv::MapLoad::Request from_yaml<interfaces::srv::MapLoad::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::MapLoad::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// AudioData::Request: command, duration_ms
|
||||
template<>
|
||||
inline interfaces::srv::AudioData::Request from_yaml<interfaces::srv::AudioData::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::AudioData::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["command"]) r.command = n["command"].as<std::string>();
|
||||
if (n["duration_ms"]) r.duration_ms = n["duration_ms"].as<int32_t>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// ClearArmError::Request: arm_id, joint_num
|
||||
template<>
|
||||
inline interfaces::srv::ClearArmError::Request from_yaml<interfaces::srv::ClearArmError::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::ClearArmError::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["arm_id"]) r.arm_id = n["arm_id"].as<int8_t>();
|
||||
if (n["joint_num"]) r.joint_num = n["joint_num"].as<int8_t>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// ASRRecognize::Request: command
|
||||
template<>
|
||||
inline interfaces::srv::ASRRecognize::Request from_yaml<interfaces::srv::ASRRecognize::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::ASRRecognize::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["command"]) r.command = n["command"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// MotorParam::Request: motor_id, max_speed, add_acc, dec_acc
|
||||
template<>
|
||||
inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorParam::Request>(const YAML::Node & n)
|
||||
@@ -317,15 +444,64 @@ inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorPara
|
||||
return r;
|
||||
}
|
||||
|
||||
// BtRebuild::Request: type, config, param
|
||||
// GripperCmd::Request: devid, loc, speed, torque
|
||||
template<>
|
||||
inline interfaces::srv::BtRebuild::Request from_yaml<interfaces::srv::BtRebuild::Request>(const YAML::Node & n)
|
||||
inline interfaces::srv::GripperCmd::Request from_yaml<interfaces::srv::GripperCmd::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::BtRebuild::Request r;
|
||||
interfaces::srv::GripperCmd::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>();
|
||||
if (n["devid"]) r.devid = n["devid"].as<uint8_t>();
|
||||
if (n["loc"]) r.loc = n["loc"].as<uint8_t>();
|
||||
if (n["speed"]) r.speed = n["speed"].as<uint8_t>();
|
||||
if (n["torque"]) r.torque = n["torque"].as<uint8_t>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// MapSave::Request: map_url
|
||||
template<>
|
||||
inline interfaces::srv::MapSave::Request from_yaml<interfaces::srv::MapSave::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::MapSave::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// TTSSynthesize::Request: command, text, voice
|
||||
template<>
|
||||
inline interfaces::srv::TTSSynthesize::Request from_yaml<interfaces::srv::TTSSynthesize::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::TTSSynthesize::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["command"]) r.command = n["command"].as<std::string>();
|
||||
if (n["text"]) r.text = n["text"].as<std::string>();
|
||||
if (n["voice"]) r.voice = n["voice"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// InverseKinematics::Request: arm_id, pose
|
||||
template<>
|
||||
inline interfaces::srv::InverseKinematics::Request from_yaml<interfaces::srv::InverseKinematics::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::InverseKinematics::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["arm_id"]) r.arm_id = n["arm_id"].as<int32_t>();
|
||||
if (auto node_0_pose = n["pose"]) {
|
||||
if (auto node_1_position = node_0_pose["position"]) {
|
||||
if (node_1_position["x"]) r.pose.position.x = node_1_position["x"].as<double>();
|
||||
if (node_1_position["y"]) r.pose.position.y = node_1_position["y"].as<double>();
|
||||
if (node_1_position["z"]) r.pose.position.z = node_1_position["z"].as<double>();
|
||||
}
|
||||
if (auto node_1_orientation = node_0_pose["orientation"]) {
|
||||
if (node_1_orientation["x"]) r.pose.orientation.x = node_1_orientation["x"].as<double>();
|
||||
if (node_1_orientation["y"]) r.pose.orientation.y = node_1_orientation["y"].as<double>();
|
||||
if (node_1_orientation["z"]) r.pose.orientation.z = node_1_orientation["z"].as<double>();
|
||||
if (node_1_orientation["w"]) r.pose.orientation.w = node_1_orientation["w"].as<double>();
|
||||
}
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
@@ -5,15 +5,33 @@
|
||||
#include <string>
|
||||
|
||||
#include "interfaces/action/arm.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
#include "interfaces/action/camera_take_photo.hpp"
|
||||
#include "interfaces/action/dual_arm.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/gripper_cmd.hpp"
|
||||
#include "interfaces/action/hand_control.hpp"
|
||||
#include "interfaces/action/leg_control.hpp"
|
||||
#include "interfaces/action/move_home.hpp"
|
||||
#include "interfaces/action/move_leg.hpp"
|
||||
#include "interfaces/action/move_to_position.hpp"
|
||||
#include "interfaces/action/move_waist.hpp"
|
||||
#include "interfaces/action/move_wheel.hpp"
|
||||
#include "interfaces/action/slam_mode.hpp"
|
||||
#include "interfaces/action/vision_grasp_object.hpp"
|
||||
#include "interfaces/action/vision_object_recognition.hpp"
|
||||
#include "interfaces/srv/asr_recognize.hpp"
|
||||
#include "interfaces/srv/audio_data.hpp"
|
||||
#include "interfaces/srv/bt_rebuild.hpp"
|
||||
#include "interfaces/srv/clear_arm_error.hpp"
|
||||
#include "interfaces/srv/gripper_cmd.hpp"
|
||||
#include "interfaces/srv/inverse_kinematics.hpp"
|
||||
#include "interfaces/srv/map_load.hpp"
|
||||
#include "interfaces/srv/map_save.hpp"
|
||||
#include "interfaces/srv/motor_info.hpp"
|
||||
#include "interfaces/srv/motor_param.hpp"
|
||||
#include "interfaces/srv/tts_synthesize.hpp"
|
||||
#include "interfaces/srv/vad_event.hpp"
|
||||
#include "interfaces/srv/vision_object_recognition.hpp"
|
||||
|
||||
namespace brain {
|
||||
@@ -25,31 +43,67 @@ template<typename ServiceT>
|
||||
struct SkillServiceTrait;
|
||||
|
||||
using SkillActionTypes = std::tuple<
|
||||
interfaces::action::VisionObjectRecognition,
|
||||
interfaces::action::Arm,
|
||||
interfaces::action::DualArm,
|
||||
interfaces::action::CameraTakePhoto,
|
||||
interfaces::action::HandControl,
|
||||
interfaces::action::MoveWaist,
|
||||
interfaces::action::MoveLeg,
|
||||
interfaces::action::MoveWheel,
|
||||
interfaces::action::MoveHome,
|
||||
interfaces::action::GripperCmd
|
||||
interfaces::action::GripperCmd,
|
||||
interfaces::action::ArmSpaceControl,
|
||||
interfaces::action::ExecuteBtAction,
|
||||
interfaces::action::HandControl,
|
||||
interfaces::action::LegControl,
|
||||
interfaces::action::MoveToPosition,
|
||||
interfaces::action::SlamMode,
|
||||
interfaces::action::VisionGraspObject
|
||||
>;
|
||||
|
||||
using SkillServiceTypes = std::tuple<
|
||||
interfaces::srv::VisionObjectRecognition,
|
||||
interfaces::srv::ASRRecognize,
|
||||
interfaces::srv::AudioData,
|
||||
interfaces::srv::BtRebuild,
|
||||
interfaces::srv::ClearArmError,
|
||||
interfaces::srv::GripperCmd,
|
||||
interfaces::srv::InverseKinematics,
|
||||
interfaces::srv::MapLoad,
|
||||
interfaces::srv::MapSave
|
||||
interfaces::srv::MapSave,
|
||||
interfaces::srv::MotorInfo,
|
||||
interfaces::srv::MotorParam,
|
||||
interfaces::srv::TTSSynthesize,
|
||||
interfaces::srv::VADEvent
|
||||
>;
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::VisionObjectRecognition>
|
||||
{
|
||||
static constexpr const char * skill_name = "VisionObjectRecognition";
|
||||
static constexpr const char * default_topic = "/vision_object_recognition";
|
||||
static bool success(const interfaces::action::VisionObjectRecognition::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::VisionObjectRecognition::Result & r) {return r.info;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::Arm>
|
||||
{
|
||||
static constexpr const char * skill_name = "Arm";
|
||||
static constexpr const char * default_topic = "ArmAction";
|
||||
static constexpr const char * default_topic = "Arm";
|
||||
static bool success(const interfaces::action::Arm::Result & r, const std::string & instance_name) {(void)instance_name; return (r.result == 0);}
|
||||
static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::DualArm>
|
||||
{
|
||||
static constexpr const char * skill_name = "DualArm";
|
||||
static constexpr const char * default_topic = "DualArm";
|
||||
static bool success(const interfaces::action::DualArm::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::DualArm::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::CameraTakePhoto>
|
||||
{
|
||||
@@ -59,15 +113,6 @@ struct SkillActionTrait<interfaces::action::CameraTakePhoto>
|
||||
static std::string message(const interfaces::action::CameraTakePhoto::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::HandControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "HandControl";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::HandControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::MoveWaist>
|
||||
{
|
||||
@@ -128,10 +173,115 @@ struct SkillActionTrait<interfaces::action::GripperCmd>
|
||||
static std::string message(const interfaces::action::GripperCmd::Result & r) {return r.state;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::ArmSpaceControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "ArmSpaceControl";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::ArmSpaceControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::ArmSpaceControl::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::ExecuteBtAction>
|
||||
{
|
||||
static constexpr const char * skill_name = "ExecuteBtAction";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::ExecuteBtAction::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::ExecuteBtAction::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::HandControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "HandControl";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::HandControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::LegControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "LegControl";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::LegControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::LegControl::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::MoveToPosition>
|
||||
{
|
||||
static constexpr const char * skill_name = "MoveToPosition";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::MoveToPosition::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::MoveToPosition::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::SlamMode>
|
||||
{
|
||||
static constexpr const char * skill_name = "SlamMode";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::SlamMode::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::SlamMode::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::VisionGraspObject>
|
||||
{
|
||||
static constexpr const char * skill_name = "VisionGraspObject";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::VisionGraspObject::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::VisionGraspObject::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::VisionObjectRecognition>
|
||||
{
|
||||
static constexpr const char * skill_name = "VisionObjectRecognition";
|
||||
static constexpr const char * default_topic = "/vision_object_recognition";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::ASRRecognize>
|
||||
{
|
||||
static constexpr const char * skill_name = "ASRRecognize";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::AudioData>
|
||||
{
|
||||
static constexpr const char * skill_name = "AudioData";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::BtRebuild>
|
||||
{
|
||||
static constexpr const char * skill_name = "BtRebuild";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::ClearArmError>
|
||||
{
|
||||
static constexpr const char * skill_name = "ClearArmError";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::GripperCmd>
|
||||
{
|
||||
static constexpr const char * skill_name = "GripperCmd";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::InverseKinematics>
|
||||
{
|
||||
static constexpr const char * skill_name = "InverseKinematics";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
@@ -149,4 +299,32 @@ struct SkillServiceTrait<interfaces::srv::MapSave>
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::MotorInfo>
|
||||
{
|
||||
static constexpr const char * skill_name = "MotorInfo";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::MotorParam>
|
||||
{
|
||||
static constexpr const char * skill_name = "MotorParam";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::TTSSynthesize>
|
||||
{
|
||||
static constexpr const char * skill_name = "TTSSynthesize";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::VADEvent>
|
||||
{
|
||||
static constexpr const char * skill_name = "VADEvent";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
} // namespace brain
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
@@ -296,7 +297,7 @@ private:
|
||||
|
||||
if (it != action_override_registrars_.end()) {
|
||||
it->second(topic, internal_skill);
|
||||
RCLCPP_INFO(node_->get_logger(), "Registered action client '%s' via Hook on '%s'",
|
||||
LOG_INFO("Registered action client '{}' via Hook on '{}'",
|
||||
internal_skill.c_str(), it->first.c_str());
|
||||
return;
|
||||
}
|
||||
@@ -332,9 +333,9 @@ private:
|
||||
result_cb = [this, internal_skill](const typename GoalHandle::WrappedResult & res) {
|
||||
if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
|
||||
SkillActionTrait<ActionT>::success(*res.result, "")) {
|
||||
RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
|
||||
LOG_INFO("action succeeded: {}", internal_skill.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(node_->get_logger(), "action cancelled: %s", internal_skill.c_str());
|
||||
LOG_WARN("action cancelled: {}", internal_skill.c_str());
|
||||
} else {
|
||||
std::string detail;
|
||||
if (res.result) {
|
||||
@@ -342,8 +343,7 @@ private:
|
||||
} else {
|
||||
detail = "result unavailable";
|
||||
}
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(), "action failed: %s (%s)", internal_skill.c_str(), detail.c_str());
|
||||
LOG_ERROR("action failed: {} ({})", internal_skill.c_str(), detail.c_str());
|
||||
}
|
||||
};
|
||||
}
|
||||
@@ -372,14 +372,14 @@ private:
|
||||
template<size_t I = 0, typename TupleT, typename RegistryT>
|
||||
bool dispatch_skill_action(
|
||||
const std::string & skill, const std::string & topic, RegistryT * reg,
|
||||
rclcpp::Logger logger, std::chrono::nanoseconds timeout, const std::string & instance_name, std::string & detail)
|
||||
std::chrono::nanoseconds timeout, const std::string & instance_name, std::string & detail)
|
||||
{
|
||||
if constexpr (I == std::tuple_size<TupleT>::value) {
|
||||
return false;
|
||||
} else {
|
||||
using ActionT = std::tuple_element_t<I, TupleT>;
|
||||
if (skill == SkillActionTrait<ActionT>::skill_name) {
|
||||
auto opt = reg->template send_and_wait<ActionT>(topic, logger, timeout);
|
||||
auto opt = reg->template send_and_wait<ActionT>(topic, timeout);
|
||||
if (opt && opt->code == rclcpp_action::ResultCode::SUCCEEDED && opt->result) {
|
||||
bool ok = SkillActionTrait<ActionT>::success(*opt->result, instance_name);
|
||||
detail = ok ? SkillActionTrait<ActionT>::message(*opt->result) : std::string(
|
||||
@@ -389,7 +389,7 @@ bool dispatch_skill_action(
|
||||
detail = "action failed";
|
||||
return false;
|
||||
}
|
||||
return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, instance_name, detail);
|
||||
return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, timeout, instance_name, detail);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -3,6 +3,7 @@
|
||||
#include <smacc2/smacc.hpp>
|
||||
#include <boost/mpl/list.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include <atomic>
|
||||
#include <string>
|
||||
@@ -49,7 +50,7 @@ struct CerebellumData
|
||||
* @brief Bound by the action execute callback: performs the whole skill
|
||||
* sequence synchronously when invoked inside StExecuting supervision thread.
|
||||
*/
|
||||
std::function<ExecResult(rclcpp::Logger)> execute_fn; // installed by action execute
|
||||
std::function<ExecResult()> execute_fn; // installed by action execute
|
||||
/**
|
||||
* @brief Completion callback invoked by terminal states (Completed/Failed/Emergency)
|
||||
* to produce the actual action result (succeed/abort) back to the client.
|
||||
@@ -117,7 +118,7 @@ struct StIdle : smacc2::SmaccState<StIdle, SmCerebellum>
|
||||
*/
|
||||
sc::result react(const EvGoalReceived &)
|
||||
{
|
||||
RCLCPP_INFO(this->getLogger(), "[SM] Transition Idle -> Executing (goal received)");
|
||||
LOG_INFO("[SM] Transition Idle -> Executing (goal received)");
|
||||
return transit<StExecuting>();
|
||||
}
|
||||
};
|
||||
@@ -140,7 +141,7 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
|
||||
/** @brief State entry: spawns supervision thread and invokes execute_fn when ready. */
|
||||
void onEntry()
|
||||
{
|
||||
RCLCPP_INFO(this->getLogger(), "[SM] Enter Executing");
|
||||
LOG_INFO("[SM] Enter Executing");
|
||||
// Supervising thread waits until execute_fn is set, then runs it.
|
||||
std::thread{[this]() {
|
||||
auto & d = runtime();
|
||||
@@ -150,12 +151,12 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
|
||||
std::this_thread::sleep_for(5ms);
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
if (now - last_warn > 10s) {
|
||||
RCLCPP_WARN(this->getLogger(), "[SM] Still waiting for execute_fn to be set...");
|
||||
LOG_WARN("[SM] Still waiting for execute_fn to be set...");
|
||||
last_warn = now;
|
||||
}
|
||||
}
|
||||
try {
|
||||
auto res = d.execute_fn(this->getLogger());
|
||||
auto res = d.execute_fn();
|
||||
d.preempt.store(!res.success, std::memory_order_release);
|
||||
d.preempt_reason = res.message;
|
||||
d.last_success.store(res.success, std::memory_order_release);
|
||||
@@ -180,19 +181,19 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
|
||||
* @return Transition to Completed state.
|
||||
*/
|
||||
sc::result react(const EvExecutionFinished &)
|
||||
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Executing -> Completed"); return transit<StCompleted>(); }
|
||||
{ LOG_INFO("[SM] Transition Executing -> Completed"); return transit<StCompleted>(); }
|
||||
/**
|
||||
* @brief Reaction: functional failure -> StFailed.
|
||||
* @return Transition to Failed state.
|
||||
*/
|
||||
sc::result react(const EvExecutionFailed &)
|
||||
{ RCLCPP_WARN(this->getLogger(), "[SM] Transition Executing -> Failed"); return transit<StFailed>(); }
|
||||
{ LOG_WARN("[SM] Transition Executing -> Failed"); return transit<StFailed>(); }
|
||||
/**
|
||||
* @brief Reaction: exception / emergency -> StEmergency.
|
||||
* @return Transition to Emergency state.
|
||||
*/
|
||||
sc::result react(const EvEmergency &)
|
||||
{ RCLCPP_ERROR(this->getLogger(), "[SM] Transition Executing -> Emergency"); return transit<StEmergency>(); }
|
||||
{ LOG_ERROR("[SM] Transition Executing -> Emergency"); return transit<StEmergency>(); }
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
@@ -223,7 +224,7 @@ struct StCompleted : smacc2::SmaccState<StCompleted, SmCerebellum>
|
||||
* @return Transition to StExecuting.
|
||||
*/
|
||||
sc::result react(const EvGoalReceived &)
|
||||
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Completed -> Executing (new goal)"); return transit<StExecuting>(); }
|
||||
{ LOG_INFO("[SM] Transition Completed -> Executing (new goal)"); return transit<StExecuting>(); }
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
@@ -249,7 +250,7 @@ struct StFailed : smacc2::SmaccState<StFailed, SmCerebellum>
|
||||
* @return Transition to StExecuting.
|
||||
*/
|
||||
sc::result react(const EvGoalReceived &)
|
||||
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Failed -> Executing (new goal)"); return transit<StExecuting>(); }
|
||||
{ LOG_INFO("[SM] Transition Failed -> Executing (new goal)"); return transit<StExecuting>(); }
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
@@ -268,7 +269,7 @@ struct StEmergency : smacc2::SmaccState<StEmergency, SmCerebellum>
|
||||
{
|
||||
auto & d = runtime();
|
||||
if (d.complete_cb) d.complete_cb(false, std::string("EMERGENCY: ")+ d.preempt_reason);
|
||||
RCLCPP_ERROR(this->getLogger(), "[SM] Emergency: %s", d.preempt_reason.c_str());
|
||||
LOG_ERROR("[SM] Emergency: {}", d.preempt_reason.c_str());
|
||||
d.execute_fn = nullptr; d.complete_cb = nullptr; d.preempt.store(false);
|
||||
}
|
||||
/**
|
||||
@@ -276,7 +277,7 @@ struct StEmergency : smacc2::SmaccState<StEmergency, SmCerebellum>
|
||||
* @return Transition to StExecuting.
|
||||
*/
|
||||
sc::result react(const EvGoalReceived &)
|
||||
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Emergency -> Executing (new goal)"); return transit<StExecuting>(); }
|
||||
{ LOG_INFO("[SM] Transition Emergency -> Executing (new goal)"); return transit<StExecuting>(); }
|
||||
};
|
||||
|
||||
} // namespace brain
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/logger.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
|
||||
#include "../include/brain/cerebellum_node.hpp"
|
||||
#include "../include/brain/cerebrum_node.hpp"
|
||||
@@ -22,8 +22,20 @@ int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
rclcpp::Logger logger = rclcpp::get_logger("brain_node");
|
||||
RCLCPP_INFO(logger, "Starting brain node...");
|
||||
hivecore::log::LoggerOptions log_opts;
|
||||
log_opts.default_level = hivecore::log::Level::INFO; // 默认日志等级
|
||||
log_opts.log_dir = "/var/log/robot"; // 主日志目录
|
||||
log_opts.fallback_log_dir = "/tmp/robot_logs"; // 备用目录（主目录不可写时使用）
|
||||
log_opts.max_file_size_mb = 50; // 单个日志文件最大 50 MB
|
||||
log_opts.max_files = 10; // 最多保留 10 个轮转文件
|
||||
log_opts.queue_size = 8192; // 异步队列容量
|
||||
log_opts.worker_threads = 1; // 后台写日志线程数
|
||||
log_opts.enable_console = true; // 同时输出到控制台
|
||||
log_opts.enable_level_sync = true; // 允许运行时动态调整日志等级
|
||||
log_opts.level_sync_interval_ms = 100; // 动态等级同步检查间隔 (ms)
|
||||
hivecore::log::Logger::init("brain_node", log_opts);
|
||||
|
||||
LOG_INFO("Starting brain node...");
|
||||
|
||||
// Create node options with parameter overrides if provided
|
||||
rclcpp::NodeOptions options;
|
||||
@@ -39,6 +51,7 @@ int main(int argc, char ** argv)
|
||||
|
||||
executor->spin();
|
||||
|
||||
hivecore::log::Logger::shutdown();
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
#include <utility>
|
||||
#include <string.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/logger.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
|
||||
namespace brain {
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -25,8 +25,8 @@
|
||||
|
||||
// Third-party / external libraries (ROS2, ament).
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include <rclcpp/logger.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
// Project headers.
|
||||
@@ -52,7 +52,7 @@
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include "arm_action_define.h"
|
||||
// #include "arm_action_define.h"
|
||||
|
||||
namespace brain
|
||||
{
|
||||
@@ -73,7 +73,7 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
|
||||
: Node("cerebellum_node", options),
|
||||
share_directory_(ament_index_cpp::get_package_share_directory("brain"))
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Cerebellum node started");
|
||||
LOG_INFO("Cerebellum node started");
|
||||
|
||||
ik_client_ = this->create_client<interfaces::srv::InverseKinematics>("inverse_kinematics");
|
||||
|
||||
@@ -85,7 +85,7 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
|
||||
robot_config_ = std::make_unique<brain::robot_config::RobotConfig>(config_path);
|
||||
auto skill_file = robot_config_->SkillFile("brain");
|
||||
if (skill_file == std::nullopt) {
|
||||
RCLCPP_ERROR(this->get_logger(), "No skill_file entry found for 'brain' in robot_config.yaml");
|
||||
LOG_ERROR("No skill_file entry found for 'brain' in robot_config.yaml");
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -94,7 +94,7 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
|
||||
? share_directory_ + *skill_file
|
||||
: robot_skill_file_path_;
|
||||
|
||||
RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
|
||||
LOG_WARN("skill file {}", robot_skill_file_path_.c_str());
|
||||
|
||||
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
|
||||
tf_buffer_->setUsingDedicatedThread(true);
|
||||
@@ -119,11 +119,10 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
|
||||
#ifndef BRAIN_DISABLE_SM
|
||||
if (!sm_exec_) {
|
||||
sm_exec_.reset(smacc2::run_async<brain::SmCerebellum>());
|
||||
RCLCPP_INFO(this->get_logger(), "SMACC2 SmCerebellum started");
|
||||
LOG_INFO("SMACC2 SmCerebellum started");
|
||||
}
|
||||
#else
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "SMACC2 disabled via BRAIN_DISABLE_SM for lightweight test context");
|
||||
LOG_INFO("SMACC2 disabled via BRAIN_DISABLE_SM for lightweight test context");
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -199,6 +198,7 @@ void CerebellumNode::DeclareAndLoadParameters()
|
||||
this->declare_parameter<int>("arm_cmd_type_take_object", arm_cmd_type_take_object_);
|
||||
this->declare_parameter<int>("arm_cmd_type_custom_min", arm_cmd_type_custom_min_);
|
||||
this->declare_parameter<int>("arm_cmd_type_custom_max", arm_cmd_type_custom_max_);
|
||||
this->declare_parameter<std::string>("dual_arm_motion_type", dual_arm_motion_type_);
|
||||
|
||||
// Read back
|
||||
this->get_parameter("robot_config_path", robot_config_path_);
|
||||
@@ -260,12 +260,19 @@ void CerebellumNode::DeclareAndLoadParameters()
|
||||
this->get_parameter("arm_cmd_type_take_object", arm_cmd_type_take_object_);
|
||||
this->get_parameter("arm_cmd_type_custom_min", arm_cmd_type_custom_min_);
|
||||
this->get_parameter("arm_cmd_type_custom_max", arm_cmd_type_custom_max_);
|
||||
this->get_parameter("dual_arm_motion_type", dual_arm_motion_type_);
|
||||
|
||||
arm_transform_timeout_sec_ = std::max(0.01, arm_transform_timeout_sec_);
|
||||
map_save_free_thresh_ = std::clamp(map_save_free_thresh_, 0.0, 1.0);
|
||||
map_save_occupied_thresh_ = std::clamp(map_save_occupied_thresh_, 0.0, 1.0);
|
||||
nav_goal_distance_tolerance_ = std::max(0.0, nav_goal_distance_tolerance_);
|
||||
|
||||
if (dual_arm_motion_type_ != "MOVEJ" && dual_arm_motion_type_ != "MOVEL") {
|
||||
LOG_WARN("Invalid dual_arm_motion_type '{}', defaulting to MOVEJ",
|
||||
dual_arm_motion_type_.c_str());
|
||||
dual_arm_motion_type_ = "MOVEJ";
|
||||
}
|
||||
|
||||
// Parse skill_timeouts param into map
|
||||
std::string raw;
|
||||
this->get_parameter("skill_timeouts", raw);
|
||||
@@ -280,8 +287,7 @@ void CerebellumNode::DeclareAndLoadParameters()
|
||||
try {
|
||||
skill_timeouts_[key] = std::stod(vstr);
|
||||
} catch (...) {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(), "Bad timeout value for %s: %s", key.c_str(),
|
||||
LOG_WARN("Bad timeout value for {}: {}", key.c_str(),
|
||||
vstr.c_str());
|
||||
}
|
||||
}
|
||||
@@ -296,13 +302,11 @@ void CerebellumNode::DeclareAndLoadParameters()
|
||||
}
|
||||
flush();
|
||||
if (!skill_timeouts_.empty()) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "[cerebellum_node] Loaded %zu skill-specific timeouts",
|
||||
LOG_INFO("[cerebellum_node] Loaded {} skill-specific timeouts",
|
||||
skill_timeouts_.size());
|
||||
}
|
||||
}
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "[cerebellum_node] abort_on_failure=%s default_action_timeout=%.1f vision_grasp_object_timeout=%.1f",
|
||||
LOG_INFO("[cerebellum_node] abort_on_failure={} default_action_timeout={:.1f} vision_grasp_object_timeout={:.1f}",
|
||||
abort_on_failure_ ? "true" : "false", default_action_timeout_sec_,
|
||||
vision_grasp_object_timeout_sec_);
|
||||
}
|
||||
@@ -317,17 +321,15 @@ void CerebellumNode::DeclareAndLoadParameters()
|
||||
void CerebellumNode::LoadSkillsFile()
|
||||
{
|
||||
if (robot_skill_file_path_.empty()) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "[cerebellum_node] No skill file path provided, skipping skill load");
|
||||
LOG_ERROR("[cerebellum_node] No skill file path provided, skipping skill load");
|
||||
return;
|
||||
}
|
||||
if (!skill_manager_->load_from_file(robot_skill_file_path_)) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "[cerebellum_node] Failed to load skills from %s", robot_skill_file_path_.c_str());
|
||||
LOG_ERROR("[cerebellum_node] Failed to load skills from {}", robot_skill_file_path_.c_str());
|
||||
return;
|
||||
}
|
||||
for (const auto & kv : skill_manager_->skills()) {
|
||||
RCLCPP_INFO(this->get_logger(), "[cerebellum_node] Loaded skill '%s'", kv.first.c_str());
|
||||
LOG_INFO("[cerebellum_node] Loaded skill '{}'", kv.first.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -364,30 +366,29 @@ void CerebellumNode::SetupExecuteBtServer()
|
||||
if (!goal) {return rclcpp_action::GoalResponse::REJECT;}
|
||||
#ifdef ENABLE_EPOCH_CHECK
|
||||
if (goal->epoch < current_epoch_) {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(), "Reject stale ExecuteBtAction goal epoch=%u < current=%u", goal->epoch,
|
||||
LOG_WARN("Reject stale ExecuteBtAction goal epoch={} < current={}", goal->epoch,
|
||||
current_epoch_.load());
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
#endif
|
||||
if (goal->epoch > current_epoch_) {
|
||||
current_epoch_ = goal->epoch;
|
||||
RCLCPP_INFO(this->get_logger(), "Switch to new epoch %u", current_epoch_.load());
|
||||
LOG_INFO("Switch to new epoch {}", current_epoch_.load());
|
||||
}
|
||||
if (sm_exec_ && sm_exec_->signalDetector) {
|
||||
sm_exec_->signalDetector->postEvent(new brain::EvGoalReceived());
|
||||
}
|
||||
// Log detailed goal contents to help debugging call propagation from Cerebrum
|
||||
RCLCPP_INFO(this->get_logger(), "ExecuteBtAction GoalReceived epoch=%u action_name=%s calls_count=%zu",
|
||||
LOG_INFO("ExecuteBtAction GoalReceived epoch={} action_name={} calls_count={}",
|
||||
goal->epoch, goal->action_name.c_str(), goal->calls.size());
|
||||
for (size_t i = 0; i < goal->calls.size(); ++i) {
|
||||
const auto & c = goal->calls[i];
|
||||
RCLCPP_INFO(this->get_logger(), "call[%zu]: name=%s instance=%s interface=%s kind=%s topic=%s timeout_ms=%u payload_len=%zu",
|
||||
LOG_INFO("call[{}]: name={} instance={} interface={} kind={} topic={} timeout_ms={} payload_len={}",
|
||||
i, c.name.c_str(), c.instance_name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.timeout_ms, c.payload_yaml.size());
|
||||
if (!c.payload_yaml.empty()) {
|
||||
// Log payload content (trim if very long)
|
||||
const std::string payload = c.payload_yaml.size() > 512 ? c.payload_yaml.substr(0, 512) + "..." : c.payload_yaml;
|
||||
// RCLCPP_INFO(this->get_logger(), "payload:{%s}", payload.c_str());
|
||||
// LOG_INFO("payload:{{}}", payload.c_str());
|
||||
}
|
||||
}
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
@@ -400,7 +401,7 @@ void CerebellumNode::SetupExecuteBtServer()
|
||||
if (it != goal_ctxs_.end() && it->second) {
|
||||
it->second->cancel.store(true, std::memory_order_release);
|
||||
}
|
||||
RCLCPP_WARN(this->get_logger(), "[ExecuteBtAction] cancellation requested (epoch=%u)", gh->get_goal()->epoch);
|
||||
LOG_WARN("[ExecuteBtAction] cancellation requested (epoch={})", gh->get_goal()->epoch);
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
};
|
||||
// Each goal executes in its own detached thread (provided by ActionServerRegistry)
|
||||
@@ -418,15 +419,15 @@ bool CerebellumNode::TransformPoseToArmFrame(
|
||||
geometry_msgs::msg::PoseStamped & transformed) const
|
||||
{
|
||||
if (!tf_buffer_) {
|
||||
RCLCPP_ERROR(this->get_logger(), "TF buffer not initialized, unable to transform target pose");
|
||||
LOG_ERROR("TF buffer not initialized, unable to transform target pose");
|
||||
return false;
|
||||
}
|
||||
if (source.header.frame_id.empty()) {
|
||||
RCLCPP_WARN(this->get_logger(), "Target pose missing source frame, skipping transformation");
|
||||
LOG_WARN("Target pose missing source frame, skipping transformation");
|
||||
return false;
|
||||
}
|
||||
if (source.header.frame_id == arm_target_frame) {
|
||||
RCLCPP_INFO(this->get_logger(), "Target pose already in %s frame, skipping transformation", arm_target_frame.c_str());
|
||||
LOG_INFO("Target pose already in {} frame, skipping transformation", arm_target_frame.c_str());
|
||||
transformed = source;
|
||||
return true;
|
||||
}
|
||||
@@ -436,28 +437,24 @@ bool CerebellumNode::TransformPoseToArmFrame(
|
||||
|
||||
// ===================
|
||||
#ifdef TF_DEBUG_LOGGING
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"TF Debug: Source [%s] pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f) t=%.4f",
|
||||
LOG_INFO("TF Debug: Source [{}] pos({:.4f}, {:.4f}, {:.4f}) quat({:.4f}, {:.4f}, {:.4f}, {:.4f}) t={:.4f}",
|
||||
source.header.frame_id.c_str(),
|
||||
source.pose.position.x, source.pose.position.y, source.pose.position.z,
|
||||
source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w,
|
||||
rclcpp::Time(source.header.stamp).seconds());
|
||||
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"TF Debug: Result [%s] pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f)",
|
||||
LOG_INFO("TF Debug: Result [{}] pos({:.4f}, {:.4f}, {:.4f}) quat({:.4f}, {:.4f}, {:.4f}, {:.4f})",
|
||||
transformed.header.frame_id.c_str(),
|
||||
transformed.pose.position.x, transformed.pose.position.y, transformed.pose.position.z,
|
||||
transformed.pose.orientation.x, transformed.pose.orientation.y, transformed.pose.orientation.z, transformed.pose.orientation.w);
|
||||
#endif
|
||||
// ===================
|
||||
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "Target pose transformed from %s to %s",
|
||||
LOG_INFO("Target pose transformed from {} to {}",
|
||||
source.header.frame_id.c_str(), arm_target_frame.c_str());
|
||||
return true;
|
||||
} catch (const tf2::TransformException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "Failed to transform target pose: %s -> %s, reason: %s",
|
||||
LOG_ERROR("Failed to transform target pose: {} -> {}, reason: {}",
|
||||
source.header.frame_id.c_str(), arm_target_frame.c_str(), ex.what());
|
||||
}
|
||||
return false;
|
||||
@@ -698,7 +695,7 @@ bool CerebellumNode::ExecuteActionSkill(
|
||||
(void)spec; // Currently unused beyond interface kind selection.
|
||||
if (!action_clients_->has_client(topic)) {
|
||||
detail = "Client not registered";
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] %s, TOPIC: %s", skill.c_str(), detail.c_str(), topic.c_str());
|
||||
LOG_ERROR("[{}] {}, TOPIC: {}", skill.c_str(), detail.c_str(), topic.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -712,7 +709,7 @@ bool CerebellumNode::ExecuteActionSkill(
|
||||
interface_base = skill;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "SKILL: [%s] (Base: %s), TOPIC:[%s]",
|
||||
LOG_INFO("SKILL: [{}] (Base: {}), TOPIC:[{}]",
|
||||
skill.c_str(), interface_base.c_str(), topic.c_str());
|
||||
|
||||
const float base_progress =
|
||||
@@ -739,10 +736,11 @@ bool CerebellumNode::ExecuteActionSkill(
|
||||
effective_timeout = std::chrono::milliseconds(timeout_ms_override);
|
||||
}
|
||||
|
||||
#if (1)
|
||||
if (interface_base == "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());
|
||||
LOG_ERROR("Failed to parse call payload for skill {}", skill.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -752,10 +750,12 @@ bool CerebellumNode::ExecuteActionSkill(
|
||||
} 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());
|
||||
LOG_ERROR("Invalid body_id: {} for skill {}", goal.body_id, skill.c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
return RunActionSkillWithProgress(skill, interface_base, topic, effective_timeout,
|
||||
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
|
||||
}
|
||||
@@ -789,16 +789,15 @@ bool CerebellumNode::RunActionSkillWithProgress(
|
||||
{
|
||||
std::atomic<bool> finished{false};
|
||||
bool ok = false;
|
||||
auto logger = this->get_logger();
|
||||
std::string local_detail;
|
||||
(void)timeout_ms_override; // currently encoded into timeout_ns already
|
||||
std::thread worker([&]() {
|
||||
const auto * prev_calls = CerebellumNode::tls_current_calls_;
|
||||
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] instance='%s' timeout=%.3fs dispatch='%s' topic='%s'",
|
||||
LOG_INFO("[{}] instance='{}' timeout={:.3f}s dispatch='{}' topic='{}'",
|
||||
skill.c_str(), instance_name.c_str(), timeout_ns.count()/1e9, interface_name.c_str(), topic.c_str());
|
||||
ok = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
|
||||
interface_name, topic, action_clients_.get(), logger, timeout_ns, instance_name, local_detail);
|
||||
interface_name, topic, action_clients_.get(), timeout_ns, instance_name, local_detail);
|
||||
CerebellumNode::tls_current_calls_ = prev_calls;
|
||||
finished.store(true, std::memory_order_release);
|
||||
});
|
||||
@@ -817,7 +816,7 @@ bool CerebellumNode::RunActionSkillWithProgress(
|
||||
try {
|
||||
worker.join();
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception in worker thread join: %s", e.what());
|
||||
LOG_ERROR("Exception in worker thread join: {}", e.what());
|
||||
}
|
||||
}
|
||||
detail = local_detail;
|
||||
@@ -851,8 +850,8 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
std::string & detail)
|
||||
{
|
||||
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));
|
||||
LOG_INFO("[Arm] send two goals: body_id=1 and body_id=0, index={}, total_skills={}", index, total_skills);
|
||||
LOG_INFO("[Arm] instance='{}' timeout={} ms", instance_name.c_str(), static_cast<long>(timeout_ms_override));
|
||||
|
||||
std::atomic<bool> right_arm_finished{false}, left_arm_finished{false};
|
||||
bool right_arm_ok = false, left_arm_ok = false;
|
||||
@@ -866,13 +865,13 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
const auto * prev_calls = CerebellumNode::tls_current_calls_;
|
||||
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
|
||||
ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
|
||||
interface_name, topic, action_clients_.get(), this->get_logger(), timeout, instance_name, d_out);
|
||||
interface_name, topic, action_clients_.get(), timeout, instance_name, d_out);
|
||||
CerebellumNode::tls_current_calls_ = prev_calls;
|
||||
done_flag.store(true, std::memory_order_release);
|
||||
time_steady = std::chrono::steady_clock::now();
|
||||
RCLCPP_WARN(this->get_logger(), "left_arm_time_steady2");
|
||||
LOG_WARN("left_arm_time_steady2");
|
||||
};
|
||||
std::thread left_arm_thread(left_arm_worker, LEFT_ARM); // LEFT_ARM=0
|
||||
std::thread left_arm_thread(left_arm_worker, interfaces::msg::ArmMotionParams::ARM_LEFT); // ARM_LEFT=0
|
||||
|
||||
auto right_arm_worker = [this, &ok_out = right_arm_ok, &done_flag = right_arm_finished, &d_out = right_arm_d,
|
||||
skill, interface_name, topic, timeout, &goal_handle, &time_steady = right_arm_time_steady, instance_name](int body_id) {
|
||||
@@ -880,13 +879,13 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
const auto * prev_calls = CerebellumNode::tls_current_calls_;
|
||||
CerebellumNode::tls_current_calls_ = &goal_handle->get_goal()->calls;
|
||||
ok_out = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
|
||||
interface_name, topic, action_clients_.get(), this->get_logger(), timeout, instance_name, d_out);
|
||||
interface_name, topic, action_clients_.get(), timeout, instance_name, d_out);
|
||||
CerebellumNode::tls_current_calls_ = prev_calls;
|
||||
done_flag.store(true, std::memory_order_release);
|
||||
time_steady = std::chrono::steady_clock::now();
|
||||
RCLCPP_WARN(this->get_logger(), "right_arm_time_steady2");
|
||||
LOG_WARN("right_arm_time_steady2");
|
||||
};
|
||||
std::thread right_arm_thread(right_arm_worker, RIGHT_ARM); // RIGHT_ARM=1
|
||||
std::thread right_arm_thread(right_arm_worker, interfaces::msg::ArmMotionParams::ARM_RIGHT); // ARM_RIGHT=1
|
||||
|
||||
const auto start_steady = std::chrono::steady_clock::now();
|
||||
|
||||
@@ -897,26 +896,26 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
}
|
||||
|
||||
double arm_exe_bias = std::chrono::duration_cast<std::chrono::milliseconds>(left_arm_time_steady - right_arm_time_steady).count();
|
||||
RCLCPP_WARN(this->get_logger(), "arm_exe_bias: %f", arm_exe_bias);
|
||||
LOG_WARN("arm_exe_bias: {}", arm_exe_bias);
|
||||
|
||||
if (timeout_ms_override > 0 && std::abs(arm_exe_bias) > timeout_ms_override) {
|
||||
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));
|
||||
LOG_ERROR("Fail in Arm arm_exe_bias: {}, timeout_ms_override={}", arm_exe_bias, static_cast<long>(timeout_ms_override));
|
||||
}
|
||||
|
||||
if (right_arm_thread.joinable()) {
|
||||
try {
|
||||
right_arm_thread.join();
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception in Arm thread 1 join: %s", e.what());
|
||||
LOG_ERROR("Exception in Arm thread 1 join: {}", e.what());
|
||||
}
|
||||
}
|
||||
if (left_arm_thread.joinable()) {
|
||||
try {
|
||||
left_arm_thread.join();
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception in Arm thread 2 join: %s", e.what());
|
||||
LOG_ERROR("Exception in Arm thread 2 join: {}", e.what());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1005,13 +1004,11 @@ void CerebellumNode::RecordSkillResult(const std::string & skill, bool success)
|
||||
void CerebellumNode::LogStatsPeriodic()
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(stats_mutex_);
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "[stats] sequences success=%zu failure=%zu",
|
||||
LOG_INFO("[stats] sequences success={} failure={}",
|
||||
total_sequence_success_, total_sequence_failure_);
|
||||
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,
|
||||
LOG_INFO("skill {}: OK={} FAIL={}", kv.first.c_str(), kv.second,
|
||||
fails);
|
||||
}
|
||||
if (stats_pub_) {
|
||||
@@ -1115,7 +1112,7 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
|
||||
}
|
||||
const auto & skill = skills[seq_index];
|
||||
const std::string topic = ResolveTopicForSkill(skill);
|
||||
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Dispatch skill=%s topic=%s, total_skills:%d",
|
||||
LOG_INFO("[ExecuteBtAction] Dispatch skill={} topic={}, total_skills:{}",
|
||||
skill.c_str(), topic.c_str(), static_cast<int>(skills.size()));
|
||||
bool ok = false; std::string detail;
|
||||
auto it = skill_manager_->skills().find(skill);
|
||||
@@ -1142,7 +1139,7 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
|
||||
if (loop_index + 1 < skills.size()) summary << ", ";
|
||||
++loop_index;
|
||||
if (!ok) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Skill %s failed%s, detail: %s", skill.c_str(),
|
||||
LOG_ERROR("Skill {} failed{}, detail: {}", skill.c_str(),
|
||||
abort_on_failure_ ? ", abort sequence" : " (continue)", detail.c_str());
|
||||
if (abort_on_failure_) break;
|
||||
}
|
||||
@@ -1171,7 +1168,6 @@ bool CerebellumNode::ValidateAndParseGoal(
|
||||
std::vector<std::string> & skills_out) const
|
||||
{
|
||||
using EBA = interfaces::action::ExecuteBtAction;
|
||||
auto logger = this->get_logger();
|
||||
const auto goal = goal_handle->get_goal();
|
||||
if (!goal) {
|
||||
auto res = std::make_shared<EBA::Result>();
|
||||
@@ -1193,10 +1189,10 @@ bool CerebellumNode::ValidateAndParseGoal(
|
||||
oss << goal->calls[i].name;
|
||||
}
|
||||
oss << "] (count=" << goal->calls.size() << ")";
|
||||
RCLCPP_INFO(logger, "%s", oss.str().c_str());
|
||||
LOG_INFO("{}", oss.str().c_str());
|
||||
} else {
|
||||
const std::string & raw = goal->action_name;
|
||||
RCLCPP_INFO(logger, "[ExecuteBtAction] Received goal action_name=%s", raw.c_str());
|
||||
LOG_INFO("[ExecuteBtAction] Received goal action_name={}", raw.c_str());
|
||||
skills_out = ParseSkillSequence(raw);
|
||||
}
|
||||
if (skills_out.empty()) {
|
||||
@@ -1274,13 +1270,12 @@ void CerebellumNode::FinalizeExecuteBtResult(
|
||||
res->message = summary_stream.str();
|
||||
res->total_skills = total_skills;
|
||||
res->succeeded_skills = succeeded_skills;
|
||||
auto logger = this->get_logger();
|
||||
if (overall_ok) {
|
||||
goal_handle->succeed(res);
|
||||
RCLCPP_INFO(logger, "%s", res->message.c_str());
|
||||
LOG_INFO("{}", res->message.c_str());
|
||||
} else {
|
||||
goal_handle->abort(res);
|
||||
RCLCPP_ERROR(logger, "%s", res->message.c_str());
|
||||
LOG_ERROR("{}", res->message.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1365,7 +1360,7 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequenceConcurrent(
|
||||
}
|
||||
const auto & skill = skills[seq_index];
|
||||
const std::string topic = ResolveTopicForSkillFromCalls(calls, skill);
|
||||
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Dispatch skill=%s topic=%s, total_skills:%d",
|
||||
LOG_INFO("[ExecuteBtAction] Dispatch skill={} topic={}, total_skills:{}",
|
||||
skill.c_str(), topic.c_str(), static_cast<int>(skills.size()));
|
||||
bool ok = false; std::string detail;
|
||||
auto it = skill_manager_->skills().find(skill);
|
||||
@@ -1392,7 +1387,7 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequenceConcurrent(
|
||||
if (loop_index + 1 < skills.size()) summary << ", ";
|
||||
++loop_index;
|
||||
if (!ok) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Skill %s failed%s, detail: %s", skill.c_str(),
|
||||
LOG_ERROR("Skill {} failed{}, detail: {}", skill.c_str(),
|
||||
abort_on_failure_ ? ", abort sequence" : " (continue)", detail.c_str());
|
||||
if (abort_on_failure_) break;
|
||||
}
|
||||
@@ -1423,10 +1418,10 @@ void CerebellumNode::JointStateCallback(const sensor_msgs::msg::JointState::Shar
|
||||
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] ",
|
||||
LOG_INFO("Received left arm joint angle[{}, {}, {}, {}, {}, {}] ",
|
||||
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] ",
|
||||
LOG_INFO("Received right arm joint angle[{}, {}, {}, {}, {}, {}] ",
|
||||
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
|
||||
@@ -1436,7 +1431,7 @@ void CerebellumNode::JointStateCallback(const sensor_msgs::msg::JointState::Shar
|
||||
int CerebellumNode::CallInverseKinematics(const tf2::Vector3& pos, const tf2::Quaternion& quat, std::vector<double>& angles, int arm_id)
|
||||
{
|
||||
if (!ik_client_->wait_for_service(std::chrono::seconds(1))) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Service inverse_kinematics not available");
|
||||
LOG_ERROR("Service inverse_kinematics not available");
|
||||
return -1;
|
||||
}
|
||||
|
||||
@@ -1462,15 +1457,18 @@ int CerebellumNode::CallInverseKinematics(const tf2::Vector3& pos, const tf2::Qu
|
||||
for (size_t i = 0; i < result->joint_angles.size(); ++i) {
|
||||
angles[i] = result->joint_angles[i];
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "arm_id %d Inverse kinematics service SUCCESS returned angles: [%f, %f, %f, %f, %f, %f]", arm_id,
|
||||
angles[0], angles[1], angles[2], angles[3], angles[4], angles[5]);
|
||||
const bool has_angles = angles.size() >= 6;
|
||||
LOG_INFO("arm_id {} Inverse kinematics service SUCCESS returned angles: [{}, {}, {}, {}, {}, {}]", arm_id,
|
||||
has_angles ? angles[0] : 0.0, has_angles ? angles[1] : 0.0,
|
||||
has_angles ? angles[2] : 0.0, has_angles ? angles[3] : 0.0,
|
||||
has_angles ? angles[4] : 0.0, has_angles ? angles[5] : 0.0);
|
||||
return 0;
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "arm_id %d Inverse kinematics service returned failure: %d", arm_id, result->result);
|
||||
LOG_ERROR("arm_id {} Inverse kinematics service returned failure: {}", arm_id, result->result);
|
||||
return -1;
|
||||
}
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "arm_id %d Inverse kinematics service call timed out", arm_id);
|
||||
LOG_ERROR("arm_id {} Inverse kinematics service call timed out", arm_id);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -24,6 +24,7 @@
|
||||
#include <behaviortree_cpp/decorator_node.h>
|
||||
#include <behaviortree_cpp/controls/parallel_node.h>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
#include <rcl_interfaces/msg/set_parameters_result.hpp>
|
||||
|
||||
// Project headers
|
||||
@@ -245,10 +246,10 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
|
||||
: rclcpp::Node("cerebrum_node", options),
|
||||
share_directory_(ament_index_cpp::get_package_share_directory("brain"))
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Cerebrum node started");
|
||||
LOG_INFO("Cerebrum node started");
|
||||
|
||||
if (!LoadRobotConfiguration()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to load robot configuration");
|
||||
LOG_ERROR("Failed to load robot configuration");
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -257,7 +258,7 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
|
||||
bt_registry_ = std::make_unique<BTRegistry>(factory_, this);
|
||||
skill_manager_ = std::make_unique<SkillManager>(this, action_registry_.get(), bt_registry_.get());
|
||||
if (!skill_manager_->load_from_file(robot_skill_file_path_)) {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to load skills from %s", robot_skill_file_path_.c_str());
|
||||
LOG_WARN("Failed to load skills from {}", robot_skill_file_path_.c_str());
|
||||
}
|
||||
|
||||
DeclareParameters();
|
||||
@@ -269,18 +270,18 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
|
||||
|
||||
bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
|
||||
if (!bt_timer_) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to create BT timer");
|
||||
LOG_ERROR("Failed to create BT timer");
|
||||
}
|
||||
// task_timer_ = this->create_wall_timer(10000ms, [this]() {CerebrumTask();});
|
||||
// if (!task_timer_) {
|
||||
// RCLCPP_ERROR(this->get_logger(), "Failed to create task timer");
|
||||
// LOG_ERROR("Failed to create task timer");
|
||||
// }
|
||||
|
||||
CreateServices();
|
||||
|
||||
RobotWorkInfoPublish();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "CerebrumNode initialized (timeout=%.2f random_pick=%zu)",
|
||||
LOG_INFO("CerebrumNode initialized (timeout={:.2f} random_pick={})",
|
||||
node_timeout_sec_, random_skill_pick_count_);
|
||||
}
|
||||
|
||||
@@ -306,10 +307,10 @@ void CerebrumNode::RegisterBtAction(
|
||||
void CerebrumNode::SendExecuteBt(const std::string & bt_action_name)
|
||||
{
|
||||
if (!action_registry_) {
|
||||
RCLCPP_WARN(this->get_logger(), "Action registry not initialized; cannot send %s", bt_action_name.c_str());
|
||||
LOG_WARN("Action registry not initialized; cannot send {}", bt_action_name.c_str());
|
||||
return;
|
||||
}
|
||||
action_registry_->send(bt_action_name, this->get_logger());
|
||||
action_registry_->send(bt_action_name);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -355,13 +356,13 @@ void CerebrumNode::RegisterActionClient()
|
||||
}
|
||||
robot_work_info_.bt_node_status = "RUNNING";
|
||||
// Debug: log outgoing calls being sent to Cerebellum
|
||||
// RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal with calls_count=%zu", g.calls.size());
|
||||
// LOG_INFO("[ExecuteBtAction] Sending goal with calls_count={}", g.calls.size());
|
||||
for (size_t i = 0; i < g.calls.size(); ++i) {
|
||||
const auto & c = g.calls[i];
|
||||
RCLCPP_INFO(this->get_logger(), " out_call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
|
||||
LOG_INFO(" out_call[{}]: name={} interface={} kind={} topic={} payload_len={}",
|
||||
i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Sending goal epoch=%u skills=%s", g.epoch, g.action_name.c_str());
|
||||
LOG_INFO("[ExecuteBtAction] Sending goal epoch={} skills={}", g.epoch, g.action_name.c_str());
|
||||
return g;
|
||||
},
|
||||
// Feedback
|
||||
@@ -373,8 +374,7 @@ void CerebrumNode::RegisterActionClient()
|
||||
return;
|
||||
}
|
||||
// For parallel per-instance leaves, we only log feedback; mapping to instance is ambiguous.
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"[ExecuteBtAction][fb][epoch=%u] stage=%s skill=%s progress=%.2f detail=%s",
|
||||
LOG_INFO("[ExecuteBtAction][fb][epoch={}] stage={} skill={} progress={:.2f} detail={}",
|
||||
feedback->epoch, feedback->stage.c_str(), feedback->current_skill.c_str(),
|
||||
feedback->progress, feedback->detail.c_str());
|
||||
},
|
||||
@@ -382,10 +382,10 @@ void CerebrumNode::RegisterActionClient()
|
||||
[this](const ActionClientRegistry::GoalHandle<EBA>::WrappedResult & res) {
|
||||
const bool ok = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success);
|
||||
if (ok) {
|
||||
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] OK: %s",
|
||||
LOG_INFO("[ExecuteBtAction][result] OK: {}",
|
||||
res.result ? res.result->message.c_str() : "<null>");
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[ExecuteBtAction][result] FAIL code=%d msg=%s",
|
||||
LOG_ERROR("[ExecuteBtAction][result] FAIL code={} msg={}",
|
||||
static_cast<int>(res.code), res.result ? res.result->message.c_str() : "<null>");
|
||||
}
|
||||
// Per-instance worker threads handle updating node status; avoid mutating shared state here.
|
||||
@@ -402,7 +402,7 @@ void CerebrumNode::SendAction(const std::string & name)
|
||||
if (!action_registry_) {
|
||||
return;
|
||||
}
|
||||
action_registry_->send(name, this->get_logger());
|
||||
action_registry_->send(name);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -443,7 +443,7 @@ void CerebrumNode::CerebrumTask()
|
||||
robot_work_info_.expt_completion_time = GetFutureTime(300);
|
||||
robot_work_time_.start = rclcpp::Clock().now();
|
||||
|
||||
RCLCPP_WARN(this->get_logger(), "CerebrumTask Switching to BT config file path: %s", path_param.config.c_str());
|
||||
LOG_WARN("CerebrumTask Switching to BT config file path: {}", path_param.config.c_str());
|
||||
return;
|
||||
}
|
||||
}
|
||||
@@ -486,7 +486,7 @@ void CerebrumNode::ExecuteBehaviorTree()
|
||||
robot_work_time_.total_seconds += (rclcpp::Clock().now() - robot_work_time_.start).seconds();
|
||||
|
||||
auto duration = (this->now() - bt_execution_start_time_).seconds();
|
||||
RCLCPP_INFO(this->get_logger(), "BT finished status=%s, scheduled=%s, duration=%.2f s",
|
||||
LOG_INFO("BT finished status={}, scheduled={}, duration={:.2f} s",
|
||||
BT::toStr(status, true).c_str(), scheduled.c_str(), duration);
|
||||
}
|
||||
}
|
||||
@@ -503,13 +503,13 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
|
||||
{
|
||||
BT::Tree new_tree;
|
||||
if (!bt_registry_->build_tree_from_xml_file(xml_file, new_tree)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed building BT from file %s", xml_file.c_str());
|
||||
LOG_ERROR("Failed building BT from file {}", xml_file.c_str());
|
||||
return;
|
||||
}
|
||||
try {
|
||||
tree_.haltTree();
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
|
||||
LOG_ERROR("Exception halting previous BT");
|
||||
}
|
||||
tree_ = std::move(new_tree);
|
||||
{
|
||||
@@ -541,7 +541,7 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
|
||||
}
|
||||
bt_execution_start_time_ = this->now();
|
||||
bt_pending_run_ = true;
|
||||
RCLCPP_INFO(this->get_logger(), "BT built from file epoch=%llu", static_cast<unsigned long long>(new_epoch));
|
||||
LOG_INFO("BT built from file epoch={}", static_cast<unsigned long long>(new_epoch));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -560,14 +560,14 @@ void CerebrumNode::UpdateBehaviorTree()
|
||||
UpdatingFlagGuard guard(bt_updating_);
|
||||
BT::Tree new_tree;
|
||||
if (!bt_registry_->build_tree_from_sequence(active_sequence_, new_tree)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed building BT for sequence %s", active_sequence_.c_str());
|
||||
LOG_ERROR("Failed building BT for sequence {}", active_sequence_.c_str());
|
||||
return;
|
||||
}
|
||||
try {
|
||||
tree_.haltTree();
|
||||
} catch (...) {
|
||||
// Swallow halt exceptions.
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
|
||||
LOG_ERROR("Exception halting previous BT");
|
||||
}
|
||||
tree_ = std::move(new_tree);
|
||||
uint64_t new_epoch = epoch_filter_.epoch() + 1;
|
||||
@@ -594,7 +594,7 @@ void CerebrumNode::UpdateBehaviorTree()
|
||||
}
|
||||
bt_execution_start_time_ = this->now();
|
||||
bt_pending_run_ = true;
|
||||
RCLCPP_INFO(this->get_logger(), "BT updated epoch=%llu sequence=%s",
|
||||
LOG_INFO("BT updated epoch={} sequence={}",
|
||||
static_cast<unsigned long long>(new_epoch), active_sequence_.c_str());
|
||||
}
|
||||
|
||||
@@ -625,7 +625,7 @@ void CerebrumNode::RegisterSkillBtActions()
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(exec_mutex_);
|
||||
if (done_instances_epoch_.find(key) != done_instances_epoch_.end()) {
|
||||
// RCLCPP_INFO(this->get_logger(), "[%s/%s] skipped: already SUCCESS in current epoch", bt_type.c_str(), instance_name.c_str());
|
||||
// LOG_INFO("[{}/{}] skipped: already SUCCESS in current epoch", bt_type.c_str(), instance_name.c_str());
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
}
|
||||
@@ -644,7 +644,7 @@ void CerebrumNode::RegisterSkillBtActions()
|
||||
std::string instance_params;
|
||||
if (!DeclareBtActionParamsForSkillInstance(skill.name, instance_name, &instance_params) ||
|
||||
!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "ExecuteBtAction server unavailable");
|
||||
LOG_ERROR("ExecuteBtAction server unavailable");
|
||||
inst.in_flight = false;
|
||||
inst.finished = true;
|
||||
inst.success = false;
|
||||
@@ -665,11 +665,10 @@ void CerebrumNode::RegisterSkillBtActions()
|
||||
UpdateLeafParallelismStatusLocked();
|
||||
}
|
||||
|
||||
auto logger = this->get_logger();
|
||||
std::thread([this, key, goal, logger]() {
|
||||
std::thread([this, key, goal]() {
|
||||
using EBA = interfaces::action::ExecuteBtAction;
|
||||
auto res = action_registry_->send_and_wait_with_goal<EBA>(
|
||||
brain::kExecuteBtActionName, logger, goal,
|
||||
brain::kExecuteBtActionName, goal,
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(node_timeout_sec_)));
|
||||
auto it = per_instance_exec_.find(key);
|
||||
if (it == per_instance_exec_.end()) { return; }
|
||||
@@ -687,7 +686,7 @@ void CerebrumNode::RegisterSkillBtActions()
|
||||
inst.finished = true;
|
||||
}).detach();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "[%s/%s] launched per-instance ExecuteBtAction goal", bt_type.c_str(), instance_name.c_str());
|
||||
LOG_INFO("[{}/{}] launched per-instance ExecuteBtAction goal", bt_type.c_str(), instance_name.c_str());
|
||||
robot_work_info_.bt_node_status = "RUNNING";
|
||||
return BT::NodeStatus::RUNNING;
|
||||
};
|
||||
@@ -723,10 +722,10 @@ void CerebrumNode::RegisterSkillBtActions()
|
||||
UpdateActionInfoStateLocked(key, "HALTED");
|
||||
UpdateLeafParallelismStatusLocked();
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_halted", bt_type.c_str());
|
||||
LOG_INFO("[{}] BTAction on_halted", bt_type.c_str());
|
||||
};
|
||||
try {RegisterBtAction(bt_type, handlers);} catch (const BT::BehaviorTreeException & e) {
|
||||
RCLCPP_WARN(this->get_logger(), "BT type exists %s: %s", bt_type.c_str(), e.what());
|
||||
LOG_WARN("BT type exists {}: {}", bt_type.c_str(), e.what());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -744,7 +743,7 @@ void CerebrumNode::RegisterSkillBtActions()
|
||||
if (!has_match) {
|
||||
done_instances_epoch_.clear();
|
||||
per_node_exec_.clear();
|
||||
RCLCPP_INFO(this->get_logger(), "Cleared done_instances_epoch_ and per_node_exec_ for retry");
|
||||
LOG_INFO("Cleared done_instances_epoch_ and per_node_exec_ for retry");
|
||||
} else {
|
||||
std::size_t removed_count = 0;
|
||||
for (auto it = done_instances_epoch_.begin(); it != done_instances_epoch_.end(); ) {
|
||||
@@ -762,14 +761,14 @@ void CerebrumNode::RegisterSkillBtActions()
|
||||
++it;
|
||||
}
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "Cleared %zu instances matching '%s'", removed_count, match_pattern.c_str());
|
||||
LOG_INFO("Cleared {} instances matching '{}'", removed_count, match_pattern.c_str());
|
||||
}
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
};
|
||||
try {
|
||||
RegisterBtAction("ClearDoneInstances_H", clear_handlers);
|
||||
} catch (const BT::BehaviorTreeException & e) {
|
||||
RCLCPP_WARN(this->get_logger(), "BT type exists ClearDoneInstances_H: %s", e.what());
|
||||
LOG_WARN("BT type exists ClearDoneInstances_H: {}", e.what());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -786,7 +785,7 @@ bool CerebrumNode::UpdateBtActionParamsForSkillInstance(
|
||||
const std::string & instance_params)
|
||||
{
|
||||
if (skill_name.empty() || instance_name.empty()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Update BtAction Params ForSkillInstance called with empty skill_name, instance_name");
|
||||
LOG_ERROR("Update BtAction Params ForSkillInstance called with empty skill_name, instance_name");
|
||||
return false;
|
||||
}
|
||||
// Per-instance namespace: cerebrum.bt.<Skill>.<Instance>.*
|
||||
@@ -799,12 +798,12 @@ bool CerebrumNode::UpdateBtActionParamsForSkillInstance(
|
||||
if (!this->has_parameter(p_payload)) {
|
||||
this->declare_parameter<std::string>(p_payload, std::string(""));
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill instance %s", instance_name.c_str());
|
||||
LOG_INFO("Declaring BT action parameters for skill instance {}", instance_name.c_str());
|
||||
try {
|
||||
this->set_parameter(rclcpp::Parameter(p_payload, instance_params));
|
||||
// RCLCPP_INFO(this->get_logger(), "Seeded sample payload for %s instance %s", skill_name.c_str(), instance_name.c_str());
|
||||
// LOG_INFO("Seeded sample payload for {} instance {}", skill_name.c_str(), instance_name.c_str());
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] seeding sample payload failed: %s", skill_name.c_str(), e.what());
|
||||
LOG_ERROR("[{}] seeding sample payload failed: {}", skill_name.c_str(), e.what());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
@@ -822,16 +821,16 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
|
||||
std::string * out_instance_params)
|
||||
{
|
||||
if (skill_name.empty() || instance_name.empty()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Declare BtAction Params ForSkillInstance called with empty skill_name or instance_name");
|
||||
LOG_ERROR("Declare BtAction Params ForSkillInstance called with empty skill_name or instance_name");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (current_bt_config_params_path_.param.empty() && active_sequence_param_overrides_.empty()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "BT params file path is empty");
|
||||
LOG_ERROR("BT params file path is empty");
|
||||
// return false; //move home no params
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill_name=%s, instance_name=%s",
|
||||
LOG_INFO("Declaring BT action parameters for skill_name={}, instance_name={}",
|
||||
skill_name.c_str(), instance_name.c_str());
|
||||
|
||||
std::string instance_params;
|
||||
@@ -852,18 +851,18 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
|
||||
if (active_sequence_param_overrides_.count(idx)) {
|
||||
instance_params = active_sequence_param_overrides_[idx];
|
||||
found_override = true;
|
||||
RCLCPP_INFO(this->get_logger(), "Found override param at index %zu, instance_params: %s", idx, instance_params.c_str());
|
||||
LOG_INFO("Found override param at index {}, instance_params: {}", idx, instance_params.c_str());
|
||||
}
|
||||
} catch (...) {
|
||||
// Failed to parse index, ignore
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse index %s", idx_str.c_str());
|
||||
LOG_ERROR("Failed to parse index {}", idx_str.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (current_path_copy.param.empty() && !found_override) {
|
||||
RCLCPP_ERROR(this->get_logger(), "BT params file path is empty");
|
||||
LOG_ERROR("BT params file path is empty");
|
||||
// return false; //move home no params
|
||||
}
|
||||
|
||||
@@ -871,23 +870,23 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
|
||||
// Use override
|
||||
} else if (current_path_copy.config == skill_name) {
|
||||
instance_params = current_path_copy.param;
|
||||
RCLCPP_INFO(this->get_logger(), "Remote params: %s", instance_params.c_str());
|
||||
LOG_INFO("Remote params: {}", instance_params.c_str());
|
||||
} else {
|
||||
//READ PARAMS FROM ROBOT CONFIG FILE
|
||||
try {
|
||||
robot_config_params_ = std::make_unique<brain::robot_config::RobotConfig>(current_path_copy.param);
|
||||
auto params = robot_config_params_->GetValue(instance_name, "params");
|
||||
if (params == std::nullopt) {
|
||||
RCLCPP_ERROR(this->get_logger(), "BT params file %s does not contain params for %s",
|
||||
LOG_ERROR("BT params file {} does not contain params for {}",
|
||||
current_path_copy.param.c_str(), instance_name.c_str());
|
||||
return false;
|
||||
} else {
|
||||
instance_params = *params;
|
||||
// RCLCPP_INFO(this->get_logger(), "Loaded BT params for %s, instance_params: %s",
|
||||
// LOG_INFO("Loaded BT params for {}, instance_params: {}",
|
||||
// instance_name.c_str(), instance_params.c_str());
|
||||
}
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
|
||||
LOG_ERROR("[{}] read params failed: {}", skill_name.c_str(), e.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -953,7 +952,7 @@ void CerebrumNode::SetupDynamicParameterHandling()
|
||||
break;
|
||||
}
|
||||
// No further validation here; downstream will interpret.
|
||||
RCLCPP_INFO(this->get_logger(), "[params] updated %s", name.c_str());
|
||||
LOG_INFO("[params] updated {}", name.c_str());
|
||||
}
|
||||
return result;
|
||||
};
|
||||
@@ -1003,11 +1002,11 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::strin
|
||||
long parsed = std::stol(v);
|
||||
if (parsed > 0) call.timeout_ms = static_cast<uint32_t>(parsed);
|
||||
} catch (...) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] invalid timeout_ms value '%s'", skill_name.c_str(), v.c_str());
|
||||
LOG_WARN("[{}] invalid timeout_ms value '{}'", skill_name.c_str(), v.c_str());
|
||||
}
|
||||
}
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
|
||||
LOG_WARN("[{}] read params failed: {}", skill_name.c_str(), e.what());
|
||||
}
|
||||
return call;
|
||||
}
|
||||
@@ -1050,7 +1049,7 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
|
||||
long parsed = std::stol(v);
|
||||
if (parsed > 0) base.timeout_ms = static_cast<uint32_t>(parsed);
|
||||
} catch (...) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s/%s] invalid timeout_ms value '%s'", skill_name.c_str(), instance_name->c_str(), v.c_str());
|
||||
LOG_WARN("[{}/{}] invalid timeout_ms value '{}'", skill_name.c_str(), instance_name->c_str(), v.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1059,7 +1058,7 @@ interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
|
||||
base.timeout_ms = 500;
|
||||
}
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s/%s] read instance params failed: %s", skill_name.c_str(), instance_name->c_str(), e.what());
|
||||
LOG_WARN("[{}/{}] read instance params failed: {}", skill_name.c_str(), instance_name->c_str(), e.what());
|
||||
}
|
||||
return base;
|
||||
}
|
||||
@@ -1176,9 +1175,7 @@ void CerebrumNode::RunVlmModel()
|
||||
} else {
|
||||
auto picked = PickRandomActionSkills(random_skill_pick_count_);
|
||||
if (!register_and_activate("VLMSequence", picked)) {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(),
|
||||
"No action-capable skills available");
|
||||
LOG_WARN("No action-capable skills available");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1226,6 +1223,7 @@ std::vector<std::string> CerebrumNode::SplitQuotedString(const std::string & raw
|
||||
std::string token;
|
||||
bool in_quote_single = false;
|
||||
bool in_quote_double = false;
|
||||
int bracket_depth = 0;
|
||||
|
||||
auto trim_and_push = [&](std::string t) {
|
||||
// 1. Trim surrounding whitespace
|
||||
@@ -1251,7 +1249,15 @@ std::vector<std::string> CerebrumNode::SplitQuotedString(const std::string & raw
|
||||
} else if (c == '"' && !in_quote_single) {
|
||||
in_quote_double = !in_quote_double;
|
||||
token.push_back(c);
|
||||
} else if (c == ',' && !in_quote_single && !in_quote_double) {
|
||||
} else if (!in_quote_single && !in_quote_double && (c == '[' || c == '{')) {
|
||||
++bracket_depth;
|
||||
token.push_back(c);
|
||||
} else if (!in_quote_single && !in_quote_double && (c == ']' || c == '}')) {
|
||||
if (bracket_depth > 0) {
|
||||
--bracket_depth;
|
||||
}
|
||||
token.push_back(c);
|
||||
} else if (c == ',' && !in_quote_single && !in_quote_double && bracket_depth == 0) {
|
||||
trim_and_push(token);
|
||||
token.clear();
|
||||
} else {
|
||||
@@ -1308,8 +1314,7 @@ void CerebrumNode::CancelActiveExecuteBtGoal()
|
||||
{
|
||||
if (action_registry_) {
|
||||
action_registry_->cancel(
|
||||
brain::kExecuteBtActionName,
|
||||
this->get_logger());
|
||||
brain::kExecuteBtActionName);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1320,7 +1325,7 @@ void CerebrumNode::StopBehaviorTree(bool cancel_goals)
|
||||
try {
|
||||
CancelActiveExecuteBtGoal();
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception while cancelling ExecuteBtAction goals");
|
||||
LOG_ERROR("Exception while cancelling ExecuteBtAction goals");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1332,7 +1337,7 @@ void CerebrumNode::StopBehaviorTree(bool cancel_goals)
|
||||
try {
|
||||
tree_.haltTree();
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception halting Behavior Tree");
|
||||
LOG_ERROR("Exception halting Behavior Tree");
|
||||
}
|
||||
|
||||
// 4) Stop periodic ticking and clear cooldown gate
|
||||
@@ -1365,7 +1370,7 @@ void CerebrumNode::StopBehaviorTree(bool cancel_goals)
|
||||
robot_work_info_.bt_node_status = "CANCELLED";
|
||||
robot_work_time_.total_seconds += (rclcpp::Clock().now() - robot_work_time_.start).seconds();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Behavior Tree stopped (epoch=%llu)", static_cast<unsigned long long>(new_epoch));
|
||||
LOG_INFO("Behavior Tree stopped (epoch={})", static_cast<unsigned long long>(new_epoch));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -1463,7 +1468,7 @@ void CerebrumNode::LoadParameters()
|
||||
this->get_parameter("allowed_action_skills", allowed_raw);
|
||||
if (!allowed_raw.empty()) {
|
||||
allowed_action_skills_ = ParseListString(allowed_raw);
|
||||
RCLCPP_INFO(this->get_logger(), "Allowed action skills count=%zu", allowed_action_skills_.size());
|
||||
LOG_INFO("Allowed action skills count={}", allowed_action_skills_.size());
|
||||
}
|
||||
|
||||
std::string fixed_seq_raw;
|
||||
@@ -1473,7 +1478,7 @@ void CerebrumNode::LoadParameters()
|
||||
if (!tmp.empty()) {fixed_skill_sequence_ = tmp;}
|
||||
}
|
||||
if (fixed_skill_sequence_) {
|
||||
RCLCPP_INFO(this->get_logger(), "Using fixed skill sequence size=%zu", fixed_skill_sequence_->size());
|
||||
LOG_INFO("Using fixed skill sequence size={}", fixed_skill_sequence_->size());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1498,7 +1503,7 @@ void CerebrumNode::CreateServices()
|
||||
StopBehaviorTree(true);
|
||||
resp->success = true;
|
||||
resp->message = "No active BT (cleanup applied)";
|
||||
RCLCPP_WARN(this->get_logger(), "No active BT to cancel (cleanup applied)");
|
||||
LOG_WARN("No active BT to cancel (cleanup applied)");
|
||||
return;
|
||||
}
|
||||
StopBehaviorTree(true);
|
||||
@@ -1522,11 +1527,11 @@ void CerebrumNode::CreateServices()
|
||||
// TODO: Implement local rebuild
|
||||
resp->success = false;
|
||||
resp->message = "Local rebuild not implemented";
|
||||
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Local rebuild not implemented");
|
||||
LOG_ERROR("cerebrum/rebuild_now Service Local rebuild not implemented");
|
||||
} else {
|
||||
resp->success = false;
|
||||
resp->message = "Unknown rebuild type";
|
||||
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Unknown rebuild type: %s", req->type.c_str());
|
||||
LOG_ERROR("cerebrum/rebuild_now Service Unknown rebuild type: {}", req->type.c_str());
|
||||
}
|
||||
|
||||
// Update work info for successful rebuilds
|
||||
@@ -1554,10 +1559,10 @@ void CerebrumNode::HandleTriggerRebuild(
|
||||
std::lock_guard<std::mutex> lk(exec_mutex_);
|
||||
active_sequence_param_overrides_.clear(); // Clear overrides on cancel
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "halting previous BT ok, CancelBTTask");
|
||||
LOG_INFO("halting previous BT ok, CancelBTTask");
|
||||
} catch (...) {
|
||||
// Swallow halt exceptions.
|
||||
RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT CancelBTTask");
|
||||
LOG_ERROR("Exception halting previous BT CancelBTTask");
|
||||
}
|
||||
resp->success = true;
|
||||
resp->message = "Rebuild triggered";
|
||||
@@ -1573,7 +1578,7 @@ void CerebrumNode::HandleTriggerRebuild(
|
||||
|
||||
std::string sch_filename = ExtractFilenameWithoutExtension(path_param.config);
|
||||
if (sch_filename != req->config) {
|
||||
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service INVALID config: %s, local: %s", req->config.c_str(), sch_filename.c_str());
|
||||
LOG_WARN("cerebrum/rebuild_now Service INVALID config: {}, local: {}", req->config.c_str(), sch_filename.c_str());
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -1591,7 +1596,7 @@ void CerebrumNode::HandleTriggerRebuild(
|
||||
robot_work_info_.task[2] = sch_filename; // current
|
||||
}
|
||||
|
||||
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Switching to BT config file path: %s", path_param.config.c_str());
|
||||
LOG_WARN("cerebrum/rebuild_now Service Switching to BT config file path: {}", path_param.config.c_str());
|
||||
match_found = true;
|
||||
break;
|
||||
}
|
||||
@@ -1602,7 +1607,7 @@ void CerebrumNode::HandleTriggerRebuild(
|
||||
} else {
|
||||
resp->success = false;
|
||||
resp->message = "Rebuild config not found";
|
||||
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Failed to find config: %s", req->config.c_str());
|
||||
LOG_ERROR("cerebrum/rebuild_now Service Failed to find config: {}", req->config.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1622,7 +1627,7 @@ void CerebrumNode::HandleGenericRebuild(
|
||||
if (!skills.empty()) {
|
||||
// Check if params match skills count
|
||||
if (!params.empty() && params.size() != skills.size()) {
|
||||
RCLCPP_WARN(this->get_logger(), "HandleGenericRebuild: Skills count (%zu) != Params count (%zu).", skills.size(), params.size());
|
||||
LOG_WARN("HandleGenericRebuild: Skills count ({}) != Params count ({}).", skills.size(), params.size());
|
||||
}
|
||||
|
||||
// Clear previous override params and update config safely
|
||||
@@ -1653,11 +1658,11 @@ void CerebrumNode::HandleGenericRebuild(
|
||||
|
||||
resp->success = true;
|
||||
resp->message = req->type + " rebuild triggered";
|
||||
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service %s rebuild triggered", req->type.c_str());
|
||||
LOG_WARN("cerebrum/rebuild_now Service {} rebuild triggered", req->type.c_str());
|
||||
} else {
|
||||
resp->success = false;
|
||||
resp->message = req->type + " rebuild failed";
|
||||
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Failed to parse %s rebuild config", req->type.c_str());
|
||||
LOG_ERROR("cerebrum/rebuild_now Service Failed to parse {} rebuild config", req->type.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1695,7 +1700,7 @@ bool CerebrumNode::LoadRobotConfiguration()
|
||||
robot_config_ = std::make_unique<brain::robot_config::RobotConfig>(config_path);
|
||||
auto skill_file = robot_config_->SkillFile("brain");
|
||||
if (skill_file == std::nullopt) {
|
||||
RCLCPP_ERROR(this->get_logger(), "No skill_file entry found for 'brain' in robot_config.yaml");
|
||||
LOG_ERROR("No skill_file entry found for 'brain' in robot_config.yaml");
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -1704,13 +1709,13 @@ bool CerebrumNode::LoadRobotConfiguration()
|
||||
? share_directory_ + *skill_file
|
||||
: skill_file_path_;
|
||||
|
||||
RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
|
||||
LOG_WARN("skill file {}", robot_skill_file_path_.c_str());
|
||||
auto path = robot_config_->ConfigParamsPath("cerebrum_node");
|
||||
if (path != std::nullopt) {
|
||||
for (const auto & p : *path) {
|
||||
if (!p.config.empty() && !p.param.empty()) {
|
||||
bt_config_params_paths_.push_back({share_directory_ + p.config, share_directory_ + p.param});
|
||||
RCLCPP_WARN(this->get_logger(), "bt config param %s %s", p.config.c_str(), p.param.c_str());
|
||||
LOG_WARN("bt config param {} {}", p.config.c_str(), p.param.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1735,7 +1740,7 @@ int CerebrumNode::SimulatedBatteryCapacity()
|
||||
if (elapsed >= kBatteryDecrementIntervalSec) {
|
||||
if (simulated_battery_capacity > 0) {
|
||||
--simulated_battery_capacity;
|
||||
RCLCPP_INFO(this->get_logger(), "Battery simulator: decreased to %d%%", simulated_battery_capacity);
|
||||
LOG_INFO("Battery simulator: decreased to {}%%", simulated_battery_capacity);
|
||||
}
|
||||
last_battery_update = this->now();
|
||||
}
|
||||
@@ -1782,7 +1787,7 @@ void CerebrumNode::RobotWorkInfoPublish()
|
||||
interfaces::msg::RobotWorkInfo>("/robot_work_info", 10);
|
||||
|
||||
if (!robot_work_info_pub_) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to create robot work info publisher");
|
||||
LOG_ERROR("Failed to create robot work info publisher");
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -1838,12 +1843,12 @@ void CerebrumNode::RobotWorkInfoPublish()
|
||||
|
||||
if (robot_work_info_pub_) {
|
||||
robot_work_info_pub_->publish(msg);
|
||||
// RCLCPP_WARN(this->get_logger(), "Publishing robot work info id: %ld", msg.msg_id);
|
||||
// LOG_WARN("Publishing robot work info id: {}", msg.msg_id);
|
||||
}
|
||||
});
|
||||
|
||||
if (!pub_robot_work_info_timer_) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to create robot work info timer");
|
||||
LOG_ERROR("Failed to create robot work info timer");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -14,6 +14,7 @@
|
||||
#include <cmath>
|
||||
#include <utility>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include <hivecore_logger/logger.hpp>
|
||||
|
||||
namespace brain
|
||||
{
|
||||
@@ -87,7 +88,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
|
||||
{
|
||||
YAML::Node root = YAML::LoadFile(yaml_path);
|
||||
if (!root || !root.IsSequence()) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Invalid skills YAML: expected sequence at root");
|
||||
LOG_ERROR("Invalid skills YAML: expected sequence at root");
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -112,7 +113,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
|
||||
// Extended form: { name: BaseName.kind, default_topic: <str> }
|
||||
const auto name_node = v["name"];
|
||||
if (!name_node || !name_node.IsScalar()) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Invalid interface entry (missing 'name'): %s", YAML::Dump(v).c_str());
|
||||
LOG_ERROR("Invalid interface entry (missing 'name'): {}", YAML::Dump(v).c_str());
|
||||
continue;
|
||||
}
|
||||
const std::string token = name_node.as<std::string>("");
|
||||
@@ -126,7 +127,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
|
||||
}
|
||||
}
|
||||
} else {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Unsupported interface entry type: %s", YAML::Dump(v).c_str());
|
||||
LOG_ERROR("Unsupported interface entry type: {}", YAML::Dump(v).c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -135,7 +136,7 @@ bool SkillManager::load_from_file(const std::string & yaml_path)
|
||||
register_interfaces_(s);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Loaded %zu skills from %s", skills_.size(), yaml_path.c_str());
|
||||
LOG_INFO("Loaded {} skills from {}", skills_.size(), yaml_path.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -275,15 +276,15 @@ static std::string join_keys(const std::unordered_map<std::string, T> & m)
|
||||
}
|
||||
|
||||
template<size_t I = 0>
|
||||
static void validate_action_registrars_complete(rclcpp::Logger logger)
|
||||
static void validate_action_registrars_complete()
|
||||
{
|
||||
if constexpr (I < std::tuple_size<SkillActionTypes>::value) {
|
||||
using ActionT = std::tuple_element_t<I, SkillActionTypes>;
|
||||
const char * name = SkillActionTrait<ActionT>::skill_name;
|
||||
if (get_action_registrars().find(name) == get_action_registrars().end()) {
|
||||
RCLCPP_ERROR(logger, "Missing action registrar for '%s'", name);
|
||||
LOG_ERROR("Missing action registrar for '{}'", name);
|
||||
}
|
||||
validate_action_registrars_complete<I + 1>(logger);
|
||||
validate_action_registrars_complete<I + 1>();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -300,7 +301,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
|
||||
// One-time runtime validation to ensure all actions in SkillActionTypes have a registrar.
|
||||
static bool s_validated = false;
|
||||
if (!s_validated) {
|
||||
validate_action_registrars_complete(node_->get_logger());
|
||||
validate_action_registrars_complete();
|
||||
s_validated = true;
|
||||
}
|
||||
|
||||
@@ -316,7 +317,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
|
||||
for (const auto & iface : s.interfaces) {
|
||||
auto parsed = parse_interface_entry(iface);
|
||||
if (!parsed) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
|
||||
LOG_ERROR("Skip invalid interface entry: {}", iface.c_str());
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -366,7 +367,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
|
||||
if (it != action_regs.end()) {
|
||||
it->second(this, topic_name, s.name);
|
||||
} else {
|
||||
RCLCPP_ERROR(node_->get_logger(), "No action registrar for interface base '%s'. Available: [%s]",
|
||||
LOG_ERROR("No action registrar for interface base '{}'. Available: [{}]",
|
||||
parsed->base.c_str(), join_keys(action_regs).c_str());
|
||||
}
|
||||
} else if (parsed->kind == "srv") {
|
||||
@@ -376,7 +377,7 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
|
||||
if (it != service_regs.end()) {
|
||||
it->second(this, topic_name, s.name);
|
||||
} else {
|
||||
RCLCPP_ERROR(node_->get_logger(), "No service registrar for interface base '%s'. Available: [%s]",
|
||||
LOG_ERROR("No service registrar for interface base '{}'. Available: [{}]",
|
||||
parsed->base.c_str(), join_keys(service_regs).c_str());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,4 +0,0 @@
|
||||
#!/bin/bash
|
||||
|
||||
colcon build --packages-select smacc2_msgs smacc2 brain
|
||||
#colcon build --packages-select smacc2_msgs smacc2 brain arm_control leg_control vision_object_recg hand_control
|
||||
@@ -1,46 +0,0 @@
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
# # 欧拉角
|
||||
# rx, ry, rz = -1.433, 0.114, -0.430
|
||||
|
||||
# # 尝试不同的旋转顺序
|
||||
# orders = ['xyz', 'zyx', 'xzy', 'yxz', 'yzx', 'zyx']
|
||||
|
||||
# print("尝试不同旋转顺序的结果：")
|
||||
# for order in orders:
|
||||
# try:
|
||||
# rot = R.from_euler(order, [rx, ry, rz], degrees=False)
|
||||
# quat = rot.as_quat() # [x, y, z, w]
|
||||
# print(f"顺序 {order}: [{quat[0]:.8f}, {quat[1]:.8f}, {quat[2]:.8f}, {quat[3]:.8f}]")
|
||||
# except:
|
||||
# continue
|
||||
|
||||
# # 特别检查XYZ顺序
|
||||
# print("\nXYZ顺序的详细计算：")
|
||||
# rot_xyz = R.from_euler('xyz', [rx, ry, rz], degrees=False)
|
||||
# quat_xyz = rot_xyz.as_quat()
|
||||
# print(f"XYZ顺序: [{quat_xyz[0]:.8f}, {quat_xyz[1]:.8f}, {quat_xyz[2]:.8f}, {quat_xyz[3]:.8f}]")
|
||||
|
||||
|
||||
# 输入欧拉角 (rx, ry, rz)，单位弧度
|
||||
# euler_angles_rad = [-1.433, 0.114, -0.430]
|
||||
# euler_angles_rad = [-1.622, -0.016, -0.4]
|
||||
# euler_angles_rad = [-0.972, -0.557, -0.901]
|
||||
# euler_angles_rad = [-1.674, 0.223, -0.747]
|
||||
# euler_angles_rad = [-1.484, 0.152, -0.26]
|
||||
# euler_angles_rad = [-1.691, -0.443, -0.452]
|
||||
# euler_angles_rad = [-1.72, -0.189, -0.223]
|
||||
# euler_angles_rad = [-2.213, -0.202, -0.391]
|
||||
# euler_angles_rad = [-1.358, -0.284, -0.255]
|
||||
# euler_angles_rad = [-1.509, -0.234, -0.416]
|
||||
# euler_angles_rad = [-1.79, -0.447, -0.133]
|
||||
euler_angles_rad = [-1.468, 0.199, -0.361]
|
||||
|
||||
# 创建旋转对象，指定欧拉角顺序为 'xyz'
|
||||
rot = R.from_euler('xyz', euler_angles_rad, degrees=False)
|
||||
|
||||
# 转换为四元数，格式为 [x, y, z, w]
|
||||
quaternion = rot.as_quat()
|
||||
|
||||
print(f"四元数 (x, y, z, w): [{quaternion[0]:.8f}, {quaternion[1]:.8f}, {quaternion[2]:.8f}, {quaternion[3]:.8f}]")
|
||||
@@ -1,207 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Convert poses to quaternions (qx, qy, qz, qw) from either:
|
||||
- Euler angles (rx, ry, rz)
|
||||
- Rotation vector (Rodrigues) (rx, ry, rz) where |r| is angle in rad
|
||||
|
||||
Supports:
|
||||
- CSV input/output with configurable column names
|
||||
- Unit options: radians/degrees for angles, mm->m for position
|
||||
- Euler order: xyz (default) or zyx
|
||||
|
||||
Examples:
|
||||
python src/scripts/euler_to_quaternion.py \
|
||||
--in poses.csv --out poses_quat.csv --mode euler \
|
||||
--rx rx --ry ry --rz rz --order xyz \
|
||||
--x x --y y --z z --mm-to-m
|
||||
|
||||
# Single value
|
||||
python src/scripts/euler_to_quaternion.py --mode euler --single 0.1 0.2 -0.3 --order zyx
|
||||
|
||||
# Rotation vector (e.g., from many robots):
|
||||
python src/scripts/euler_to_quaternion.py --mode rotvec --single -1.433 0.114 -0.430
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import math
|
||||
from typing import Tuple, List
|
||||
|
||||
|
||||
def euler_to_quaternion(rx: float, ry: float, rz: float, order: str = "xyz", degrees: bool = False) -> Tuple[float, float, float, float]:
|
||||
"""Convert Euler angles to quaternion [x, y, z, w].
|
||||
|
||||
Args:
|
||||
rx, ry, rz: Euler angles. If degrees=True, values are in degrees; else radians.
|
||||
order: Rotation order. Supported: 'xyz' (default), 'zyx'. Intrinsic rotations.
|
||||
degrees: Whether the input angles are in degrees.
|
||||
|
||||
Returns:
|
||||
(qx, qy, qz, qw)
|
||||
"""
|
||||
if degrees:
|
||||
rx, ry, rz = math.radians(rx), math.radians(ry), math.radians(rz)
|
||||
|
||||
def q_from_axis_angle(ax: str, angle: float) -> Tuple[float, float, float, float]:
|
||||
s = math.sin(angle / 2.0)
|
||||
c = math.cos(angle / 2.0)
|
||||
if ax == 'x':
|
||||
return (s, 0.0, 0.0, c)
|
||||
if ax == 'y':
|
||||
return (0.0, s, 0.0, c)
|
||||
if ax == 'z':
|
||||
return (0.0, 0.0, s, c)
|
||||
raise ValueError('Invalid axis')
|
||||
|
||||
def q_mul(q1: Tuple[float, float, float, float], q2: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]:
|
||||
x1, y1, z1, w1 = q1
|
||||
x2, y2, z2, w2 = q2
|
||||
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
|
||||
y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
|
||||
z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
|
||||
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
|
||||
return (x, y, z, w)
|
||||
|
||||
if order == 'xyz':
|
||||
qx = q_from_axis_angle('x', rx)
|
||||
qy = q_from_axis_angle('y', ry)
|
||||
qz = q_from_axis_angle('z', rz)
|
||||
q = q_mul(q_mul(qx, qy), qz)
|
||||
elif order == 'zyx':
|
||||
qz = q_from_axis_angle('z', rz)
|
||||
qy = q_from_axis_angle('y', ry)
|
||||
qx = q_from_axis_angle('x', rx)
|
||||
q = q_mul(q_mul(qz, qy), qx)
|
||||
else:
|
||||
raise ValueError("Unsupported Euler order. Use 'xyz' or 'zyx'.")
|
||||
|
||||
# Normalize
|
||||
norm = math.sqrt(sum(v * v for v in q))
|
||||
return tuple(v / (norm + 1e-12) for v in q) # type: ignore
|
||||
|
||||
|
||||
def rotvec_to_quaternion(rx: float, ry: float, rz: float) -> Tuple[float, float, float, float]:
|
||||
"""Convert rotation vector (Rodrigues) to quaternion [x,y,z,w].
|
||||
The vector direction is the rotation axis and its norm is the rotation angle in radians.
|
||||
"""
|
||||
angle = math.sqrt(rx * rx + ry * ry + rz * rz)
|
||||
if angle < 1e-12:
|
||||
return (0.0, 0.0, 0.0, 1.0)
|
||||
ax = rx / angle
|
||||
ay = ry / angle
|
||||
az = rz / angle
|
||||
s = math.sin(angle / 2.0)
|
||||
c = math.cos(angle / 2.0)
|
||||
q = (ax * s, ay * s, az * s, c)
|
||||
norm = math.sqrt(sum(v * v for v in q))
|
||||
return tuple(v / (norm + 1e-12) for v in q) # type: ignore
|
||||
|
||||
|
||||
def convert_csv(in_path: str, out_path: str,
|
||||
rx_col: str = 'rx', ry_col: str = 'ry', rz_col: str = 'rz',
|
||||
x_col: str | None = None, y_col: str | None = None, z_col: str | None = None,
|
||||
order: str = 'xyz', degrees: bool = False, mm_to_m: bool = False,
|
||||
mode: str = 'euler') -> None:
|
||||
"""Read CSV, append quaternion columns (qx,qy,qz,qw) and write to output.
|
||||
|
||||
If x/y/z columns are provided, optionally convert mm->m.
|
||||
"""
|
||||
with open(in_path, 'r', newline='') as f_in:
|
||||
reader = csv.DictReader(f_in)
|
||||
fieldnames: List[str] = list(reader.fieldnames or [])
|
||||
# Ensure quaternion columns at the end
|
||||
for c in ['qx', 'qy', 'qz', 'qw']:
|
||||
if c not in fieldnames:
|
||||
fieldnames.append(c)
|
||||
# If converting position units, overwrite or add x_m/y_m/z_m
|
||||
if x_col and y_col and z_col and mm_to_m:
|
||||
for c in ['x_m', 'y_m', 'z_m']:
|
||||
if c not in fieldnames:
|
||||
fieldnames.append(c)
|
||||
|
||||
with open(out_path, 'w', newline='') as f_out:
|
||||
writer = csv.DictWriter(f_out, fieldnames=fieldnames)
|
||||
writer.writeheader()
|
||||
for row in reader:
|
||||
try:
|
||||
rx = float(row[rx_col])
|
||||
ry = float(row[ry_col])
|
||||
rz = float(row[rz_col])
|
||||
except KeyError as e:
|
||||
raise KeyError(f"Missing rotation columns in CSV: {e}")
|
||||
if mode == 'euler':
|
||||
qx, qy, qz, qw = euler_to_quaternion(rx, ry, rz, order=order, degrees=degrees)
|
||||
elif mode == 'rotvec':
|
||||
qx, qy, qz, qw = rotvec_to_quaternion(rx, ry, rz)
|
||||
else:
|
||||
raise ValueError("mode must be 'euler' or 'rotvec'")
|
||||
row.update({'qx': f"{qx:.8f}", 'qy': f"{qy:.8f}", 'qz': f"{qz:.8f}", 'qw': f"{qw:.8f}"})
|
||||
|
||||
if x_col and y_col and z_col and (x_col in row and y_col in row and z_col in row):
|
||||
try:
|
||||
x = float(row[x_col])
|
||||
y = float(row[y_col])
|
||||
z = float(row[z_col])
|
||||
if mm_to_m:
|
||||
row['x_m'] = f"{x / 1000.0:.6f}"
|
||||
row['y_m'] = f"{y / 1000.0:.6f}"
|
||||
row['z_m'] = f"{z / 1000.0:.6f}"
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
writer.writerow(row)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='Convert pose angles to quaternion (qx,qy,qz,qw) from Euler or rotation vector.')
|
||||
parser.add_argument('--mode', type=str, default='euler', choices=['euler', 'rotvec'], help='Input angle format')
|
||||
parser.add_argument('--in', dest='in_path', type=str, help='Input CSV file path')
|
||||
parser.add_argument('--out', dest='out_path', type=str, help='Output CSV file path')
|
||||
parser.add_argument('--rx', type=str, default='rx', help='Column name for rx')
|
||||
parser.add_argument('--ry', type=str, default='ry', help='Column name for ry')
|
||||
parser.add_argument('--rz', type=str, default='rz', help='Column name for rz')
|
||||
parser.add_argument('--x', type=str, default=None, help='Column name for x (position)')
|
||||
parser.add_argument('--y', type=str, default=None, help='Column name for y (position)')
|
||||
parser.add_argument('--z', type=str, default=None, help='Column name for z (position)')
|
||||
parser.add_argument('--order', type=str, default='xyz', choices=['xyz', 'zyx'], help='Euler order')
|
||||
parser.add_argument('--degrees', action='store_true', help='Input angles are in degrees (default radians)')
|
||||
parser.add_argument('--mm-to-m', action='store_true', help='Convert position units from mm to m when x/y/z provided')
|
||||
|
||||
# single conversion mode
|
||||
parser.add_argument('--single', nargs=3, type=float, metavar=('RX', 'RY', 'RZ'),
|
||||
help='Convert a single Euler triplet to quaternion (prints to stdout)')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.single is not None:
|
||||
rx, ry, rz = args.single
|
||||
if args.mode == 'euler':
|
||||
q = euler_to_quaternion(rx, ry, rz, order=args.order, degrees=args.degrees)
|
||||
else:
|
||||
q = rotvec_to_quaternion(rx, ry, rz)
|
||||
print(f"qx,qy,qz,qw = {q[0]:.8f}, {q[1]:.8f}, {q[2]:.8f}, {q[3]:.8f}")
|
||||
return
|
||||
|
||||
if not args.in_path or not args.out_path:
|
||||
parser.error('CSV mode requires --in and --out')
|
||||
|
||||
convert_csv(
|
||||
in_path=args.in_path,
|
||||
out_path=args.out_path,
|
||||
rx_col=args.rx,
|
||||
ry_col=args.ry,
|
||||
rz_col=args.rz,
|
||||
x_col=args.x,
|
||||
y_col=args.y,
|
||||
z_col=args.z,
|
||||
order=args.order,
|
||||
degrees=args.degrees,
|
||||
mm_to_m=args.mm_to_m,
|
||||
mode=args.mode,
|
||||
)
|
||||
print(f"Written: {args.out_path}")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -31,6 +31,7 @@ Notes:
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import glob
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
@@ -94,6 +95,51 @@ def resolve_interface_subdir(root: str, sub: str) -> str:
|
||||
return candidates[0]
|
||||
|
||||
|
||||
def normalize_ros_type(ros_type: str) -> str:
|
||||
bt = ros_type.replace('::', '/')
|
||||
bt = bt.replace('/msg/', '/').replace('/Msg/', '/')
|
||||
return bt
|
||||
|
||||
|
||||
def split_pkg_msg(ros_type: str):
|
||||
if '/' not in ros_type:
|
||||
return None, None
|
||||
pkg, msg = ros_type.split('/', 1)
|
||||
return pkg, msg
|
||||
|
||||
|
||||
def find_msg_file(interfaces_root: str, pkg: str, msg: str) -> Optional[str]:
|
||||
if not pkg or not msg:
|
||||
return None
|
||||
if pkg == 'interfaces':
|
||||
msg_dir = resolve_interface_subdir(interfaces_root, 'msg')
|
||||
if not os.path.isdir(msg_dir):
|
||||
return None
|
||||
msg_file = os.path.join(msg_dir, msg + '.msg')
|
||||
if os.path.exists(msg_file):
|
||||
return msg_file
|
||||
found = find_file_case_insensitive(msg_dir, msg + '.msg')
|
||||
if found:
|
||||
return found
|
||||
snake_path = os.path.join(msg_dir, to_snake(msg) + '.msg')
|
||||
if os.path.exists(snake_path):
|
||||
return snake_path
|
||||
return None
|
||||
|
||||
distro = os.environ.get('ROS_DISTRO')
|
||||
if distro:
|
||||
candidate = os.path.join('/opt/ros', distro, 'share', pkg, 'msg', msg + '.msg')
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
for candidate in glob.glob(os.path.join('/opt/ros', '*', 'share', pkg, 'msg', msg + '.msg')):
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
candidate = os.path.join('/usr/share', pkg, 'msg', msg + '.msg')
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
return None
|
||||
|
||||
|
||||
def parse_action_file(path: str) -> List[Tuple[str, str]]:
|
||||
"""Parse a .action file, return list of (ros_type, field_name) only for Goal part."""
|
||||
try:
|
||||
@@ -108,6 +154,11 @@ def parse_action_file(path: str) -> List[Tuple[str, str]]:
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
if '=' in line:
|
||||
continue
|
||||
m = re.match(r"^([A-Za-z0-9_\[\]/]+)\s+([A-Za-z0-9_]+).*$", line)
|
||||
if m:
|
||||
ty = m.group(1)
|
||||
@@ -116,6 +167,47 @@ def parse_action_file(path: str) -> List[Tuple[str, str]]:
|
||||
return fields
|
||||
|
||||
|
||||
def parse_msg_file(path: str) -> List[Tuple[str, str]]:
|
||||
"""Parse a .msg file, return list of (ros_type, field_name)."""
|
||||
try:
|
||||
with open(path, 'r', encoding='utf-8') as f:
|
||||
content = f.read()
|
||||
except FileNotFoundError:
|
||||
return []
|
||||
fields: List[Tuple[str, str]] = []
|
||||
for line in content.splitlines():
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
if '=' in line:
|
||||
continue
|
||||
m = re.match(r"^([A-Za-z0-9_\[\]/]+)\s+([A-Za-z0-9_]+).*$", line)
|
||||
if m:
|
||||
ty = m.group(1)
|
||||
name = m.group(2)
|
||||
fields.append((ty, name))
|
||||
return fields
|
||||
|
||||
|
||||
_MSG_FIELDS_CACHE: Dict[Tuple[str, str], Optional[List[Tuple[str, str]]]] = {}
|
||||
|
||||
|
||||
def get_msg_fields(interfaces_root: str, pkg: str, msg: str) -> Optional[List[Tuple[str, str]]]:
|
||||
key = (pkg, msg)
|
||||
if key in _MSG_FIELDS_CACHE:
|
||||
return _MSG_FIELDS_CACHE[key]
|
||||
path = find_msg_file(interfaces_root, pkg, msg)
|
||||
if path and os.path.exists(path):
|
||||
fields = parse_msg_file(path)
|
||||
else:
|
||||
fields = None
|
||||
_MSG_FIELDS_CACHE[key] = fields
|
||||
return fields
|
||||
|
||||
|
||||
def locate_action_file(interfaces_root: str, base: str, type: str) -> Optional[str]:
|
||||
action_dir = resolve_interface_subdir(interfaces_root, type)
|
||||
# 1) Try as-is (CamelCase)
|
||||
@@ -141,7 +233,8 @@ def locate_action_file(interfaces_root: str, base: str, type: str) -> Optional[s
|
||||
return None
|
||||
|
||||
|
||||
def make_placeholder_for_type(ros_type: str) -> object:
|
||||
def make_placeholder_for_type(ros_type: str, interfaces_root: str, current_pkg: Optional[str] = None,
|
||||
visited: Optional[set] = None) -> object:
|
||||
"""Return a default placeholder Python value for a ROS field type.
|
||||
|
||||
- Primitive -> default numeric/bool/string
|
||||
@@ -150,42 +243,62 @@ def make_placeholder_for_type(ros_type: str) -> object:
|
||||
- geometry_msgs/TwistStamped -> nested dict with common keys
|
||||
- Otherwise -> None
|
||||
"""
|
||||
if visited is None:
|
||||
visited = set()
|
||||
is_array = ros_type.endswith('[]')
|
||||
base = ros_type[:-2] if is_array else ros_type
|
||||
# normalize namespace
|
||||
base_norm = base.replace('::', '/')
|
||||
base_norm = base_norm.replace('/msg/', '/').replace('/Msg/', '/')
|
||||
base_norm = normalize_ros_type(base)
|
||||
|
||||
if is_array:
|
||||
return []
|
||||
if '/' not in base_norm:
|
||||
# primitive
|
||||
return PRIMITIVES_DEFAULTS.get(base_norm, None)
|
||||
if base_norm in ("geometry_msgs/PoseStamped",):
|
||||
return {
|
||||
'frame_id': '',
|
||||
'stamp_sec': 0,
|
||||
'stamp_nanosec': 0,
|
||||
val = PRIMITIVES_DEFAULTS.get(base_norm, None)
|
||||
return [] if is_array else val
|
||||
|
||||
# known geometry messages fallback
|
||||
if base_norm in ("geometry_msgs/Pose",):
|
||||
val = {
|
||||
'position': {'x': 0.0, 'y': 0.0, 'z': 0.0},
|
||||
'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0},
|
||||
}
|
||||
if base_norm in ("geometry_msgs/TwistStamped",):
|
||||
return {
|
||||
'frame_id': '',
|
||||
'stamp_sec': 0,
|
||||
'stamp_nanosec': 0,
|
||||
'linear': {'x': 0.0, 'y': 0.0, 'z': 0.0},
|
||||
'angular': {'x': 0.0, 'y': 0.0, 'z': 0.0},
|
||||
}
|
||||
# unknown complex -> None
|
||||
return None
|
||||
return [val] if is_array else val
|
||||
if base_norm in ("geometry_msgs/Point",):
|
||||
val = {'x': 0.0, 'y': 0.0, 'z': 0.0}
|
||||
return [val] if is_array else val
|
||||
if base_norm in ("geometry_msgs/Quaternion",):
|
||||
val = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
|
||||
return [val] if is_array else val
|
||||
|
||||
pkg, msg = split_pkg_msg(base_norm)
|
||||
if not pkg:
|
||||
pkg = current_pkg
|
||||
msg = base_norm
|
||||
if not pkg or not msg:
|
||||
return [] if is_array else None
|
||||
|
||||
if (pkg, msg) in visited:
|
||||
return [] if is_array else None
|
||||
visited.add((pkg, msg))
|
||||
|
||||
fields = get_msg_fields(interfaces_root, pkg, msg)
|
||||
if not fields:
|
||||
visited.discard((pkg, msg))
|
||||
return [] if is_array else None
|
||||
|
||||
mapping: Dict[str, object] = {}
|
||||
for ftype, fname in fields:
|
||||
mapping[fname] = make_placeholder_for_type(ftype, interfaces_root, pkg, visited)
|
||||
|
||||
visited.discard((pkg, msg))
|
||||
if is_array:
|
||||
return [mapping]
|
||||
return mapping
|
||||
|
||||
|
||||
def build_yaml_params_from_fields(fields: Sequence[Tuple[str, str]]) -> str:
|
||||
def build_yaml_params_from_fields(fields: Sequence[Tuple[str, str]], interfaces_root: str) -> str:
|
||||
"""Create a YAML snippet string from .action Goal fields."""
|
||||
mapping: Dict[str, object] = {}
|
||||
for ros_type, name in fields:
|
||||
mapping[name] = make_placeholder_for_type(ros_type)
|
||||
mapping[name] = make_placeholder_for_type(ros_type, interfaces_root)
|
||||
# Dump as YAML block string
|
||||
# Ensure multi-line by setting default_flow_style=False
|
||||
snippet = yaml.safe_dump(mapping, allow_unicode=True, sort_keys=False)
|
||||
@@ -272,7 +385,7 @@ def main():
|
||||
if action_file == None:
|
||||
action_file = locate_action_file(interfaces_root, base, "srv")
|
||||
fields: List[Tuple[str, str]] = parse_action_file(action_file) if action_file else []
|
||||
yaml_params = build_yaml_params_from_fields(fields)
|
||||
yaml_params = build_yaml_params_from_fields(fields, interfaces_root)
|
||||
results.append({'name': instance_name, 'params': yaml_params})
|
||||
|
||||
# Write output as YAML list; ensure deterministic order as in XML
|
||||
|
||||
@@ -19,6 +19,7 @@ and array primitives. For complex ROS message types it will emit a TODO comment.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import glob
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
@@ -66,15 +67,10 @@ HEADER_POSTAMBLE = '\n} // namespace brain\n'
|
||||
|
||||
|
||||
def to_snake(s: str) -> str:
|
||||
out = ''
|
||||
for i,ch in enumerate(s):
|
||||
if ch.isupper():
|
||||
if i>0:
|
||||
out += '_'
|
||||
out += ch.lower()
|
||||
else:
|
||||
out += ch
|
||||
return out
|
||||
# Convert CamelCase (including initialisms like ASRRecognize) to snake_case
|
||||
s1 = re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', s)
|
||||
s2 = re.sub(r'([a-z0-9])([A-Z])', r'\1_\2', s1)
|
||||
return s2.lower()
|
||||
|
||||
|
||||
def parse_action_file(path: str):
|
||||
@@ -89,6 +85,13 @@ def parse_action_file(path: str):
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
# Strip comments before checking constants
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
# Skip constants like: uint8 STATUS_PLANNING=0
|
||||
if '=' in line:
|
||||
continue
|
||||
# lines like: float32 move_pitch_degree
|
||||
m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
|
||||
if m:
|
||||
@@ -109,6 +112,37 @@ def parse_srv_file(path: str):
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
# Strip comments before checking constants
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
# Skip constants like: uint8 STATUS_PLANNING=0
|
||||
if '=' in line:
|
||||
continue
|
||||
m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
|
||||
if m:
|
||||
ty = m.group(1)
|
||||
name = m.group(2)
|
||||
fields.append((ty, name))
|
||||
return fields
|
||||
|
||||
|
||||
def parse_msg_file(path: str):
|
||||
"""Parse a .msg file, return list of (type,name) fields."""
|
||||
with open(path, 'r', encoding='utf-8') as f:
|
||||
content = f.read()
|
||||
fields = []
|
||||
for line in content.splitlines():
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
# Strip comments before checking constants
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
# Skip constants like: uint8 STATUS_PLANNING=0
|
||||
if '=' in line:
|
||||
continue
|
||||
m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
|
||||
if m:
|
||||
ty = m.group(1)
|
||||
@@ -160,7 +194,148 @@ def resolve_interface_subdir(root: str, sub: str):
|
||||
return candidates[0]
|
||||
|
||||
|
||||
def gen_converter_action(base: str, fields, include_path: str):
|
||||
def normalize_ros_type(ros_type: str) -> str:
|
||||
bt = ros_type.replace('::', '/')
|
||||
bt = bt.replace('/msg/', '/')
|
||||
bt = bt.replace('/Msg/', '/')
|
||||
return bt
|
||||
|
||||
|
||||
def split_pkg_msg(ros_type: str):
|
||||
if '/' not in ros_type:
|
||||
return None, None
|
||||
pkg, msg = ros_type.split('/', 1)
|
||||
return pkg, msg
|
||||
|
||||
|
||||
def find_msg_file(interfaces_root: str, pkg: str, msg: str):
|
||||
if not pkg or not msg:
|
||||
return None
|
||||
if pkg == 'interfaces':
|
||||
msg_dir = resolve_interface_subdir(interfaces_root, 'msg')
|
||||
if not os.path.isdir(msg_dir):
|
||||
return None
|
||||
msg_file = os.path.join(msg_dir, msg + '.msg')
|
||||
if os.path.exists(msg_file):
|
||||
return msg_file
|
||||
found = find_file_case_insensitive(msg_dir, msg + '.msg')
|
||||
if found:
|
||||
return found
|
||||
snake_path = os.path.join(msg_dir, to_snake(msg) + '.msg')
|
||||
if os.path.exists(snake_path):
|
||||
return snake_path
|
||||
return None
|
||||
|
||||
# Try common ROS share locations for other packages (e.g. geometry_msgs)
|
||||
distro = os.environ.get('ROS_DISTRO')
|
||||
if distro:
|
||||
candidate = os.path.join('/opt/ros', distro, 'share', pkg, 'msg', msg + '.msg')
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
for candidate in glob.glob(os.path.join('/opt/ros', '*', 'share', pkg, 'msg', msg + '.msg')):
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
candidate = os.path.join('/usr/share', pkg, 'msg', msg + '.msg')
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
return None
|
||||
|
||||
|
||||
_MSG_FIELDS_CACHE = {}
|
||||
|
||||
|
||||
def get_msg_fields(interfaces_root: str, pkg: str, msg: str):
|
||||
key = (pkg, msg)
|
||||
if key in _MSG_FIELDS_CACHE:
|
||||
return _MSG_FIELDS_CACHE[key]
|
||||
path = find_msg_file(interfaces_root, pkg, msg)
|
||||
if path and os.path.exists(path):
|
||||
fields = parse_msg_file(path)
|
||||
else:
|
||||
fields = None
|
||||
_MSG_FIELDS_CACHE[key] = fields
|
||||
return fields
|
||||
|
||||
|
||||
def msg_cpp_type(pkg: str, msg: str):
|
||||
return f'{pkg}::msg::{msg}'
|
||||
|
||||
|
||||
def emit_msg_fields(cpp, node_expr: str, target_expr: str, fields, indent: str,
|
||||
interfaces_root: str, visited: set, current_pkg: str, depth: int = 0):
|
||||
for ty, fname in fields:
|
||||
is_array, cpp_type, base_type = make_cpp_type(ty)
|
||||
if is_array and cpp_type:
|
||||
cpp.append(f'{indent}if ({node_expr}["{fname}"]) {{')
|
||||
cpp.append(f'{indent} auto vec = {node_expr}["{fname}"].as<std::vector<{cpp_type}>>();')
|
||||
cpp.append(f'{indent} {target_expr}.{fname}.assign(vec.begin(), vec.end());')
|
||||
cpp.append(f'{indent}}}')
|
||||
elif not is_array and cpp_type:
|
||||
cpp.append(f'{indent}if ({node_expr}["{fname}"]) {target_expr}.{fname} = {node_expr}["{fname}"].as<{cpp_type}>();')
|
||||
else:
|
||||
bt = normalize_ros_type(base_type)
|
||||
pkg, msg = split_pkg_msg(bt)
|
||||
if not pkg:
|
||||
pkg = current_pkg
|
||||
msg = bt
|
||||
if pkg and msg and (pkg, msg) not in visited:
|
||||
nested_fields = get_msg_fields(interfaces_root, pkg, msg)
|
||||
if nested_fields:
|
||||
visited.add((pkg, msg))
|
||||
if is_array:
|
||||
cpp.append(f'{indent}if ({node_expr}["{fname}"] && {node_expr}["{fname}"].IsSequence()) {{')
|
||||
cpp.append(f'{indent} for (const auto & item : {node_expr}["{fname}"]) {{')
|
||||
cpp.append(f'{indent} {msg_cpp_type(pkg, msg)} tmp;')
|
||||
emit_msg_fields(cpp, 'item', 'tmp', nested_fields, indent + ' ', interfaces_root, visited, pkg, depth + 1)
|
||||
cpp.append(f'{indent} {target_expr}.{fname}.push_back(tmp);')
|
||||
cpp.append(f'{indent} }}')
|
||||
cpp.append(f'{indent}}}')
|
||||
else:
|
||||
sub_var = f'node_{depth}_{fname}'
|
||||
cpp.append(f'{indent}if (auto {sub_var} = {node_expr}["{fname}"]) {{')
|
||||
emit_msg_fields(cpp, sub_var, f'{target_expr}.{fname}', nested_fields, indent + ' ', interfaces_root, visited, pkg, depth + 1)
|
||||
cpp.append(f'{indent}}}')
|
||||
visited.discard((pkg, msg))
|
||||
continue
|
||||
cpp.append(f'{indent}// TODO: parse field `{fname}` of ROS type `{ty}` into {target_expr}.{fname} (complex type)')
|
||||
|
||||
|
||||
def emit_complex_field(cpp, name: str, base_type: str, is_array: bool, target_root: str,
|
||||
interfaces_root: str):
|
||||
bt = normalize_ros_type(base_type)
|
||||
pkg, msg = split_pkg_msg(bt)
|
||||
if not pkg or not msg:
|
||||
return False
|
||||
fields = get_msg_fields(interfaces_root, pkg, msg)
|
||||
if not fields:
|
||||
return False
|
||||
visited = {(pkg, msg)}
|
||||
if is_array:
|
||||
cpp.append(f' if (n["{name}"] && n["{name}"].IsSequence()) {{')
|
||||
cpp.append(f' for (const auto & item : n["{name}"]) {{')
|
||||
cpp.append(f' {msg_cpp_type(pkg, msg)} tmp;')
|
||||
emit_msg_fields(cpp, 'item', 'tmp', fields, ' ', interfaces_root, visited, pkg, 1)
|
||||
cpp.append(f' {target_root}.{name}.push_back(tmp);')
|
||||
cpp.append(' }')
|
||||
cpp.append(' }')
|
||||
else:
|
||||
sub_var = f'node_0_{name}'
|
||||
cpp.append(f' if (auto {sub_var} = n["{name}"]) {{')
|
||||
emit_msg_fields(cpp, sub_var, f'{target_root}.{name}', fields, ' ', interfaces_root, visited, pkg, 1)
|
||||
cpp.append(' }')
|
||||
return True
|
||||
|
||||
|
||||
def add_msg_includes_for_fields(fields, includes):
|
||||
for ty, _ in fields:
|
||||
base_ty = ty[:-2] if ty.endswith('[]') else ty
|
||||
base_ty = normalize_ros_type(base_ty)
|
||||
pkg, msg = split_pkg_msg(base_ty)
|
||||
if pkg == 'interfaces' and msg:
|
||||
includes.add(f'"interfaces/msg/{to_snake(msg)}.hpp"')
|
||||
|
||||
|
||||
def gen_converter_action(base: str, fields, include_path: str, interfaces_root: str):
|
||||
# base: MoveWaist
|
||||
cpp = []
|
||||
cpp.append(f'// {base}::Goal: ' + ', '.join([n for _,n in fields]))
|
||||
@@ -182,47 +357,7 @@ def gen_converter_action(base: str, fields, include_path: str):
|
||||
# map to as<type>()
|
||||
cpp.append(f' if (n["{name}"]) g.{name} = n["{name}"].as<{cpp_type}>();')
|
||||
else:
|
||||
# complex types: handle some known messages
|
||||
# normalize geometry type names like geometry_msgs/msg/PoseStamped -> geometry_msgs/PoseStamped
|
||||
bt = base_type.replace('::', '/')
|
||||
bt = bt.replace('/msg/', '/')
|
||||
bt = bt.replace('/Msg/', '/')
|
||||
if bt in ("geometry_msgs/PoseStamped", "geometry_msgs/PoseStamped") and not is_array:
|
||||
cpp.append(' // geometry_msgs::msg::PoseStamped')
|
||||
cpp.append(' if (n) {')
|
||||
cpp.append(' if (n["frame_id"]) g.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
|
||||
cpp.append(' if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
|
||||
cpp.append(' if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
|
||||
cpp.append(' if (auto p = n["position"]) {')
|
||||
cpp.append(' if (p["x"]) g.{0}.pose.position.x = p["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (p["y"]) g.{0}.pose.position.y = p["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (p["z"]) g.{0}.pose.position.z = p["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' if (auto q = n["orientation"]) {')
|
||||
cpp.append(' if (q["x"]) g.{0}.pose.orientation.x = q["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["y"]) g.{0}.pose.orientation.y = q["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["z"]) g.{0}.pose.orientation.z = q["z"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["w"]) g.{0}.pose.orientation.w = q["w"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' }')
|
||||
elif bt in ("geometry_msgs/TwistStamped", "geometry_msgs/TwistStamped") and not is_array:
|
||||
cpp.append(' // geometry_msgs::msg::TwistStamped')
|
||||
cpp.append(' if (n) {')
|
||||
cpp.append(' if (n["frame_id"]) g.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
|
||||
cpp.append(' if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
|
||||
cpp.append(' if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
|
||||
cpp.append(' if (auto lin = n["linear"]) {')
|
||||
cpp.append(' if (lin["x"]) g.{0}.twist.linear.x = lin["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (lin["y"]) g.{0}.twist.linear.y = lin["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (lin["z"]) g.{0}.twist.linear.z = lin["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' if (auto ang = n["angular"]) {')
|
||||
cpp.append(' if (ang["x"]) g.{0}.twist.angular.x = ang["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (ang["y"]) g.{0}.twist.angular.y = ang["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (ang["z"]) g.{0}.twist.angular.z = ang["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' }')
|
||||
else:
|
||||
if not emit_complex_field(cpp, name, base_type, is_array, 'g', interfaces_root):
|
||||
cpp.append(f' // TODO: parse field `{name}` of ROS type `{ty}` into g.{name} (complex type)')
|
||||
# post-processing example: if both data_array and data_length exist, set length from array size
|
||||
if has_data_array and has_data_length:
|
||||
@@ -236,7 +371,7 @@ def gen_converter_action(base: str, fields, include_path: str):
|
||||
return '\n'.join(cpp)
|
||||
|
||||
|
||||
def gen_converter_srv(base: str, fields, include_path: str):
|
||||
def gen_converter_srv(base: str, fields, include_path: str, interfaces_root: str):
|
||||
cpp = []
|
||||
cpp.append(f'// {base}::Request: ' + ', '.join([n for _,n in fields]))
|
||||
cpp.append('template<>')
|
||||
@@ -254,45 +389,7 @@ def gen_converter_srv(base: str, fields, include_path: str):
|
||||
elif not is_array and cpp_type:
|
||||
cpp.append(f' if (n["{name}"]) r.{name} = n["{name}"].as<{cpp_type}>();')
|
||||
else:
|
||||
bt = base_type.replace('::', '/')
|
||||
bt = bt.replace('/msg/', '/')
|
||||
bt = bt.replace('/Msg/', '/')
|
||||
if bt in ("geometry_msgs/PoseStamped", "geometry_msgs/PoseStamped") and not is_array:
|
||||
cpp.append(' // geometry_msgs::msg::PoseStamped')
|
||||
cpp.append(' if (n) {')
|
||||
cpp.append(' if (n["frame_id"]) r.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
|
||||
cpp.append(' if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
|
||||
cpp.append(' if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
|
||||
cpp.append(' if (auto p = n["position"]) {')
|
||||
cpp.append(' if (p["x"]) r.{0}.pose.position.x = p["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (p["y"]) r.{0}.pose.position.y = p["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (p["z"]) r.{0}.pose.position.z = p["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' if (auto q = n["orientation"]) {')
|
||||
cpp.append(' if (q["x"]) r.{0}.pose.orientation.x = q["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["y"]) r.{0}.pose.orientation.y = q["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["z"]) r.{0}.pose.orientation.z = q["z"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["w"]) r.{0}.pose.orientation.w = q["w"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' }')
|
||||
elif bt in ("geometry_msgs/TwistStamped", "geometry_msgs/TwistStamped") and not is_array:
|
||||
cpp.append(' // geometry_msgs::msg::TwistStamped')
|
||||
cpp.append(' if (n) {')
|
||||
cpp.append(' if (n["frame_id"]) r.{0}.header.frame_id = n["frame_id"].as<std::string>();'.format(name))
|
||||
cpp.append(' if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
|
||||
cpp.append(' if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
|
||||
cpp.append(' if (auto lin = n["linear"]) {')
|
||||
cpp.append(' if (lin["x"]) r.{0}.twist.linear.x = lin["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (lin["y"]) r.{0}.twist.linear.y = lin["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (lin["z"]) r.{0}.twist.linear.z = lin["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' if (auto ang = n["angular"]) {')
|
||||
cpp.append(' if (ang["x"]) r.{0}.twist.angular.x = ang["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (ang["y"]) r.{0}.twist.angular.y = ang["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (ang["z"]) r.{0}.twist.angular.z = ang["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' }')
|
||||
else:
|
||||
if not emit_complex_field(cpp, name, base_type, is_array, 'r', interfaces_root):
|
||||
cpp.append(f' // TODO: parse field `{name}` of ROS type `{ty}` into r.{name} (complex type)')
|
||||
cpp.append(' }')
|
||||
cpp.append(' return r;')
|
||||
@@ -378,7 +475,8 @@ def main():
|
||||
fields = []
|
||||
else:
|
||||
fields = parse_action_file(action_file)
|
||||
converters.append(gen_converter_action(base, fields, header_snake))
|
||||
add_msg_includes_for_fields(fields, includes)
|
||||
converters.append(gen_converter_action(base, fields, header_snake, interfaces_root))
|
||||
added_actions.add(base)
|
||||
elif kind.startswith('srv') or kind.startswith('service'):
|
||||
if base in added_srvs:
|
||||
@@ -408,7 +506,8 @@ def main():
|
||||
fields = []
|
||||
else:
|
||||
fields = parse_srv_file(srv_file)
|
||||
converters.append(gen_converter_srv(base, fields, header_snake))
|
||||
add_msg_includes_for_fields(fields, includes)
|
||||
converters.append(gen_converter_srv(base, fields, header_snake, interfaces_root))
|
||||
added_srvs.add(base)
|
||||
# end of iface loop
|
||||
pass
|
||||
@@ -427,7 +526,8 @@ def main():
|
||||
header_snake = to_snake(base_type)
|
||||
includes.add(f'"interfaces/action/{header_snake}.hpp"')
|
||||
fields = parse_action_file(os.path.join(action_dir, fname))
|
||||
converters.append(gen_converter_action(base_type, fields, header_snake))
|
||||
add_msg_includes_for_fields(fields, includes)
|
||||
converters.append(gen_converter_action(base_type, fields, header_snake, interfaces_root))
|
||||
added_actions.add(base_type)
|
||||
|
||||
srv_dir = resolve_interface_subdir(interfaces_root, 'srv')
|
||||
@@ -442,7 +542,8 @@ def main():
|
||||
header_snake = to_snake(base_type)
|
||||
includes.add(f'"interfaces/srv/{header_snake}.hpp"')
|
||||
fields = parse_srv_file(os.path.join(srv_dir, fname))
|
||||
converters.append(gen_converter_srv(base_type, fields, header_snake))
|
||||
add_msg_includes_for_fields(fields, includes)
|
||||
converters.append(gen_converter_srv(base_type, fields, header_snake, interfaces_root))
|
||||
added_srvs.add(base_type)
|
||||
|
||||
# assemble header
|
||||
|
||||
@@ -14,6 +14,7 @@ The script is intentionally conservative: it emits empty default_topic values; y
|
||||
"""
|
||||
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
import yaml
|
||||
from pathlib import Path
|
||||
@@ -118,6 +119,14 @@ for entry in skills:
|
||||
if base not in service_default_map or skill_name == base:
|
||||
service_default_map[base] = dtopic
|
||||
|
||||
# Ensure all available interfaces are included so traits exist for any action/srv used in code
|
||||
for a in actions:
|
||||
if a not in skill_action_types:
|
||||
skill_action_types.append(a)
|
||||
for s in services:
|
||||
if s not in skill_service_types:
|
||||
skill_service_types.append(s)
|
||||
|
||||
# Helper to map base name to include path / C++ type name.
|
||||
# Assumption: interfaces live in package `interfaces` and types are under `interfaces::action::Name` or `interfaces::srv::Name`.
|
||||
|
||||
@@ -128,15 +137,10 @@ def service_cpp_type(base):
|
||||
return f"interfaces::srv::{base}"
|
||||
|
||||
def to_snake_case(s: str) -> str:
|
||||
out = []
|
||||
for i, ch in enumerate(s):
|
||||
if ch.isupper():
|
||||
if i != 0:
|
||||
out.append('_')
|
||||
out.append(ch.lower())
|
||||
else:
|
||||
out.append(ch)
|
||||
return ''.join(out)
|
||||
# Convert CamelCase (including initialisms like ASRRecognize) to snake_case
|
||||
s1 = re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', s)
|
||||
s2 = re.sub(r'([a-z0-9])([A-Z])', r'\1_\2', s1)
|
||||
return s2.lower()
|
||||
|
||||
|
||||
def parse_action_result_fields(action_file: Path):
|
||||
|
||||
@@ -1,588 +0,0 @@
|
||||
import numpy as np
|
||||
import cv2
|
||||
from typing import List, Tuple
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import glob
|
||||
import csv
|
||||
|
||||
def hand_eye_calibration(robot_poses: List[np.ndarray],
|
||||
camera_poses: List[np.ndarray],
|
||||
mode: str = 'eye_in_hand') -> np.ndarray:
|
||||
"""
|
||||
执行手眼标定，解决AX = XB问题。
|
||||
|
||||
参数:
|
||||
robot_poses (list of np.array): 机械臂末端在基座坐标系中的位姿列表 (4x4齐次矩阵)。
|
||||
camera_poses (list of np.array): 标定板在相机坐标系中的位姿列表 (4x4齐次矩阵)。注意：这是 T_c_t（target->camera），与OpenCV的 R_target2cam/t_target2cam 一致。
|
||||
mode (str): 标定模式。'eye_in_hand' 或 'eye_to_hand'。
|
||||
|
||||
返回:
|
||||
X (np.array): 求解出的变换矩阵X (4x4齐次矩阵)。
|
||||
对于 eye_in_hand: X = camera^T_end_effector (相机在机械臂末端坐标系中的位姿)
|
||||
对于 eye_to_hand: X = base^T_camera (相机在机器人基座坐标系中的位姿)
|
||||
"""
|
||||
# 输入验证
|
||||
n = len(robot_poses)
|
||||
if n != len(camera_poses):
|
||||
raise ValueError("机器人位姿数量与相机位姿数量必须相同。")
|
||||
if n < 3:
|
||||
raise ValueError("至少需要三组数据（且包含显著旋转）才能进行稳定标定。")
|
||||
|
||||
# OpenCV calibrateHandEye 需要每一帧的绝对位姿：
|
||||
# - R_gripper2base, t_gripper2base: T_g^b（夹爪到基座）
|
||||
# - R_target2cam, t_target2cam: T_t^c（标定板到相机）
|
||||
# 传入时请确保 robot_poses 为 T_b^g（基座到夹爪），camera_poses 为 T_c^t（目标到相机）
|
||||
R_gripper2base: List[np.ndarray] = []
|
||||
t_gripper2base: List[np.ndarray] = []
|
||||
R_target2cam: List[np.ndarray] = []
|
||||
t_target2cam: List[np.ndarray] = []
|
||||
|
||||
for i in range(n):
|
||||
T_b_g = robot_poses[i]
|
||||
if T_b_g.shape != (4, 4):
|
||||
raise ValueError("robot_poses 中的矩阵必须是 4x4")
|
||||
T_g_b = np.linalg.inv(T_b_g)
|
||||
R_gripper2base.append(T_g_b[:3, :3])
|
||||
t_gripper2base.append(T_g_b[:3, 3].reshape(3, 1))
|
||||
|
||||
T_c_t = camera_poses[i]
|
||||
if T_c_t.shape != (4, 4):
|
||||
raise ValueError("camera_poses 中的矩阵必须是 4x4")
|
||||
R_target2cam.append(T_c_t[:3, :3])
|
||||
t_target2cam.append(T_c_t[:3, 3].reshape(3, 1))
|
||||
|
||||
# 选择算法，失败时自动尝试其他方法
|
||||
methods = [
|
||||
getattr(cv2, 'CALIB_HAND_EYE_TSAI', None),
|
||||
getattr(cv2, 'CALIB_HAND_EYE_PARK', None),
|
||||
getattr(cv2, 'CALIB_HAND_EYE_HORAUD', None),
|
||||
getattr(cv2, 'CALIB_HAND_EYE_ANDREFF', None),
|
||||
getattr(cv2, 'CALIB_HAND_EYE_DANIILIDIS', None),
|
||||
]
|
||||
methods = [m for m in methods if m is not None]
|
||||
|
||||
last_err = None
|
||||
for method in methods:
|
||||
try:
|
||||
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
|
||||
R_gripper2base, t_gripper2base,
|
||||
R_target2cam, t_target2cam,
|
||||
method=method,
|
||||
)
|
||||
# 成功返回
|
||||
X_cam_gripper = np.eye(4)
|
||||
X_cam_gripper[:3, :3] = R_cam2gripper
|
||||
X_cam_gripper[:3, 3] = t_cam2gripper.flatten()
|
||||
|
||||
if mode == 'eye_in_hand':
|
||||
# 直接返回 camera^T_gripper
|
||||
return X_cam_gripper
|
||||
else:
|
||||
# eye_to_hand：需要 base^T_camera。已知：
|
||||
# 对每一帧 i，有 T_b^c_i = T_b^g_i * T_g^c（其中 T_g^c = (T_c^g)^{-1}）
|
||||
T_g_c = np.linalg.inv(X_cam_gripper)
|
||||
T_b_c_list = []
|
||||
for T_b_g in robot_poses:
|
||||
T_b_c_list.append(T_b_g @ T_g_c)
|
||||
return average_SE3(T_b_c_list)
|
||||
except Exception as e:
|
||||
last_err = e
|
||||
continue
|
||||
|
||||
raise RuntimeError(f"hand-eye 标定失败：{last_err}")
|
||||
|
||||
def create_homogeneous_matrix(rvec, tvec):
|
||||
"""
|
||||
根据旋转向量和平移向量创建4x4齐次变换矩阵。
|
||||
|
||||
参数:
|
||||
rvec: 3x1旋转向量
|
||||
tvec: 3x1平移向量
|
||||
|
||||
返回:
|
||||
T: 4x4齐次变换矩阵
|
||||
"""
|
||||
R, _ = cv2.Rodrigues(rvec)
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R
|
||||
T[:3, 3] = tvec.flatten()
|
||||
return T
|
||||
|
||||
|
||||
def rot_to_quat(R: np.ndarray) -> np.ndarray:
|
||||
"""将旋转矩阵转为四元数 [x, y, z, w]（右手，单位四元数）。"""
|
||||
tr = np.trace(R)
|
||||
if tr > 0.0:
|
||||
S = np.sqrt(tr + 1.0) * 2.0
|
||||
qw = 0.25 * S
|
||||
qx = (R[2, 1] - R[1, 2]) / S
|
||||
qy = (R[0, 2] - R[2, 0]) / S
|
||||
qz = (R[1, 0] - R[0, 1]) / S
|
||||
else:
|
||||
if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
|
||||
S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
|
||||
qx = 0.25 * S
|
||||
qy = (R[0, 1] + R[1, 0]) / S
|
||||
qz = (R[0, 2] + R[2, 0]) / S
|
||||
qw = (R[2, 1] - R[1, 2]) / S
|
||||
elif R[1, 1] > R[2, 2]:
|
||||
S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
|
||||
qx = (R[0, 1] + R[1, 0]) / S
|
||||
qy = 0.25 * S
|
||||
qz = (R[1, 2] + R[2, 1]) / S
|
||||
qw = (R[0, 2] - R[2, 0]) / S
|
||||
else:
|
||||
S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
|
||||
qx = (R[0, 2] + R[2, 0]) / S
|
||||
qy = (R[1, 2] + R[2, 1]) / S
|
||||
qz = 0.25 * S
|
||||
qw = (R[1, 0] - R[0, 1]) / S
|
||||
q = np.array([qx, qy, qz, qw])
|
||||
return q / (np.linalg.norm(q) + 1e-12)
|
||||
|
||||
|
||||
def quat_to_rot(q: np.ndarray) -> np.ndarray:
|
||||
"""四元数 [x, y, z, w] 转旋转矩阵。"""
|
||||
x, y, z, w = q
|
||||
xx, yy, zz = x*x, y*y, z*z
|
||||
xy, xz, yz = x*y, x*z, y*z
|
||||
wx, wy, wz = w*x, w*y, w*z
|
||||
R = np.array([
|
||||
[1 - 2*(yy + zz), 2*(xy - wz), 2*(xz + wy)],
|
||||
[2*(xy + wz), 1 - 2*(xx + zz), 2*(yz - wx)],
|
||||
[2*(xz - wy), 2*(yz + wx), 1 - 2*(xx + yy)]
|
||||
])
|
||||
return R
|
||||
|
||||
|
||||
def average_SE3(T_list: List[np.ndarray]) -> np.ndarray:
|
||||
"""对一组 SE(3) 变换做简单平均（四元数+平移平均）。"""
|
||||
if not T_list:
|
||||
raise ValueError("T_list 为空")
|
||||
# 平移均值
|
||||
t_stack = np.stack([T[:3, 3] for T in T_list], axis=0)
|
||||
t_mean = np.mean(t_stack, axis=0)
|
||||
# 旋转均值（单位四元数平均后归一化）
|
||||
q_list = [rot_to_quat(T[:3, :3]) for T in T_list]
|
||||
# 对齐四元数符号到第一个四元数的半球，避免相互抵消
|
||||
q0 = q_list[0]
|
||||
aligned = []
|
||||
for q in q_list:
|
||||
if np.dot(q, q0) < 0:
|
||||
aligned.append(-q)
|
||||
else:
|
||||
aligned.append(q)
|
||||
q = np.mean(np.stack(aligned, axis=0), axis=0)
|
||||
q = q / (np.linalg.norm(q) + 1e-12)
|
||||
R_mean = quat_to_rot(q)
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R_mean
|
||||
T[:3, 3] = t_mean
|
||||
return T
|
||||
|
||||
|
||||
def hand_eye_residual(robot_poses: List[np.ndarray], camera_poses: List[np.ndarray], X_cam_gripper: np.ndarray) -> Tuple[float, float]:
|
||||
"""基于 AX ≈ X B 的关系计算残差（旋转角度均值，平移均值）。
|
||||
返回 (rot_err_deg, trans_err_m)。"""
|
||||
rot_errs = []
|
||||
trans_errs = []
|
||||
n = len(robot_poses)
|
||||
# 构造相对运动对，避免偏置
|
||||
for i in range(n - 1):
|
||||
A = np.linalg.inv(robot_poses[i + 1]) @ robot_poses[i] # T_b^g_{i+1}^{-1} * T_b^g_i
|
||||
B = np.linalg.inv(camera_poses[i + 1]) @ camera_poses[i] # T_c^t_{i+1}^{-1} * T_c^t_i
|
||||
# OpenCV定义 X 为 T_c^g（camera->gripper）
|
||||
lhs = np.linalg.inv(A) # OpenCV论文中常用 A 为 g2b，但这里用的相对位姿定义略有差异
|
||||
# 直接用 AX 与 XB 的等价残差度量
|
||||
AX = A @ np.linalg.inv(X_cam_gripper)
|
||||
XB = np.linalg.inv(X_cam_gripper) @ B
|
||||
Delta = np.linalg.inv(AX) @ XB
|
||||
R = Delta[:3, :3]
|
||||
t = Delta[:3, 3]
|
||||
# 旋转角度
|
||||
angle = np.rad2deg(np.arccos(np.clip((np.trace(R) - 1) / 2.0, -1.0, 1.0)))
|
||||
rot_errs.append(abs(angle))
|
||||
trans_errs.append(np.linalg.norm(t))
|
||||
return float(np.mean(rot_errs)), float(np.mean(trans_errs))
|
||||
|
||||
# ==================== 示例用法 ====================
|
||||
|
||||
def generate_synthetic_dataset(num_poses: int = 12, seed: int = 42) -> Tuple[List[np.ndarray], List[np.ndarray]]:
|
||||
"""生成可解的模拟数据集 (T_b^g 列表, T_c^t 列表)。"""
|
||||
rng = np.random.default_rng(seed)
|
||||
|
||||
def hat(v):
|
||||
return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
|
||||
|
||||
def exp_so3(w):
|
||||
th = np.linalg.norm(w)
|
||||
if th < 1e-12:
|
||||
return np.eye(3)
|
||||
k = w / th
|
||||
K = hat(k)
|
||||
return np.eye(3) + np.sin(th) * K + (1 - np.cos(th)) * (K @ K)
|
||||
|
||||
def make_T(R, t):
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R
|
||||
T[:3, 3] = t
|
||||
return T
|
||||
|
||||
# 真实（未知）相机在夹爪中的位姿：T_c^g
|
||||
R_c_g = exp_so3(np.deg2rad([10, -5, 15]))
|
||||
t_c_g = np.array([0.03, -0.02, 0.15])
|
||||
T_c_g_true = make_T(R_c_g, t_c_g)
|
||||
|
||||
# 世界（=基座）到标定板的固定位姿：T_b^t
|
||||
R_b_t = exp_so3(np.deg2rad([0, 0, 0]))
|
||||
t_b_t = np.array([0.4, 0.0, 0.3])
|
||||
T_b_t = make_T(R_b_t, t_b_t)
|
||||
|
||||
robot_poses: List[np.ndarray] = [] # T_b^g
|
||||
camera_poses: List[np.ndarray] = [] # T_c^t
|
||||
|
||||
for _ in range(num_poses):
|
||||
# 生成多轴、较大幅度的运动（避免退化）
|
||||
ang = np.deg2rad(rng.uniform([-30, -30, -30], [30, 30, 30]))
|
||||
# 让 SO(3) 采样更丰富：
|
||||
R_b_g = (np.eye(3) @ rot_perturb(ang))
|
||||
t_b_g = rng.uniform([-0.2, -0.2, 0.3], [0.2, 0.2, 0.6])
|
||||
T_b_g = make_T(R_b_g, t_b_g)
|
||||
robot_poses.append(T_b_g)
|
||||
|
||||
# 相机位姿：T_b^c = T_b^g * T_g^c，其中 T_g^c = (T_c^g)^{-1}
|
||||
T_g_c = np.linalg.inv(T_c_g_true)
|
||||
T_b_c = T_b_g @ T_g_c
|
||||
|
||||
# 目标在相机坐标系：T_c^t = (T_b^c)^{-1} * T_b^t
|
||||
T_c_t = np.linalg.inv(T_b_c) @ T_b_t
|
||||
camera_poses.append(T_c_t)
|
||||
|
||||
return robot_poses, camera_poses
|
||||
|
||||
|
||||
def rot_perturb(ang_vec: np.ndarray) -> np.ndarray:
|
||||
"""给定欧拉角近似（rad）的向量，输出旋转矩阵（用罗德里格公式按轴角）。"""
|
||||
axis = ang_vec
|
||||
th = np.linalg.norm(axis)
|
||||
if th < 1e-12:
|
||||
return np.eye(3)
|
||||
k = axis / th
|
||||
K = np.array([[0, -k[2], k[1]], [k[2], 0, -k[0]], [-k[1], k[0], 0]])
|
||||
return np.eye(3) + np.sin(th) * K + (1 - np.cos(th)) * (K @ K)
|
||||
|
||||
|
||||
def to_json_SE3(T: np.ndarray) -> dict:
|
||||
R = T[:3, :3]
|
||||
t = T[:3, 3]
|
||||
q = rot_to_quat(R)
|
||||
return {
|
||||
"matrix": T.tolist(),
|
||||
"rotation_matrix": R.tolist(),
|
||||
"translation": t.tolist(),
|
||||
"quaternion_xyzw": q.tolist(),
|
||||
}
|
||||
|
||||
|
||||
def load_se3_matrix(path: str) -> np.ndarray:
|
||||
"""加载 4x4 齐次矩阵。支持 .npy / .npz(json-like)/.json。"""
|
||||
if not os.path.exists(path):
|
||||
raise FileNotFoundError(path)
|
||||
ext = os.path.splitext(path)[1].lower()
|
||||
if ext == ".npy":
|
||||
M = np.load(path)
|
||||
elif ext == ".npz":
|
||||
d = np.load(path)
|
||||
key = 'matrix' if 'matrix' in d.files else d.files[0]
|
||||
M = d[key]
|
||||
elif ext == ".json":
|
||||
with open(path, 'r') as f:
|
||||
data = json.load(f)
|
||||
M = np.array(data.get('matrix', data))
|
||||
else:
|
||||
raise ValueError(f"不支持的文件类型: {ext}")
|
||||
M = np.array(M)
|
||||
if M.shape != (4, 4):
|
||||
raise ValueError(f"矩阵形状应为(4,4)，实际为{M.shape}")
|
||||
return M
|
||||
|
||||
|
||||
def load_camera_calib(path: str) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""加载相机内参，支持 OpenCV YAML/JSON（cameraMatrix, distCoeffs）或简单 JSON：{"K":[], "D":[]}。"""
|
||||
import yaml
|
||||
with open(path, 'r') as f:
|
||||
txt = f.read()
|
||||
data = None
|
||||
try:
|
||||
data = yaml.safe_load(txt)
|
||||
except Exception:
|
||||
try:
|
||||
data = json.loads(txt)
|
||||
except Exception:
|
||||
raise ValueError("无法解析相机标定文件，支持 YAML/JSON，包含 camera_matrix/cameraMatrix/K 和 dist_coeffs/distCoeffs/D 字段")
|
||||
|
||||
# 支持多种键名
|
||||
K = data.get('camera_matrix') or data.get('cameraMatrix') or data.get('K')
|
||||
if isinstance(K, dict) and 'data' in K:
|
||||
K = K['data']
|
||||
D = data.get('dist_coeffs') or data.get('distCoeffs') or data.get('D')
|
||||
if isinstance(D, dict) and 'data' in D:
|
||||
D = D['data']
|
||||
if K is None or D is None:
|
||||
raise ValueError("标定文件需包含 camera_matrix/cameraMatrix/K 与 dist_coeffs/distCoeffs/D")
|
||||
K = np.array(K, dtype=float).reshape(3, 3)
|
||||
D = np.array(D, dtype=float).reshape(-1, 1)
|
||||
return K, D
|
||||
|
||||
|
||||
def build_chessboard_object_points(rows: int, cols: int, square: float) -> np.ndarray:
|
||||
"""生成棋盘格 3D 角点（单位：米），Z=0 平面，原点在棋盘左上角角点。"""
|
||||
objp = np.zeros((rows * cols, 3), np.float32)
|
||||
grid = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
|
||||
objp[:, :2] = grid * square
|
||||
return objp
|
||||
|
||||
|
||||
def detect_chessboard(gray: np.ndarray, rows: int, cols: int) -> Tuple[bool, np.ndarray]:
|
||||
flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
ok, corners = cv2.findChessboardCorners(gray, (cols, rows), flags)
|
||||
if not ok:
|
||||
return False, None
|
||||
# 亚像素优化
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
|
||||
return True, corners.reshape(-1, 2)
|
||||
|
||||
|
||||
def detect_charuco(gray: np.ndarray, rows: int, cols: int, square: float, marker: float, dict_name: str = 'DICT_4X4_50') -> Tuple[bool, np.ndarray, np.ndarray]:
|
||||
"""检测 Charuco 角点，返回 (ok, charuco_corners, charuco_ids)。需要 opencv-contrib-python。"""
|
||||
if not hasattr(cv2, 'aruco'):
|
||||
raise RuntimeError("需要 opencv-contrib-python 才能使用 Charuco。请安装 opencv-contrib-python==4.11.0.86")
|
||||
aruco = cv2.aruco
|
||||
dictionary = getattr(aruco, dict_name)
|
||||
aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, dict_name))
|
||||
board = aruco.CharucoBoard((cols, rows), square, marker, aruco_dict)
|
||||
params = aruco.DetectorParameters()
|
||||
detector = aruco.ArucoDetector(aruco_dict, params)
|
||||
corners, ids, _ = detector.detectMarkers(gray)
|
||||
if ids is None or len(ids) == 0:
|
||||
return False, None, None
|
||||
retval, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
|
||||
ok = retval is not None and retval >= 4
|
||||
return ok, (charuco_corners.reshape(-1, 2) if ok else None), (charuco_ids if ok else None)
|
||||
|
||||
|
||||
def solve_pnp_from_image(image_path: str, K: np.ndarray, D: np.ndarray,
|
||||
pattern: str, rows: int, cols: int, square: float,
|
||||
charuco_marker: float = None) -> np.ndarray:
|
||||
img = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
|
||||
if img is None:
|
||||
raise FileNotFoundError(f"无法读取图像: {image_path}")
|
||||
if pattern == 'chessboard':
|
||||
ok, corners = detect_chessboard(img, rows, cols)
|
||||
if not ok:
|
||||
raise RuntimeError(f"棋盘检测失败: {image_path}")
|
||||
objp = build_chessboard_object_points(rows, cols, square)
|
||||
# PnP
|
||||
retval, rvec, tvec = cv2.solvePnP(objp, corners, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
|
||||
if not retval:
|
||||
raise RuntimeError(f"solvePnP 失败: {image_path}")
|
||||
return create_homogeneous_matrix(rvec, tvec)
|
||||
elif pattern == 'charuco':
|
||||
if charuco_marker is None:
|
||||
raise ValueError("使用 charuco 时必须提供 --charuco-marker（米）")
|
||||
ok, charuco_corners, charuco_ids = detect_charuco(img, rows, cols, square, charuco_marker)
|
||||
if not ok:
|
||||
raise RuntimeError(f"Charuco 检测失败: {image_path}")
|
||||
# 从 board 索引构建 3D 角点
|
||||
aruco = cv2.aruco
|
||||
aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, 'DICT_4X4_50'))
|
||||
board = aruco.CharucoBoard((cols, rows), square, charuco_marker, aruco_dict)
|
||||
obj_pts = []
|
||||
img_pts = []
|
||||
for idx, corner in zip(charuco_ids.flatten(), charuco_corners.reshape(-1, 2)):
|
||||
obj_pts.append(board.getChessboardCorners()[idx][0])
|
||||
img_pts.append(corner)
|
||||
obj_pts = np.array(obj_pts, dtype=np.float32)
|
||||
img_pts = np.array(img_pts, dtype=np.float32)
|
||||
retval, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
|
||||
if not retval:
|
||||
raise RuntimeError(f"solvePnP 失败: {image_path}")
|
||||
return create_homogeneous_matrix(rvec, tvec)
|
||||
else:
|
||||
raise ValueError("pattern 仅支持 chessboard 或 charuco")
|
||||
|
||||
|
||||
def load_robot_poses_file(path: str) -> List[np.ndarray]:
|
||||
"""从文件加载 robot_poses 列表。支持 .npz(robot_poses), .json(list or dict.matrix), .csv( image, tx,ty,tz,qx,qy,qz,qw )。"""
|
||||
ext = os.path.splitext(path)[1].lower()
|
||||
if ext == '.npz':
|
||||
d = np.load(path)
|
||||
arr = d['robot_poses']
|
||||
assert arr.ndim == 3 and arr.shape[1:] == (4, 4)
|
||||
return [arr[i] for i in range(arr.shape[0])]
|
||||
if ext == '.json':
|
||||
with open(path, 'r') as f:
|
||||
data = json.load(f)
|
||||
if isinstance(data, dict) and 'robot_poses' in data:
|
||||
mats = data['robot_poses']
|
||||
else:
|
||||
mats = data
|
||||
return [np.array(M).reshape(4, 4) for M in mats]
|
||||
if ext == '.csv':
|
||||
poses = []
|
||||
with open(path, 'r') as f:
|
||||
reader = csv.DictReader(f)
|
||||
for row in reader:
|
||||
tx, ty, tz = float(row['tx']), float(row['ty']), float(row['tz'])
|
||||
qx, qy, qz, qw = float(row['qx']), float(row['qy']), float(row['qz']), float(row['qw'])
|
||||
R = quat_to_rot(np.array([qx, qy, qz, qw]))
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R
|
||||
T[:3, 3] = np.array([tx, ty, tz])
|
||||
poses.append(T)
|
||||
return poses
|
||||
raise ValueError(f"不支持的 robot poses 文件类型: {ext}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Hand-Eye calibration (eye-in-hand / eye-to-hand)")
|
||||
parser.add_argument("--data", type=str, default=None,
|
||||
help=".npz dataset with arrays 'robot_poses' and 'camera_poses' of shape (N,4,4)")
|
||||
parser.add_argument("--mode", type=str, default=None, choices=["eye_in_hand", "eye_to_hand"],
|
||||
help="Calibration mode. If omitted, both modes are computed")
|
||||
parser.add_argument("--out", type=str, default=None, help="Output JSON file for the calibration result")
|
||||
parser.add_argument("--export-dataset", type=str, default=None,
|
||||
help="Export the dataset used (robot_poses,camera_poses) to .npz at this path")
|
||||
parser.add_argument("--seed", type=int, default=42, help="Synthetic dataset RNG seed when no --data is provided")
|
||||
parser.add_argument("--num-poses", type=int, default=12, help="Synthetic dataset pose count when no --data is provided")
|
||||
parser.add_argument("--eval", action="store_true", help="Compute simple AX≈XB residual metrics (eye-in-hand)")
|
||||
# Image-based camera pose estimation
|
||||
parser.add_argument("--images", type=str, default=None,
|
||||
help="Directory or glob for images captured by the depth camera (color/IR). Sorted alphanumerically.")
|
||||
parser.add_argument("--camera-calib", type=str, default=None, help="Camera intrinsic file (YAML/JSON)")
|
||||
parser.add_argument("--pattern", type=str, default="chessboard", choices=["chessboard", "charuco"], help="Calibration board pattern")
|
||||
parser.add_argument("--board-rows", type=int, default=6, help="Inner rows of chessboard/charuco")
|
||||
parser.add_argument("--board-cols", type=int, default=9, help="Inner cols of chessboard/charuco")
|
||||
parser.add_argument("--square-size", type=float, default=0.024, help="Square size in meters")
|
||||
parser.add_argument("--charuco-marker", type=float, default=None, help="Charuco marker size in meters (for charuco only)")
|
||||
parser.add_argument("--robot-poses-file", type=str, default=None, help="Path to robot poses file (.npz/.json/.csv)")
|
||||
# Frames handling
|
||||
parser.add_argument("--robot-poses-frame", type=str, default="robot_base", choices=["robot_base", "arm_base"],
|
||||
help="Frame of robot_poses in the dataset: robot_base (default) or arm_base")
|
||||
parser.add_argument("--arm-to-robot-base", type=str, default=None,
|
||||
help="Optional path to 4x4 matrix of T_robot_base^arm_base (compose robot poses to robot base)")
|
||||
args = parser.parse_args()
|
||||
|
||||
# Load or generate dataset
|
||||
if args.data:
|
||||
d = np.load(args.data)
|
||||
robot_arr = d["robot_poses"]
|
||||
cam_arr = d["camera_poses"]
|
||||
assert robot_arr.ndim == 3 and robot_arr.shape[1:] == (4, 4)
|
||||
assert cam_arr.ndim == 3 and cam_arr.shape[1:] == (4, 4)
|
||||
robot_poses = [robot_arr[i] for i in range(robot_arr.shape[0])]
|
||||
camera_poses = [cam_arr[i] for i in range(cam_arr.shape[0])]
|
||||
print(f"已从 {args.data} 加载数据集，共 {len(robot_poses)} 组")
|
||||
elif args.images and args.robot_poses_file and args.camera_calib:
|
||||
# Build dataset from images + robot poses
|
||||
# 1) images
|
||||
if os.path.isdir(args.images):
|
||||
img_paths = sorted(glob.glob(os.path.join(args.images, '*')))
|
||||
else:
|
||||
img_paths = sorted(glob.glob(args.images))
|
||||
if not img_paths:
|
||||
raise FileNotFoundError(f"未找到图像: {args.images}")
|
||||
# 2) robot poses
|
||||
robot_poses = load_robot_poses_file(args.robot_poses_file)
|
||||
if len(robot_poses) != len(img_paths):
|
||||
raise ValueError(f"robot poses 数量({len(robot_poses)})与图像数量({len(img_paths)})不一致。请确保一一对应且顺序匹配。")
|
||||
# 3) camera intrinsics
|
||||
K, D = load_camera_calib(args.camera_calib)
|
||||
# 4) detect and solvePnP
|
||||
camera_poses = []
|
||||
for img in img_paths:
|
||||
T_c_t = solve_pnp_from_image(img, K, D, args.pattern, args.board_rows, args.board_cols, args.square_size, args.charuco_marker)
|
||||
camera_poses.append(T_c_t)
|
||||
print(f"已从图像与位姿构建数据集，共 {len(camera_poses)} 组")
|
||||
else:
|
||||
robot_poses, camera_poses = generate_synthetic_dataset(num_poses=args.num_poses, seed=args.seed)
|
||||
print(f"生成了 {len(robot_poses)} 组模拟位姿数据 (seed={args.seed})")
|
||||
|
||||
# Optional export of dataset for reproducibility
|
||||
if args.export_dataset:
|
||||
np.savez(args.export_dataset,
|
||||
robot_poses=np.stack(robot_poses, axis=0),
|
||||
camera_poses=np.stack(camera_poses, axis=0))
|
||||
print(f"已导出数据集到: {args.export_dataset}")
|
||||
|
||||
# Frame composition: if robot_poses are in arm_base, optionally compose to robot_base
|
||||
reference_base = "robot_base"
|
||||
if args.robot_poses_frame == "arm_base":
|
||||
reference_base = "arm_base"
|
||||
if args.arm_to_robot_base:
|
||||
T_rb_ab = load_se3_matrix(args.arm_to_robot_base) # T_robot_base^arm_base
|
||||
robot_poses = [T_rb_ab @ T_ab_g for T_ab_g in robot_poses]
|
||||
reference_base = "robot_base"
|
||||
print("已将 arm_base 框架下的末端位姿转换到 robot_base 框架")
|
||||
|
||||
def run_mode(mode_name: str) -> np.ndarray:
|
||||
print(f"\n=== 标定模式: {mode_name} ===")
|
||||
X = hand_eye_calibration(robot_poses, camera_poses, mode=mode_name)
|
||||
print("标定结果 X:")
|
||||
print(X)
|
||||
if mode_name == 'eye_to_hand':
|
||||
print(f"参考基座: {reference_base}")
|
||||
print(f"平移部分: [{X[0, 3]:.4f}, {X[1, 3]:.4f}, {X[2, 3]:.4f}]")
|
||||
return X
|
||||
|
||||
results = {}
|
||||
try:
|
||||
if args.mode:
|
||||
X = run_mode(args.mode)
|
||||
results[args.mode] = X
|
||||
else:
|
||||
X_eih = run_mode('eye_in_hand')
|
||||
X_eth = run_mode('eye_to_hand')
|
||||
results['eye_in_hand'] = X_eih
|
||||
results['eye_to_hand'] = X_eth
|
||||
|
||||
if args.eval:
|
||||
# 仅计算 eye-in-hand 残差（需要 X_cam_gripper），若未跑则先跑一次
|
||||
if 'eye_in_hand' in results:
|
||||
X_cam_gripper = results['eye_in_hand']
|
||||
else:
|
||||
X_cam_gripper = hand_eye_calibration(robot_poses, camera_poses, mode='eye_in_hand')
|
||||
rot_err_deg, trans_err = hand_eye_residual(robot_poses, camera_poses, X_cam_gripper)
|
||||
print(f"残差评估：旋转均值 {rot_err_deg:.3f} 度，平移均值 {trans_err:.4f} m")
|
||||
|
||||
if args.out:
|
||||
# 若选择了特定模式，写该模式；否则写两者
|
||||
payload = {"metadata": {"reference_base": reference_base,
|
||||
"robot_poses_frame": args.robot_poses_frame,
|
||||
"composed_with_arm_to_robot_base": bool(args.arm_to_robot_base)}}
|
||||
if args.mode:
|
||||
payload[args.mode] = to_json_SE3(results[args.mode])
|
||||
else:
|
||||
payload.update({k: to_json_SE3(v) for k, v in results.items()})
|
||||
with open(args.out, 'w') as f:
|
||||
json.dump(payload, f, indent=2)
|
||||
print(f"已保存标定结果到: {args.out}")
|
||||
|
||||
except Exception as e:
|
||||
print(f"标定过程中发生错误: {e}")
|
||||
|
||||
# ==================== 实际应用提示 ====================
|
||||
if not args.data:
|
||||
print("\n=== 实际应用说明 ===")
|
||||
print("1. 从机器人控制器读取末端位姿 (通常是4x4齐次矩阵或位姿向量，注意这是 基座->末端 T_b^g)")
|
||||
print("2. 使用 OpenCV solvePnP 从标定板图像解算 T_c^t (标定板到相机):")
|
||||
print(" retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)")
|
||||
print(" camera_pose = create_homogeneous_matrix(rvec, tvec) # 即 T_c^t")
|
||||
print("3. 收集多组对应的 T_b^g 和 T_c^t（包含多轴较大旋转，>= 8-12 组）")
|
||||
print("4. 运行本脚本 --data dataset.npz --mode eye_to_hand --out result.json --eval")
|
||||
print("\n若 robot_poses 在 arm_base 框架下：")
|
||||
print("- 如无 arm_base->robot_base 变换，脚本将输出 arm_base^T_camera（eye_to_hand）")
|
||||
print("- 若提供 --arm-to-robot-base T_robot_base_from_arm_base.npy，将自动转换并输出 robot_base^T_camera")
|
||||
@@ -1,224 +0,0 @@
|
||||
# 手眼标定（Eye-in-Hand / Eye-to-Hand）操作指南
|
||||
|
||||
本指南配合脚本 `src/scripts/hand_eye_cali.py` 使用，完成相机与机器人之间的外参（手眼）标定。脚本基于 OpenCV 的 `calibrateHandEye` 实现，支持“眼在手上 (eye-in-hand)”与“眼在手外 (eye-to-hand)”两种模式。
|
||||
|
||||
---
|
||||
|
||||
## 一、原理与坐标定义
|
||||
|
||||
- 机器人末端在基座坐标系下的位姿：`T_b^g`（4x4 齐次矩阵）
|
||||
- 标定板在相机坐标系下的位姿：`T_c^t`（4x4 齐次矩阵），可由 `solvePnP` 得到
|
||||
- 需求目标：
|
||||
- 眼在手上 (eye-in-hand)：求 `T_c^g`（相机在末端坐标系下的位姿）
|
||||
- 眼在手外 (eye-to-hand)：求 `T_b^c`（相机在基座坐标系下的位姿）
|
||||
|
||||
OpenCV `calibrateHandEye` 的输入为每一帧的“绝对位姿”：
|
||||
- `R_gripper2base, t_gripper2base` 对应 `T_g^b`（由 `T_b^g` 求逆获得）
|
||||
- `R_target2cam, t_target2cam` 对应 `T_t^c`（`T_c^t` 的旋转和平移部分）
|
||||
|
||||
---
|
||||
|
||||
## 二、前置条件
|
||||
|
||||
- 已有相机的内参（fx, fy, cx, cy, 畸变等），并能在图像中稳定检测到标定板角点或 AprilTag/ArUco 标记
|
||||
- 能够从机器人系统读取末端在基座下的位姿 `T_b^g`
|
||||
- 至少采集 8–12 组数据，包含多轴、较大幅度旋转和平移（信息量不足会导致求解失败或误差大）
|
||||
|
||||
---
|
||||
|
||||
## 三、数据采集
|
||||
|
||||
1. 固定标定板（推荐刚性固定在稳定位置），或在“眼在手上”场景中保持相机跟随末端运动
|
||||
2. 对每一帧：
|
||||
- 记录机器人末端位姿 `T_b^g`（4x4）
|
||||
- 在对应图像中用 `solvePnP` 算法求解 `T_c^t`（4x4）
|
||||
3. 采集方式一：直接保存数据集 `.npz`
|
||||
- 数组名必须是 `robot_poses` 和 `camera_poses`
|
||||
- 形状为 `(N, 4, 4)`
|
||||
|
||||
数据格式示例（Python 生成）：
|
||||
```python
|
||||
np.savez('dataset.npz',
|
||||
robot_poses=np.stack(robot_pose_list, axis=0),
|
||||
camera_poses=np.stack(camera_pose_list, axis=0))
|
||||
```
|
||||
|
||||
4. 采集方式二：仅保存图像与机器人位姿
|
||||
- 保存 N 张图像（彩色或红外），用于检测棋盘/Charuco 角点
|
||||
- 同时保存 N 条机器人末端位姿 `T_b^g`（或 `T_ab^g`）到 `.csv/.json/.npz`
|
||||
- 运行时脚本会：
|
||||
1) 从相机内参文件（YAML/JSON）读入 `K,D`
|
||||
2) 对每张图像用 `solvePnP` 求解 `T_c^t`
|
||||
3) 组合成数据集并求解手眼外参
|
||||
|
||||
> 仅有机械臂基座（arm_base）的位姿可以吗？可以。
|
||||
> - 如果记录的是 `T_ab^g`（arm_base->末端），也能完成手眼标定。
|
||||
> - `eye_to_hand` 模式下，脚本将输出 `arm_base^T_camera`。若 `T_rb^ab`（robot_base->arm_base），可传给脚本自动合成 `robot_base^T_camera`。
|
||||
|
||||
|
||||
## 四、脚本使用方法
|
||||
|
||||
脚本路径：`src/scripts/hand_eye_cali.py`
|
||||
|
||||
- 计算两种模式并导出 JSON：
|
||||
```bash
|
||||
python src/scripts/hand_eye_cali.py \
|
||||
--data dataset.npz \
|
||||
--out handeye_result.json \
|
||||
--eval
|
||||
```
|
||||
|
||||
- 仅计算眼在手外 (eye-to-hand)：
|
||||
```bash
|
||||
python src/scripts/hand_eye_cali.py \
|
||||
--data dataset.npz \
|
||||
--mode eye_to_hand \
|
||||
--out handeye_result.json
|
||||
```
|
||||
|
||||
- 没有数据时，可生成模拟数据用于测试：
|
||||
```bash
|
||||
python src/scripts/hand_eye_cali.py \
|
||||
--num-poses 12 --eval \
|
||||
--out /tmp/handeye_result.json \
|
||||
--export-dataset /tmp/handeye_dataset.npz
|
||||
```
|
||||
|
||||
### 仅用图像 + 末端位姿进行标定
|
||||
|
||||
前提：准备好相机标定文件（YAML/JSON，包含 `cameraMatrix`/`camera_matrix`/`K` 和 `distCoeffs`/`dist_coeffs`/`D`）。
|
||||
|
||||
- 棋盘格（默认 6x9 内角点，格长 0.024 m）：
|
||||
```bash
|
||||
python src/scripts/hand_eye_cali.py \
|
||||
--images data/images/*.png \
|
||||
--camera-calib data/camera.yaml \
|
||||
--robot-poses-file data/robot_poses.csv \
|
||||
--pattern chessboard --board-rows 6 --board-cols 9 --square-size 0.024 \
|
||||
--robot-poses-frame arm_base \
|
||||
--mode eye_to_hand \
|
||||
--out handeye_result.json --eval
|
||||
```
|
||||
|
||||
- Charuco（需要 opencv-contrib-python）：
|
||||
```bash
|
||||
python src/scripts/hand_eye_cali.py \
|
||||
--images data/images/*.png \
|
||||
--camera-calib data/camera.yaml \
|
||||
--robot-poses-file data/robot_poses.csv \
|
||||
--pattern charuco --board-rows 6 --board-cols 9 --square-size 0.024 --charuco-marker 0.016 \
|
||||
--robot-poses-frame robot_base \
|
||||
--mode eye_to_hand \
|
||||
--out handeye_result.json --eval
|
||||
```
|
||||
|
||||
robot_poses.csv 示例（头部为列名，按行对齐图像顺序）：
|
||||
```csv
|
||||
image,tx,ty,tz,qx,qy,qz,qw
|
||||
0001.png,0.10,0.00,0.45,0.00,0.00,0.00,1.00
|
||||
0002.png,0.10,0.05,0.45,0.05,0.00,0.00,0.9987
|
||||
...
|
||||
```
|
||||
|
||||
相机 YAML 示例（可兼容 OpenCV 格式）：
|
||||
```yaml
|
||||
camera_matrix: {data: [fx, 0, cx, 0, fy, cy, 0, 0, 1]}
|
||||
dist_coeffs: {data: [k1, k2, p1, p2, k3]}
|
||||
```
|
||||
|
||||
### 参数说明
|
||||
- `--data`: 读取 `.npz` 数据集（包含 `robot_poses`、`camera_poses`）
|
||||
- `--mode`: `eye_in_hand` 或 `eye_to_hand`（缺省则两者都算）
|
||||
- `--out`: 输出 JSON 路径（包含 4x4 矩阵、旋转矩阵、平移、四元数）
|
||||
- `--eval`: 计算 AX≈XB 的简单残差（眼在手上），用于快速自检
|
||||
- `--export-dataset`: 将当前使用的数据集导出（便于复现）
|
||||
- `--num-poses`, `--seed`: 生成模拟数据的数量与种子（无 `--data` 时生效）
|
||||
- `--robot-poses-frame`: `robot_base`（默认）或 `arm_base`，用于指明 `robot_poses` 的基座框架
|
||||
- `--arm-to-robot-base`: `T_robot_base^arm_base` 的 4x4 矩阵文件路径（.npy/.npz/.json），若提供则在内部进行 `T_b^g = T_rb^ab @ T_ab^g` 组合
|
||||
-. `--images`: 图像目录或通配（按文件名排序）
|
||||
-. `--camera-calib`: 相机内参文件（YAML/JSON）
|
||||
-. `--pattern`: `chessboard` 或 `charuco`
|
||||
-. `--board-rows/--board-cols/--square-size`: 标定板参数（单位：米）
|
||||
-. `--charuco-marker`: Charuco 的方块内 marker 尺寸（米）
|
||||
-. `--robot-poses-file`: 末端位姿文件（.csv/.json/.npz），与图像一一对应
|
||||
|
||||
---
|
||||
|
||||
## 五、结果解释与落地
|
||||
|
||||
- 脚本输出 JSON 中字段：
|
||||
- `matrix`: 4x4 齐次矩阵（行主序）
|
||||
- `rotation_matrix`: 3x3 旋转矩阵
|
||||
- `translation`: 3 维平移 `[x, y, z]`（单位：米）
|
||||
- `quaternion_xyzw`: 四元数 `[x, y, z, w]`
|
||||
|
||||
- 眼在手外结果：
|
||||
- 若 `--robot-poses-frame robot_base` 或提供了 `--arm-to-robot-base`，则输出为 `robot_base^T_camera`
|
||||
- 若 `--robot-poses-frame arm_base` 且未提供 arm->robot 变换，则输出为 `arm_base^T_camera`
|
||||
- 发布 TF 示例：
|
||||
```bash
|
||||
ros2 run tf2_ros static_transform_publisher \
|
||||
x y z qx qy qz qw \
|
||||
base_link camera_link
|
||||
```
|
||||
将 `x y z qx qy qz qw` 替换为 JSON 中的 `translation` 与 `quaternion_xyzw`，帧名根据机器人而定（如 `base_link` 与 `camera_link`）。
|
||||
|
||||
- 眼在手上（`camera^T_end_effector`）则在末端坐标系下描述相机位姿，常用于抓取/视觉伺服求解。
|
||||
|
||||
---
|
||||
|
||||
## 六、常见问题与排查
|
||||
|
||||
- 报错“Not enough informative motions”或残差很大：
|
||||
- 增加数据数量（>=12）
|
||||
- 扩大旋转幅度，覆盖多个轴，避免纯平移或单轴小角度
|
||||
- 确认 `T_b^g` 与 `T_c^t` 的定义方向正确（基座->末端、标定板->相机）
|
||||
- `solvePnP` 不稳定/跳变：
|
||||
- 使用更鲁棒的标定板（AprilTag/Charuco）
|
||||
- 固定/稳定曝光，提升角点检测质量
|
||||
- 确认相机内参/畸变准确
|
||||
- 结果帧名/方向不符合期望：
|
||||
- 仔细对照：脚本的 `eye_to_hand` 输出的是 `base^T_camera`，`eye_in_hand` 输出的是 `camera^T_end_effector`
|
||||
- 若需要 `end_effector^T_camera`，取逆即可
|
||||
|
||||
---
|
||||
|
||||
## 七、建议采集策略
|
||||
|
||||
- 首先让末端在 3 个轴上分别做正/反方向旋转，各配合一定平移
|
||||
- 保证每一帧采集时 `T_b^g` 与 `T_c^t` 时间匹配（尽量同步）
|
||||
- 目标板尽可能占据成像区域较大比例，避免深远距离下的姿态估计不稳定
|
||||
|
||||
---
|
||||
|
||||
## 八、附录：数据制作参考
|
||||
|
||||
使用 `solvePnP` 生成 `T_c^t`：
|
||||
```python
|
||||
# objectPoints: (N,3) mm 或 m；imagePoints: (N,2) 像素
|
||||
retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)
|
||||
T_c_t = create_homogeneous_matrix(rvec, tvec)
|
||||
```
|
||||
|
||||
保存数据集：
|
||||
```python
|
||||
np.savez('dataset.npz',
|
||||
robot_poses=np.stack(robot_pose_list, axis=0),
|
||||
camera_poses=np.stack(camera_pose_list, axis=0))
|
||||
```
|
||||
|
||||
若 `robot_poses` 是以机械臂基座 `arm_base` 为基准，且已知 `T_robot_base^arm_base`，可以在运行时提供：
|
||||
```bash
|
||||
python src/scripts/hand_eye_cali.py \
|
||||
--data dataset.npz \
|
||||
--robot-poses-frame arm_base \
|
||||
--arm-to-robot-base T_robot_base_from_arm_base.npy \
|
||||
--mode eye_to_hand \
|
||||
--out handeye_result.json
|
||||
```
|
||||
|
||||
---
|
||||
## 九、数据处理脚本
|
||||
|
||||
提供一个模板脚本，读取机器人驱动与视觉检测的实时话题，自动同步采样并生成 dataset.npz
|
||||
将手眼标定结果直接写为 ROS2 的 static_transform_publisher 配置或 URDF 片段，便于一键加载
|
||||
@@ -1,207 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Detect calibration board in images, estimate camera pose via solvePnP,
|
||||
and output quaternions (qx,qy,qz,qw) for up to N images.
|
||||
|
||||
Supports:
|
||||
- chessboard or Charuco detection
|
||||
- YAML/JSON camera intrinsics (OpenCV-style keys)
|
||||
- Save to CSV and print to stdout
|
||||
|
||||
Example:
|
||||
python src/scripts/images_to_quaternions.py \
|
||||
--images data/images/*.png \
|
||||
--camera-calib data/camera.yaml \
|
||||
--pattern chessboard --board-rows 6 --board-cols 9 --square-size 0.024 \
|
||||
--limit 12 --out /tmp/quats.csv
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import glob
|
||||
import json
|
||||
import os
|
||||
from typing import Tuple, List
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
def load_camera_calib(path: str) -> Tuple[np.ndarray, np.ndarray]:
|
||||
import yaml
|
||||
with open(path, 'r') as f:
|
||||
txt = f.read()
|
||||
try:
|
||||
data = yaml.safe_load(txt)
|
||||
except Exception:
|
||||
data = json.loads(txt)
|
||||
K = data.get('camera_matrix') or data.get('cameraMatrix') or data.get('K')
|
||||
if isinstance(K, dict) and 'data' in K:
|
||||
K = K['data']
|
||||
D = data.get('dist_coeffs') or data.get('distCoeffs') or data.get('D')
|
||||
if isinstance(D, dict) and 'data' in D:
|
||||
D = D['data']
|
||||
if K is None or D is None:
|
||||
raise ValueError('Calibration must contain camera_matrix/cameraMatrix/K and dist_coeffs/distCoeffs/D')
|
||||
K = np.array(K, dtype=float).reshape(3, 3)
|
||||
D = np.array(D, dtype=float).reshape(-1, 1)
|
||||
return K, D
|
||||
|
||||
|
||||
def build_chessboard_object_points(rows: int, cols: int, square: float) -> np.ndarray:
|
||||
objp = np.zeros((rows * cols, 3), np.float32)
|
||||
grid = np.mgrid[0:cols, 0:rows].T.reshape(-1, 2)
|
||||
objp[:, :2] = grid * square
|
||||
return objp
|
||||
|
||||
|
||||
def detect_chessboard(gray: np.ndarray, rows: int, cols: int) -> Tuple[bool, np.ndarray]:
|
||||
flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
ok, corners = cv2.findChessboardCorners(gray, (cols, rows), flags)
|
||||
if not ok:
|
||||
return False, None
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
|
||||
return True, corners.reshape(-1, 2)
|
||||
|
||||
|
||||
def detect_charuco(gray: np.ndarray, rows: int, cols: int, square: float, marker: float) -> Tuple[bool, np.ndarray, np.ndarray]:
|
||||
if not hasattr(cv2, 'aruco'):
|
||||
raise RuntimeError('opencv-contrib-python is required for Charuco detection')
|
||||
aruco = cv2.aruco
|
||||
aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, 'DICT_4X4_50'))
|
||||
board = aruco.CharucoBoard((cols, rows), square, marker, aruco_dict)
|
||||
params = aruco.DetectorParameters()
|
||||
detector = aruco.ArucoDetector(aruco_dict, params)
|
||||
corners, ids, _ = detector.detectMarkers(gray)
|
||||
if ids is None or len(ids) == 0:
|
||||
return False, None, None
|
||||
retval, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
|
||||
ok = retval is not None and retval >= 4
|
||||
return ok, (charuco_corners.reshape(-1, 2) if ok else None), (charuco_ids if ok else None)
|
||||
|
||||
|
||||
def rot_to_quat(R: np.ndarray) -> np.ndarray:
|
||||
tr = np.trace(R)
|
||||
if tr > 0.0:
|
||||
S = np.sqrt(tr + 1.0) * 2.0
|
||||
qw = 0.25 * S
|
||||
qx = (R[2, 1] - R[1, 2]) / S
|
||||
qy = (R[0, 2] - R[2, 0]) / S
|
||||
qz = (R[1, 0] - R[0, 1]) / S
|
||||
else:
|
||||
if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
|
||||
S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
|
||||
qx = 0.25 * S
|
||||
qy = (R[0, 1] + R[1, 0]) / S
|
||||
qz = (R[0, 2] + R[2, 0]) / S
|
||||
qw = (R[2, 1] - R[1, 2]) / S
|
||||
elif R[1, 1] > R[2, 2]:
|
||||
S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
|
||||
qx = (R[0, 1] + R[1, 0]) / S
|
||||
qy = 0.25 * S
|
||||
qz = (R[1, 2] + R[2, 1]) / S
|
||||
qw = (R[0, 2] - R[2, 0]) / S
|
||||
else:
|
||||
S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
|
||||
qx = (R[0, 2] + R[2, 0]) / S
|
||||
qy = (R[1, 2] + R[2, 1]) / S
|
||||
qz = 0.25 * S
|
||||
qw = (R[1, 0] - R[0, 1]) / S
|
||||
q = np.array([qx, qy, qz, qw])
|
||||
return q / (np.linalg.norm(q) + 1e-12)
|
||||
|
||||
|
||||
def pnp_quaternion_for_image(image_path: str, K: np.ndarray, D: np.ndarray,
|
||||
pattern: str, rows: int, cols: int, square: float,
|
||||
charuco_marker: float | None = None) -> Tuple[np.ndarray, np.ndarray]:
|
||||
gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
|
||||
if gray is None:
|
||||
raise FileNotFoundError(f'Cannot read image: {image_path}')
|
||||
if pattern == 'chessboard':
|
||||
ok, corners = detect_chessboard(gray, rows, cols)
|
||||
if not ok:
|
||||
raise RuntimeError(f'Chessboard detection failed: {image_path}')
|
||||
objp = build_chessboard_object_points(rows, cols, square)
|
||||
retval, rvec, tvec = cv2.solvePnP(objp, corners, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
|
||||
if not retval:
|
||||
raise RuntimeError(f'solvePnP failed: {image_path}')
|
||||
elif pattern == 'charuco':
|
||||
if charuco_marker is None:
|
||||
raise ValueError('charuco requires --charuco-marker')
|
||||
ok, charuco_corners, charuco_ids = detect_charuco(gray, rows, cols, square, charuco_marker)
|
||||
if not ok:
|
||||
raise RuntimeError(f'Charuco detection failed: {image_path}')
|
||||
aruco = cv2.aruco
|
||||
aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, 'DICT_4X4_50'))
|
||||
board = aruco.CharucoBoard((cols, rows), square, charuco_marker, aruco_dict)
|
||||
obj_pts = []
|
||||
img_pts = []
|
||||
for idx, corner in zip(charuco_ids.flatten(), charuco_corners.reshape(-1, 2)):
|
||||
obj_pts.append(board.getChessboardCorners()[idx][0])
|
||||
img_pts.append(corner)
|
||||
obj_pts = np.array(obj_pts, dtype=np.float32)
|
||||
img_pts = np.array(img_pts, dtype=np.float32)
|
||||
retval, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, K, D, flags=cv2.SOLVEPNP_ITERATIVE)
|
||||
if not retval:
|
||||
raise RuntimeError(f'solvePnP failed: {image_path}')
|
||||
else:
|
||||
raise ValueError('pattern must be chessboard or charuco')
|
||||
|
||||
R, _ = cv2.Rodrigues(rvec)
|
||||
q = rot_to_quat(R)
|
||||
return q, tvec.flatten()
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='Estimate quaternions from board detections in images (solvePnP).')
|
||||
parser.add_argument('--images', type=str, required=True, help='Directory or glob for images')
|
||||
parser.add_argument('--camera-calib', type=str, required=True, help='Camera intrinsics (YAML/JSON)')
|
||||
parser.add_argument('--pattern', type=str, default='chessboard', choices=['chessboard', 'charuco'])
|
||||
parser.add_argument('--board-rows', type=int, default=6)
|
||||
parser.add_argument('--board-cols', type=int, default=9)
|
||||
parser.add_argument('--square-size', type=float, default=0.024, help='meters')
|
||||
parser.add_argument('--charuco-marker', type=float, default=None, help='meters for Charuco marker size')
|
||||
parser.add_argument('--limit', type=int, default=12, help='Max number of images to process')
|
||||
parser.add_argument('--out', type=str, default=None, help='Output CSV path')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# Collect images
|
||||
if os.path.isdir(args.images):
|
||||
img_paths = sorted(glob.glob(os.path.join(args.images, '*')))
|
||||
else:
|
||||
img_paths = sorted(glob.glob(args.images))
|
||||
if not img_paths:
|
||||
raise FileNotFoundError(f'No images found: {args.images}')
|
||||
img_paths = img_paths[: args.limit]
|
||||
|
||||
# Load intrinsics
|
||||
K, D = load_camera_calib(args.camera_calib)
|
||||
|
||||
# Process
|
||||
results: List[Tuple[str, np.ndarray]] = []
|
||||
for p in img_paths:
|
||||
try:
|
||||
q, t = pnp_quaternion_for_image(
|
||||
p, K, D,
|
||||
args.pattern, args.board_rows, args.board_cols, args.square_size,
|
||||
args.charuco_marker,
|
||||
)
|
||||
results.append((p, q))
|
||||
print(f'{os.path.basename(p)}: qx={q[0]:.8f}, qy={q[1]:.8f}, qz={q[2]:.8f}, qw={q[3]:.8f}')
|
||||
except Exception as e:
|
||||
print(f'WARN: {p}: {e}')
|
||||
|
||||
if args.out:
|
||||
import csv
|
||||
with open(args.out, 'w', newline='') as f:
|
||||
writer = csv.writer(f)
|
||||
writer.writerow(['image', 'qx', 'qy', 'qz', 'qw'])
|
||||
for p, q in results:
|
||||
writer.writerow([os.path.basename(p), f'{q[0]:.8f}', f'{q[1]:.8f}', f'{q[2]:.8f}', f'{q[3]:.8f}'])
|
||||
print(f'Written CSV: {args.out}')
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,115 +0,0 @@
|
||||
# 姿态到四元数转换说明
|
||||
|
||||
本文档说明如何将姿态表示（欧拉角与旋转向量 Rodrigues/Axis-Angle）转换为四元数 (qx, qy, qz, qw)，包含原理、公式、边界情况与一个完整的计算示例，并给出现有脚本 `src/scripts/euler_to_quaternion.py` 的使用方法。
|
||||
|
||||
## 基本约定
|
||||
- 四元数记为 q = [x, y, z, w]，其中 w 为实部，(x, y, z) 为虚部（与 ROS、OpenCV 常见约定一致）。
|
||||
- 旋转向量 r = (rx, ry, rz) 的方向为旋转轴，模长 |r| 为旋转角（弧度）。
|
||||
- 欧拉角使用“内在旋转”约定，支持顺序 xyz（默认）与 zyx。
|
||||
|
||||
---
|
||||
|
||||
## 1) 旋转向量 (Rodrigues) → 四元数
|
||||
给定旋转向量 r = (rx, ry, rz)：
|
||||
1. 计算旋转角与单位旋转轴：
|
||||
- 角度：θ = |r| = sqrt(rx^2 + ry^2 + rz^2)
|
||||
- 轴：u = r / θ = (ux, uy, uz)
|
||||
2. 由轴角到四元数：
|
||||
- s = sin(θ/2), c = cos(θ/2)
|
||||
- q = (ux·s, uy·s, uz·s, c)
|
||||
3. 数值稳定性：若 θ 非常小（例如 < 1e-12），可近似认为 q ≈ (0, 0, 0, 1)。
|
||||
4. 归一化：实际实现中会做一次单位化 q ← q / ||q||，以避免数值误差。
|
||||
|
||||
> 注：四元数 q 与 -q 表示同一旋转（双覆表示）。
|
||||
|
||||
### 示例（完整计算过程）
|
||||
已知 r = (-1.433, 0.114, -0.430)（单位：rad）：
|
||||
- θ = |r| ≈ 1.5004615956
|
||||
- u = r / θ ≈ (-0.955039439, 0.075976620, -0.286578478)
|
||||
- s = sin(θ/2) ≈ 0.6818076141
|
||||
- c = cos(θ/2) ≈ 0.7315315286
|
||||
- q = (ux·s, uy·s, uz·s, c)
|
||||
≈ (-0.6511531610, 0.0518014378, -0.1953913882, 0.7315315286)
|
||||
|
||||
因此，对应四元数为：
|
||||
- qx,qy,qz,qw ≈ -0.65115316, 0.05180144, -0.19539139, 0.73153153
|
||||
|
||||
---
|
||||
|
||||
## 2) 欧拉角 → 四元数
|
||||
对于欧拉角 (rx, ry, rz)，若输入单位为弧度：
|
||||
- 以 xyz 顺序为例（依次绕自身 x、y、z 轴）：
|
||||
1. 构造三个轴角四元数：
|
||||
- qx = (sin(rx/2), 0, 0, cos(rx/2))
|
||||
- qy = (0, sin(ry/2), 0, cos(ry/2))
|
||||
- qz = (0, 0, sin(rz/2), cos(rz/2))
|
||||
2. 乘法顺序为 q = qx ⊗ qy ⊗ qz（内在旋转，右乘积）。
|
||||
- 对于 zyx 顺序：q = qz ⊗ qy ⊗ qx。
|
||||
- 最后同样进行单位化。
|
||||
|
||||
> 提示：不同库/软件可能有“外在 vs 内在”、“左乘 vs 右乘”、“轴顺序”等差异，需确保与使用方约定一致。
|
||||
|
||||
---
|
||||
|
||||
## 3) 边界与常见问题
|
||||
- 零角或极小角：直接返回单位四元数 (0,0,0,1) 或采用泰勒展开近似。
|
||||
- 符号一致性：q 与 -q 表示同一旋转，批量处理时常将 w 约束为非负以保证连续性（可选）。
|
||||
- 单位：所有角度必须使用弧度；若源数据是度，请先转弧度（乘以 π/180）。
|
||||
- 数值稳定：建议在输出前做单位化，避免浮点累积误差。
|
||||
|
||||
---
|
||||
|
||||
## 4) 使用脚本批量/单次转换
|
||||
脚本路径：`src/scripts/euler_to_quaternion.py`
|
||||
|
||||
- 单次旋转向量 → 四元数：
|
||||
```bash
|
||||
python src/scripts/euler_to_quaternion.py --mode rotvec --single RX RY RZ
|
||||
# 例如：
|
||||
python src/scripts/euler_to_quaternion.py --mode rotvec --single -1.433 0.114 -0.430
|
||||
```
|
||||
|
||||
- CSV 批量（默认欧拉角，若为旋转向量请加 --mode rotvec）：
|
||||
```bash
|
||||
python src/scripts/euler_to_quaternion.py \
|
||||
--in input.csv --out output.csv \
|
||||
--rx rx --ry ry --rz rz \
|
||||
--mode rotvec
|
||||
```
|
||||
|
||||
- 欧拉角（内在旋转顺序 xyz 或 zyx）：
|
||||
```bash
|
||||
python src/scripts/euler_to_quaternion.py \
|
||||
--mode euler --order xyz --single RX RY RZ
|
||||
```
|
||||
|
||||
> 如果输入 CSV 同时包含位姿中的位置列（x,y,z），脚本支持可选的单位转换 `--mm-to-m`（将毫米换算为米，并额外输出 x_m,y_m,z_m 列）。
|
||||
|
||||
---
|
||||
|
||||
## 5) 本次 12 组旋转向量的结果
|
||||
以下 12 组 (rx, ry, rz)（单位：rad）对应的四元数 (qx, qy, qz, qw)：
|
||||
|
||||
1. -0.65115316, 0.05180144, -0.19539139, 0.73153153
|
||||
2. -0.71991924, -0.00710155, -0.17753865, 0.67092912
|
||||
3. -0.44521470, -0.25512818, -0.41269388, 0.75258039
|
||||
4. -0.72304324, 0.09631938, -0.32264833, 0.60318248
|
||||
5. -0.67311368, 0.06894426, -0.11793097, 0.72681287
|
||||
6. -0.73524204, -0.19261515, -0.19652833, 0.61943132
|
||||
7. -0.75500427, -0.08296268, -0.09788718, 0.64304265
|
||||
8. -0.88627353, -0.08089799, -0.15658968, 0.42831579
|
||||
9. -0.62408775, -0.13051614, -0.11718879, 0.76141106
|
||||
10. -0.67818166, -0.10516535, -0.18696062, 0.70289090
|
||||
11. -0.77275040, -0.19297175, -0.05741665, 0.60193194
|
||||
12. -0.66493346, 0.09013744, -0.16351565, 0.72318833
|
||||
|
||||
---
|
||||
|
||||
## 6) 参考实现
|
||||
脚本 `euler_to_quaternion.py` 中的核心函数：
|
||||
- `rotvec_to_quaternion(rx, ry, rz)`：实现了第 1 节所述的 Rodrigues → 四元数转换，并在小角度与归一化方面做了稳健处理。
|
||||
- `euler_to_quaternion(rx, ry, rz, order, degrees)`：实现了第 2 节所述的欧拉角 → 四元数转换，支持 xyz/zyx 两种顺序与度/弧度输入。
|
||||
|
||||
如需将结果保存为 CSV 或用于后续手眼标定、TF 发布等，可直接复用该脚本的命令行接口。
|
||||
|
||||
---
|
||||
@@ -1,3 +0,0 @@
|
||||
# Pinned to avoid NumPy 2.x ABI issues with some wheels
|
||||
numpy<2
|
||||
opencv-python==4.11.0.86
|
||||
@@ -1,119 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
rotate_pose.py
|
||||
|
||||
Small utility to rotate a 7-element pose (x,y,z,qx,qy,qz,qw or qw,qx,qy,qz)
|
||||
around a given axis by a specified angle (degrees).
|
||||
|
||||
Usage examples:
|
||||
./rotate_pose.py --pose 0.1 0.2 0.3 -0.6 0.1 -0.1 0.7 --axis z --angle 180 --order qxqyqzqw --frame world
|
||||
python3 rotate_pose.py --pose 0.179387 0.341708 -0.099286 -0.88140507 0.12851699 -0.17498077 0.41951188 --axis z --angle 180 --order qxqyqzqw --frame world
|
||||
|
||||
Supported frames:
|
||||
- world: apply rotation in world frame (q' = q_rot * q)
|
||||
- body: apply rotation in body/local frame (q' = q * q_rot)
|
||||
|
||||
Supported quaternion order strings: "qxqyqzqw" (default) or "wxyz" (qw,qx,qy,qz)
|
||||
|
||||
"""
|
||||
import argparse
|
||||
import math
|
||||
from typing import List, Tuple
|
||||
|
||||
|
||||
def normalize_quat(q: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]:
|
||||
x, y, z, w = q
|
||||
n = math.sqrt(x * x + y * y + z * z + w * w)
|
||||
if n == 0:
|
||||
return (0.0, 0.0, 0.0, 1.0)
|
||||
return (x / n, y / n, z / n, w / n)
|
||||
|
||||
|
||||
def axis_angle_to_quat(axis: str, angle_deg: float) -> Tuple[float, float, float, float]:
|
||||
a = math.radians(angle_deg)
|
||||
ca = math.cos(a / 2.0)
|
||||
sa = math.sin(a / 2.0)
|
||||
ax = axis.lower()
|
||||
if ax == 'x':
|
||||
return (sa, 0.0, 0.0, ca)
|
||||
if ax == 'y':
|
||||
return (0.0, sa, 0.0, ca)
|
||||
if ax == 'z':
|
||||
return (0.0, 0.0, sa, ca)
|
||||
raise ValueError('axis must be x,y or z')
|
||||
|
||||
|
||||
def quat_mul(a: Tuple[float,float,float,float], b: Tuple[float,float,float,float]) -> Tuple[float,float,float,float]:
|
||||
ax, ay, az, aw = a
|
||||
bx, by, bz, bw = b
|
||||
x = aw*bx + ax*bw + ay*bz - az*by
|
||||
y = aw*by - ax*bz + ay*bw + az*bx
|
||||
z = aw*bz + ax*by - ay*bx + az*bw
|
||||
w = aw*bw - ax*bx - ay*by - az*bz
|
||||
return (x, y, z, w)
|
||||
|
||||
|
||||
def rotate_position(pos: Tuple[float,float,float], axis: str, angle_deg: float) -> Tuple[float,float,float]:
|
||||
# For 180-degree rotations we can do simple sign flips but implement general rotation using quaternion
|
||||
qrot = axis_angle_to_quat(axis, angle_deg)
|
||||
# convert pos to quaternion (v,0)
|
||||
px, py, pz = pos
|
||||
p = (px, py, pz, 0.0)
|
||||
qrot_n = normalize_quat(qrot)
|
||||
qrot_conj = (-qrot_n[0], -qrot_n[1], -qrot_n[2], qrot_n[3])
|
||||
tmp = quat_mul(qrot_n, p)
|
||||
p_rot = quat_mul(tmp, qrot_conj)
|
||||
return (p_rot[0], p_rot[1], p_rot[2])
|
||||
|
||||
|
||||
def parse_pose(vals: List[float], order: str) -> Tuple[Tuple[float,float,float], Tuple[float,float,float,float]]:
|
||||
if order == 'qxqyqzqw':
|
||||
if len(vals) != 7:
|
||||
raise ValueError('need 7 values for pose')
|
||||
return (vals[0], vals[1], vals[2]), (vals[3], vals[4], vals[5], vals[6])
|
||||
if order == 'wxyz':
|
||||
if len(vals) != 7:
|
||||
raise ValueError('need 7 values for pose')
|
||||
return (vals[0], vals[1], vals[2]), (vals[3], vals[4], vals[5], vals[6]) if False else (vals[4], vals[5], vals[6], vals[3])
|
||||
raise ValueError('unsupported order')
|
||||
|
||||
|
||||
def format_pose(pos: Tuple[float,float,float], quat: Tuple[float,float,float,float], order: str) -> List[float]:
|
||||
if order == 'qxqyqzqw':
|
||||
return [pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]]
|
||||
if order == 'wxyz':
|
||||
# qw,qx,qy,qz
|
||||
return [pos[0], pos[1], pos[2], quat[3], quat[0], quat[1], quat[2]]
|
||||
raise ValueError('unsupported order')
|
||||
|
||||
|
||||
def main():
|
||||
p = argparse.ArgumentParser()
|
||||
p.add_argument('--pose', nargs=7, type=float, required=True, help='pose values')
|
||||
p.add_argument('--axis', choices=['x','y','z'], default='z')
|
||||
p.add_argument('--angle', type=float, default=180.0)
|
||||
p.add_argument('--order', choices=['qxqyqzqw','wxyz'], default='qxqyqzqw')
|
||||
p.add_argument('--frame', choices=['world','body'], default='world')
|
||||
args = p.parse_args()
|
||||
|
||||
pos, quat = parse_pose(list(args.pose), args.order)
|
||||
# rotation quaternion
|
||||
qrot = axis_angle_to_quat(args.axis, args.angle)
|
||||
qrot = normalize_quat(qrot)
|
||||
|
||||
# apply rotation to position in world frame: pos' = R * pos
|
||||
pos_rot = rotate_position(pos, args.axis, args.angle)
|
||||
|
||||
# apply rotation to orientation
|
||||
if args.frame == 'world':
|
||||
qnew = quat_mul(qrot, quat)
|
||||
else:
|
||||
qnew = quat_mul(quat, qrot)
|
||||
qnew = normalize_quat(qnew)
|
||||
|
||||
out = format_pose(pos_rot, qnew, args.order)
|
||||
# Print as numeric list without quotes
|
||||
print('rotated_pose:', [float('{:.8f}'.format(x)) for x in out])
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,152 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
set -euo pipefail
|
||||
|
||||
# Resolve workspace root (script may be invoked from repo root)
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
WS_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
|
||||
|
||||
# Source ROS 2 workspace (temporarily disable nounset to avoid unbound vars inside setup scripts)
|
||||
if [[ -f "${WS_ROOT}/install/setup.bash" ]]; then
|
||||
# shellcheck source=/dev/null
|
||||
set +u
|
||||
source "${WS_ROOT}/install/setup.bash"
|
||||
set -u
|
||||
else
|
||||
echo "[run.sh] install/setup.bash not found at ${WS_ROOT}/install/setup.bash"
|
||||
echo "[run.sh] Please build the workspace first, e.g.,: colcon build && source install/setup.bash"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Human-friendly logging format with local wall-clock time and colors
|
||||
export RCUTILS_COLORIZED_OUTPUT=${RCUTILS_COLORIZED_OUTPUT:-1}
|
||||
export RCUTILS_CONSOLE_OUTPUT_FORMAT='[{time}] [{severity}] [{name}]: {message}'
|
||||
|
||||
LOG_TO_FILES=0
|
||||
LOG_LEVEL="info" # debug|info|warn|error|fatal
|
||||
RUN_BRAIN=1
|
||||
RUN_MOTION=1
|
||||
|
||||
usage() {
|
||||
cat <<EOF
|
||||
Usage: ${0##*/} [options]
|
||||
|
||||
Options:
|
||||
-l, --log-to-files Write each node's stdout/stderr to log/run/<timestamp>/*.log
|
||||
-L, --log-level LEVEL Set ROS 2 log level (default: info). Ex: debug, warn, error
|
||||
--brain-only Run only brain node
|
||||
--arm_control-only Run only arm_control node
|
||||
-h, --help Show this help and exit
|
||||
|
||||
Environment:
|
||||
RCUTILS_COLORIZED_OUTPUT Colorize console logs (default: 1)
|
||||
RCUTILS_CONSOLE_OUTPUT_FORMAT Log format (already configured here)
|
||||
EOF
|
||||
}
|
||||
|
||||
# Parse args
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
-l|--log-to-files)
|
||||
LOG_TO_FILES=1; shift ;;
|
||||
-L|--log-level)
|
||||
LOG_LEVEL="${2:-info}"; shift 2 ;;
|
||||
--brain-only)
|
||||
RUN_BRAIN=1; RUN_MOTION=0; shift ;;
|
||||
--arm_control-only)
|
||||
RUN_BRAIN=0; RUN_MOTION=1; shift ;;
|
||||
-h|--help)
|
||||
usage; exit 0 ;;
|
||||
*)
|
||||
echo "Unknown option: $1" >&2
|
||||
usage; exit 2 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
# 获取当前日期用于归档 (例如: 20260114)
|
||||
date_day() { date +"%Y%m%d"; }
|
||||
# 获取可读的时间戳用于文件名 (例如: 2026-01-14_15-30-05)
|
||||
readable_timestamp() { date +"%Y-%m-%d_%H-%M-%S"; }
|
||||
|
||||
# 1. 统一存放路径到当天的目录下
|
||||
DAILY_LOG_ROOT="${WS_ROOT}/logs/$(date_day)"
|
||||
# 2. 为每次启动创建一个带时间的子目录 (保证多次运行不冲突)
|
||||
LOG_DIR="${DAILY_LOG_ROOT}/robot_logs_$(readable_timestamp)"
|
||||
mkdir -p "${LOG_DIR}"
|
||||
|
||||
# 强制 ROS 2 自身的内部日志也记录到此目录，防止散落在 ~/.ros/log
|
||||
export ROS_LOG_DIR="${LOG_DIR}"
|
||||
|
||||
pids=()
|
||||
names=()
|
||||
|
||||
start_node() {
|
||||
local name="$1"; shift
|
||||
local pkg="$1"; shift
|
||||
local exe="$1"; shift
|
||||
# 3. 日志文件名可配置：在此处修改格式
|
||||
local node_ts=$(readable_timestamp)
|
||||
local log_file="${LOG_DIR}/${name}_${node_ts}.log"
|
||||
|
||||
if (( LOG_TO_FILES )); then
|
||||
echo "[run.sh] Starting ${name} -> ros2 run ${pkg} ${exe} (logs: ${log_file})"
|
||||
(
|
||||
# 禁用 ANSI 颜色以保持日志文件整洁
|
||||
RCUTILS_COLORIZED_OUTPUT=0 exec ros2 run "${pkg}" "${exe}" --ros-args --log-level "${LOG_LEVEL}"
|
||||
) >>"${log_file}" 2>&1 &
|
||||
else
|
||||
echo "[run.sh] Starting ${name} -> ros2 run ${pkg} ${exe} --ros-args --log-level ${LOG_LEVEL}"
|
||||
ros2 run "${pkg}" "${exe}" --ros-args --log-level "${LOG_LEVEL}" &
|
||||
fi
|
||||
local pid=$!
|
||||
pids+=("${pid}")
|
||||
names+=("${name}")
|
||||
}
|
||||
|
||||
cleanup_done=0
|
||||
cleanup() {
|
||||
(( cleanup_done )) && return 0
|
||||
cleanup_done=1
|
||||
echo ""
|
||||
echo "[run.sh] Shutting down (${#pids[@]} processes)..."
|
||||
# Send SIGINT first for graceful shutdown
|
||||
for pid in "${pids[@]:-}"; do
|
||||
if kill -0 "$pid" 2>/dev/null; then
|
||||
kill -INT "$pid" 2>/dev/null || true
|
||||
fi
|
||||
done
|
||||
# Give them a moment to exit gracefully
|
||||
sleep 1
|
||||
# Force kill any stubborn processes
|
||||
for pid in "${pids[@]:-}"; do
|
||||
if kill -0 "$pid" 2>/dev/null; then
|
||||
kill -TERM "$pid" 2>/dev/null || true
|
||||
fi
|
||||
done
|
||||
wait || true
|
||||
echo "[run.sh] All processes stopped."
|
||||
}
|
||||
|
||||
trap cleanup INT TERM
|
||||
|
||||
# Start requested nodes
|
||||
if (( RUN_MOTION )); then
|
||||
start_node "arm_control" "arm_control" "motion_node"
|
||||
fi
|
||||
if (( RUN_BRAIN )); then
|
||||
start_node "brain" "brain" "brain_node"
|
||||
fi
|
||||
|
||||
if (( ${#pids[@]} == 0 )); then
|
||||
echo "[run.sh] Nothing to run. Use --brain-only or --arm_control-only to pick a node."
|
||||
exit 0
|
||||
fi
|
||||
|
||||
echo "[run.sh] Started: ${names[*]}"
|
||||
echo "[run.sh] Press Ctrl+C to stop."
|
||||
|
||||
# Wait for all background processes
|
||||
wait
|
||||
|
||||
# Ensure cleanup executes on normal exit too
|
||||
cleanup
|
||||
@@ -1,94 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
# Defaults
|
||||
ACTION_NAME="/ArmSpaceControl"
|
||||
FRAME_ID="base_link"
|
||||
X=0.5
|
||||
Y=0.0
|
||||
Z=0.3
|
||||
YAW_DEG=0.0
|
||||
ANGLE_UNIT="deg" # deg|rad
|
||||
DRY_RUN=0
|
||||
LOG_LEVEL="info"
|
||||
|
||||
usage(){
|
||||
cat <<EOF
|
||||
Usage: ${0##*/} [options]
|
||||
|
||||
Options:
|
||||
-a, --action NAME Action name (default: /arm_space_control)
|
||||
-f, --frame ID Target frame_id (default: base_link)
|
||||
--x VALUE Position x (m) (default: 0.5)
|
||||
--y VALUE Position y (m) (default: 0.0)
|
||||
--z VALUE Position z (m) (default: 0.3)
|
||||
--yaw VALUE Yaw angle (default: 0.0)
|
||||
--rad Interpret yaw as radians (default is degrees)
|
||||
--deg Interpret yaw as degrees (default)
|
||||
--log-level LEVEL ROS 2 log level (default: info)
|
||||
--dry-run Print goal YAML and exit
|
||||
-h, --help Show this help
|
||||
|
||||
Examples:
|
||||
${0##*/} --x 0.5 --y 0.0 --z 0.3 --yaw 90 --deg --frame base_link
|
||||
${0##*/} --action /ArmSpaceControl --yaw 1.57 --rad
|
||||
EOF
|
||||
}
|
||||
|
||||
# Parse args
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
-a|--action) ACTION_NAME="$2"; shift 2 ;;
|
||||
-f|--frame) FRAME_ID="$2"; shift 2 ;;
|
||||
--x) X="$2"; shift 2 ;;
|
||||
--y) Y="$2"; shift 2 ;;
|
||||
--z) Z="$2"; shift 2 ;;
|
||||
--yaw) YAW_DEG="$2"; shift 2 ;;
|
||||
--rad) ANGLE_UNIT="rad"; shift ;;
|
||||
--deg) ANGLE_UNIT="deg"; shift ;;
|
||||
--log-level) LOG_LEVEL="$2"; shift 2 ;;
|
||||
--dry-run) DRY_RUN=1; shift ;;
|
||||
-h|--help) usage; exit 0 ;;
|
||||
*) echo "Unknown option: $1" >&2; usage; exit 2 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
# Compute quaternion from yaw only (roll=pitch=0)
|
||||
# qx=qy=0, qz=sin(yaw/2), qw=cos(yaw/2)
|
||||
python_quat='import math,sys; yaw=float(sys.argv[1]); print(f"{math.sin(yaw/2.0)} {math.cos(yaw/2.0)}")'
|
||||
|
||||
if [[ "$ANGLE_UNIT" == "deg" ]]; then
|
||||
# Convert degrees to radians in Python for accuracy
|
||||
read -r QZ QW < <(python3 -c 'import math,sys; yaw_deg=float(sys.argv[1]); yaw=math.radians(yaw_deg); print(f"{math.sin(yaw/2.0)} {math.cos(yaw/2.0)}")' "$YAW_DEG")
|
||||
else
|
||||
read -r QZ QW < <(python3 -c "$python_quat" "$YAW_DEG")
|
||||
fi
|
||||
|
||||
# Build YAML (avoid shell-escaping issues; no quotes around frame_id)
|
||||
GOAL_YAML=$(cat <<YAML
|
||||
{target: {header: {frame_id: ${FRAME_ID}}, pose: {position: {x: ${X}, y: ${Y}, z: ${Z}}, orientation: {x: 0.0, y: 0.0, z: ${QZ}, w: ${QW}}}}}
|
||||
YAML
|
||||
)
|
||||
|
||||
if (( DRY_RUN )); then
|
||||
echo "Will send to action: ${ACTION_NAME}"
|
||||
echo "Goal YAML:"
|
||||
echo "$GOAL_YAML"
|
||||
exit 0
|
||||
fi
|
||||
|
||||
# Source workspace
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
WS_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
|
||||
if [[ -f "${WS_ROOT}/install/setup.bash" ]]; then
|
||||
# shellcheck source=/dev/null
|
||||
source "${WS_ROOT}/install/setup.bash"
|
||||
else
|
||||
echo "install/setup.bash not found at ${WS_ROOT}/install/setup.bash" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Send goal
|
||||
set -x
|
||||
ros2 action send_goal --feedback "${ACTION_NAME}" interfaces/action/ArmSpaceControl "$GOAL_YAML" --ros-args --log-level "${LOG_LEVEL}"
|
||||
set +x
|
||||
@@ -19,6 +19,10 @@ find_package(Boost COMPONENTS thread REQUIRED)
|
||||
find_package(tracetools)
|
||||
find_package(LTTngUST REQUIRED)
|
||||
|
||||
# hivecore logger SDK
|
||||
set(hivecore_logger_cpp_DIR "/opt/hivecore/logger-sdk/1.0.1/lib/cmake/hivecore_logger_cpp")
|
||||
find_package(hivecore_logger_cpp REQUIRED)
|
||||
|
||||
set(dependencies
|
||||
rclcpp
|
||||
smacc2_msgs
|
||||
@@ -26,7 +30,9 @@ set(dependencies
|
||||
tracetools
|
||||
)
|
||||
|
||||
include_directories(include ${LTTNGUST_INCLUDE_DIRS})
|
||||
include_directories(include ${LTTNGUST_INCLUDE_DIRS}
|
||||
/opt/hivecore/logger-sdk/1.0.1/include
|
||||
)
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src *.cpp)
|
||||
add_library(smacc2 SHARED ${SRC_FILES})
|
||||
@@ -42,6 +48,7 @@ ament_target_dependencies(smacc2 ${dependencies})
|
||||
target_link_libraries(smacc2
|
||||
${Boost_LIBRARIES}
|
||||
${LTTNGUST_LIBRARIES}
|
||||
hivecore_logger_cpp::hivecore_logger_cpp
|
||||
)
|
||||
|
||||
ament_export_include_directories(include)
|
||||
|
||||
@@ -21,11 +21,13 @@
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/client_bases/smacc_action_client.hpp>
|
||||
#include <sstream>
|
||||
#include <smacc2/smacc_default_events.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
|
||||
#include <optional>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -86,9 +88,7 @@ public:
|
||||
|
||||
void waitForServer()
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->getLogger(),
|
||||
"Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>());
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>(); return _oss.str(); }());
|
||||
client_->wait_for_action_server();
|
||||
}
|
||||
|
||||
@@ -125,8 +125,7 @@ public:
|
||||
auto * ev = new EvType();
|
||||
// ev->client = this;
|
||||
// ev->resultMessage = *result;
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "Action client Posting EVENT %s", demangleSymbol(typeid(ev).name()).c_str());
|
||||
LOG_INFO("Action client Posting EVENT {}", demangleSymbol(typeid(ev).name()).c_str());
|
||||
this->postEvent(ev);
|
||||
}
|
||||
|
||||
@@ -148,7 +147,7 @@ public:
|
||||
actionFeedbackEvent->client = this;
|
||||
actionFeedbackEvent->feedbackMessage = msg;
|
||||
this->postEvent(actionFeedbackEvent);
|
||||
RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", demangleType(typeid(*this)).c_str());
|
||||
LOG_DEBUG("[{}] FEEDBACK EVENT", demangleType(typeid(*this)).c_str());
|
||||
};
|
||||
|
||||
feedback_cb = [this](auto client, auto feedback) { this->onFeedback(client, feedback); };
|
||||
@@ -275,13 +274,11 @@ public:
|
||||
// if goal ids are not matched, the older goa call this callback so ignore the result
|
||||
// if matched, it must be processed (including aborted)
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Action result callback, getting shared future");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Action result callback, getting shared future"; return _oss.str(); }());
|
||||
// auto goalHandle = result->get();
|
||||
// goalHandle_ = lastRequest_->get();
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Action client Result goal id: "
|
||||
<< rclcpp_action::to_string(result.goal_id));
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Action client Result goal id: "
|
||||
<< rclcpp_action::to_string(result.goal_id); return _oss.str(); }());
|
||||
|
||||
// if (goalHandle_->get_goal_id() == result.goal_id)
|
||||
// {
|
||||
@@ -293,15 +290,13 @@ public:
|
||||
|
||||
if (resultCallbackPtr != nullptr)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Result CB calling user callback:"
|
||||
<< demangleSymbol(typeid(*resultCallbackPtr).name()));
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Result CB calling user callback:"
|
||||
<< demangleSymbol(typeid(*resultCallbackPtr).name()); return _oss.str(); }());
|
||||
(*resultCallbackPtr)(result);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Result CB calling default callback");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Result CB calling default callback"; return _oss.str(); }());
|
||||
this->onResult(result);
|
||||
}
|
||||
|
||||
@@ -323,23 +318,18 @@ public:
|
||||
// RCLCPP_INFO_STREAM(getLogger(), getName() << ": no previous request.");
|
||||
// }
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] client ready clients: "
|
||||
<< this->client_->get_number_of_ready_clients());
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(),
|
||||
"[" << getName() << "] Waiting it is ready? " << client_->action_server_is_ready());
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] client ready clients: "
|
||||
<< this->client_->get_number_of_ready_clients(); return _oss.str(); }());
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Waiting it is ready? " << client_->action_server_is_ready(); return _oss.str(); }());
|
||||
|
||||
RCLCPP_INFO_STREAM(getLogger(), getName() << ": async send goal.");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << getName() << ": async send goal."; return _oss.str(); }());
|
||||
auto lastRequest = this->client_->async_send_goal(goal, options);
|
||||
this->lastRequest_ = lastRequest;
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Action request "
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] Action request "; return _oss.str(); }());
|
||||
// << rclcpp_action::to_string(this->goalHandle_->get_goal_id()) <<". Goal sent to " << this->action_endpoint_
|
||||
// << "\": " << std::endl
|
||||
// << goal
|
||||
);
|
||||
|
||||
// if (client_->isServerConnected())
|
||||
// {
|
||||
@@ -395,46 +385,43 @@ protected:
|
||||
// const auto &resultType = this->getState();
|
||||
const auto & resultType = result_msg.code;
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] response result ["
|
||||
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response result ["
|
||||
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType; return _oss.str(); }());
|
||||
|
||||
if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] request result: Success", this->getName().c_str());
|
||||
LOG_INFO("[{}] request result: Success", this->getName().c_str());
|
||||
onSucceeded_(result_msg);
|
||||
postSuccessEvent(result_msg);
|
||||
}
|
||||
else if (resultType == rclcpp_action::ResultCode::ABORTED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] request result: Aborted", this->getName().c_str());
|
||||
LOG_INFO("[{}] request result: Aborted", this->getName().c_str());
|
||||
onAborted_(result_msg);
|
||||
postAbortedEvent(result_msg);
|
||||
}
|
||||
else if (resultType == rclcpp_action::ResultCode::CANCELED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] request result: Cancelled", this->getName().c_str());
|
||||
LOG_INFO("[{}] request result: Cancelled", this->getName().c_str());
|
||||
onCancelled_(result_msg);
|
||||
postCancelledEvent(result_msg);
|
||||
}
|
||||
/*
|
||||
else if (resultType == actionlib::SimpleClientGoalState::REJECTED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(),"[%s] request result: Rejected", this->getName().c_str());
|
||||
LOG_INFO("[{}] request result: Rejected", this->getName().c_str());
|
||||
onRejected_(result_msg);
|
||||
postRejectedEvent(result_msg);
|
||||
}
|
||||
else if (resultType == actionlib::SimpleClientGoalState::PREEMPTED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(),"[%s] request result: Preempted", this->getName().c_str());
|
||||
LOG_INFO("[{}] request result: Preempted", this->getName().c_str());
|
||||
onPreempted_(result_msg);
|
||||
postPreemptedEvent(result_msg);
|
||||
}*/
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[%s] request result: NOT HANDLED TYPE: %d", this->getName().c_str(),
|
||||
(int)resultType);
|
||||
LOG_INFO("[{}] request result: NOT HANDLED TYPE: {}", this->getName().c_str(), (int)resultType);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
@@ -21,6 +21,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -46,7 +48,7 @@ public:
|
||||
{
|
||||
if (!this->topicName)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "topic publisher with no topic name set. Skipping advertising.");
|
||||
LOG_ERROR("topic publisher with no topic name set. Skipping advertising.");
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -58,8 +60,7 @@ public:
|
||||
qos.durability(*durability);
|
||||
qos.reliability(*reliability);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Client Publisher to topic: " << topicName);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Client Publisher to topic: " << topicName; return _oss.str(); }());
|
||||
pub_ = getNode()->create_publisher<MessageType>(*(this->topicName), qos);
|
||||
|
||||
this->initialized_ = true;
|
||||
|
||||
@@ -21,6 +21,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -43,12 +45,11 @@ public:
|
||||
{
|
||||
if (!serviceName_)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "service client with no service name set. Skipping.");
|
||||
LOG_ERROR("service client with no service name set. Skipping.");
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Client Service: " << *serviceName_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Client Service: " << *serviceName_; return _oss.str(); }());
|
||||
this->initialized_ = true;
|
||||
client_ = getNode()->create_client<ServiceType>(*serviceName_);
|
||||
}
|
||||
|
||||
@@ -21,6 +21,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
|
||||
@@ -67,14 +69,11 @@ public:
|
||||
{
|
||||
if (!serviceName_)
|
||||
{
|
||||
RCLCPP_ERROR_STREAM(
|
||||
getLogger(),
|
||||
"[" << this->getName() << "] service server with no service name set. Skipping.");
|
||||
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] service server with no service name set. Skipping."; return _oss.str(); }());
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Client Service: " << *serviceName_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Client Service: " << *serviceName_; return _oss.str(); }());
|
||||
|
||||
server_ = getNode()->create_service<TService>(
|
||||
*serviceName_, std::bind(
|
||||
|
||||
@@ -21,6 +21,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
@@ -101,12 +103,11 @@ protected:
|
||||
|
||||
if (!topicName)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "topic client with no topic name set. Skipping subscribing");
|
||||
LOG_ERROR("topic client with no topic name set. Skipping subscribing");
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Subscribing to topic: " << *topicName);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Subscribing to topic: " << *topicName; return _oss.str(); }());
|
||||
|
||||
rclcpp::SensorDataQoS qos;
|
||||
if (queueSize) qos.keep_last(*queueSize);
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
#pragma once
|
||||
#include <smacc2/impl/smacc_asynchronous_client_behavior_impl.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -45,41 +47,36 @@ public:
|
||||
|
||||
void onEntry() override
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] creating ros service client: " << serviceName_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] creating ros service client: " << serviceName_; return _oss.str(); }());
|
||||
|
||||
client_ = getNode()->create_client<ServiceType>(serviceName_);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] making service request to " << serviceName_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] making service request to " << serviceName_; return _oss.str(); }());
|
||||
|
||||
resultFuture_ = client_->async_send_request(request_).future.share();
|
||||
|
||||
std::future_status status = resultFuture_.wait_for(0s);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "thread state: " << (int)status << " ok " << rclcpp::ok() << " shutdown "
|
||||
<< this->isShutdownRequested() << "");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "thread state: " << (int)status << " ok " << rclcpp::ok() << " shutdown "
|
||||
<< this->isShutdownRequested() << ""; return _oss.str(); }());
|
||||
|
||||
while (status != std::future_status::ready && rclcpp::ok() && !this->isShutdownRequested())
|
||||
{
|
||||
RCLCPP_INFO_STREAM_THROTTLE(
|
||||
getLogger(), *getNode()->get_clock(), 1000,
|
||||
"[" << this->getName() << "] waiting response ");
|
||||
LOG_INFO_THROTTLE(1000, "{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] waiting response "; return _oss.str(); }());
|
||||
rclcpp::sleep_for(pollRate_);
|
||||
status = resultFuture_.wait_for(0s);
|
||||
}
|
||||
|
||||
if (status == std::future_status::ready)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response received "; return _oss.str(); }());
|
||||
result_ = resultFuture_.get();
|
||||
onServiceResponse(result_);
|
||||
this->postSuccessEvent();
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response not received ");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response not received "; return _oss.str(); }());
|
||||
this->postFailureEvent();
|
||||
}
|
||||
}
|
||||
@@ -97,7 +94,7 @@ protected:
|
||||
|
||||
virtual void onServiceResponse(std::shared_ptr<typename ServiceType::Response> /*result*/)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] response received "; return _oss.str(); }());
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -22,6 +22,7 @@
|
||||
|
||||
#include <functional>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -60,9 +61,7 @@ public:
|
||||
std::function<std::shared_ptr<smacc2::SmaccAsyncClientBehavior>()> delayedCBFactoryFn =
|
||||
[this, args...]()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[CbSequence] then creating new sub behavior %s ",
|
||||
demangleSymbol<TBehavior>().c_str());
|
||||
LOG_INFO("[CbSequence] then creating new sub behavior {} ", demangleSymbol<TBehavior>().c_str());
|
||||
auto createdBh = std::shared_ptr<TBehavior>(new TBehavior(args...));
|
||||
|
||||
this->getCurrentState()->getOrthogonal<TOrthogonal>()->addClientBehavior(createdBh);
|
||||
|
||||
@@ -21,7 +21,9 @@
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <sstream>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -50,9 +52,8 @@ public:
|
||||
rclcpp::SensorDataQoS qos;
|
||||
// qos.reliable();
|
||||
// rclcpp::SubscriptionOptions sub_option;
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[CbWaitTopicMessage] waiting message from topic: "
|
||||
<< topicname_ << "[" << demangledTypeName<TMessage>() << "]");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbWaitTopicMessage] waiting message from topic: "
|
||||
<< topicname_ << "[" << demangledTypeName<TMessage>() << "]"; return _oss.str(); }());
|
||||
|
||||
// sub_ = getNode()->create_subscription<TMessage>(
|
||||
// topicname_, qos,
|
||||
@@ -70,7 +71,7 @@ public:
|
||||
{
|
||||
if (guardFn_ == nullptr || guardFn_(*msg))
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[CbWaitTopicMessage] message received.");
|
||||
LOG_INFO("[CbWaitTopicMessage] message received.");
|
||||
success = true;
|
||||
this->postSuccessEvent();
|
||||
}
|
||||
|
||||
@@ -15,6 +15,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/component.hpp>
|
||||
#include <sstream>
|
||||
#include <smacc2/smacc_default_events.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
@@ -25,6 +26,7 @@
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -91,25 +93,23 @@ public:
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(actionMutex_);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Action result callback, goal id: "
|
||||
<< rclcpp_action::to_string(result.goal_id));
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Action result callback, goal id: "
|
||||
<< rclcpp_action::to_string(result.goal_id); return _oss.str(); }());
|
||||
|
||||
auto resultCallbackPtr = resultCallback.lock();
|
||||
if (resultCallbackPtr != nullptr)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Calling user result callback");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Calling user result callback"; return _oss.str(); }());
|
||||
(*resultCallbackPtr)(result);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Using default result handling");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Using default result handling"; return _oss.str(); }());
|
||||
this->onResult(result);
|
||||
}
|
||||
};
|
||||
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Sending goal to action server");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Sending goal to action server"; return _oss.str(); }());
|
||||
|
||||
auto goalFuture = client_->async_send_goal(goal, options);
|
||||
lastRequest_ = goalFuture;
|
||||
@@ -123,7 +123,7 @@ public:
|
||||
|
||||
if (lastRequest_ && lastRequest_->valid())
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Cancelling current goal");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Cancelling current goal"; return _oss.str(); }());
|
||||
|
||||
auto cancelFuture = client_->async_cancel_all_goals();
|
||||
lastCancelResponse_ = cancelFuture;
|
||||
@@ -131,7 +131,7 @@ public:
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN_STREAM(getLogger(), "[" << this->getName() << "] No active goal to cancel");
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] No active goal to cancel"; return _oss.str(); }());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -142,9 +142,7 @@ public:
|
||||
{
|
||||
if (client_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(),
|
||||
"[" << this->getName() << "] Waiting for action server: " << *actionServerName);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Waiting for action server: " << *actionServerName; return _oss.str(); }());
|
||||
client_->wait_for_action_server();
|
||||
}
|
||||
}
|
||||
@@ -154,13 +152,11 @@ public:
|
||||
{
|
||||
if (!actionServerName)
|
||||
{
|
||||
RCLCPP_ERROR_STREAM(getLogger(), "[" << this->getName() << "] Action server name not set!");
|
||||
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Action server name not set!"; return _oss.str(); }());
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(),
|
||||
"[" << this->getName() << "] Initializing action client for: " << *actionServerName);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Initializing action client for: " << *actionServerName; return _oss.str(); }());
|
||||
|
||||
client_ = rclcpp_action::create_client<ActionType>(getNode(), *actionServerName);
|
||||
|
||||
@@ -188,7 +184,7 @@ public:
|
||||
actionFeedbackEvent->client = this;
|
||||
actionFeedbackEvent->feedbackMessage = feedback;
|
||||
this->postEvent(actionFeedbackEvent);
|
||||
RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", this->getName().c_str());
|
||||
LOG_DEBUG("[{}] FEEDBACK EVENT", this->getName().c_str());
|
||||
};
|
||||
}
|
||||
|
||||
@@ -241,33 +237,30 @@ private:
|
||||
{
|
||||
const auto & resultType = result_msg.code;
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Action result ["
|
||||
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Action result ["
|
||||
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType; return _oss.str(); }());
|
||||
|
||||
if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] Action result: Success", this->getName().c_str());
|
||||
LOG_INFO("[{}] Action result: Success", this->getName().c_str());
|
||||
onActionSucceeded_(result_msg);
|
||||
postSuccessEvent(result_msg);
|
||||
}
|
||||
else if (resultType == rclcpp_action::ResultCode::ABORTED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] Action result: Aborted", this->getName().c_str());
|
||||
LOG_INFO("[{}] Action result: Aborted", this->getName().c_str());
|
||||
onActionAborted_(result_msg);
|
||||
postAbortedEvent(result_msg);
|
||||
}
|
||||
else if (resultType == rclcpp_action::ResultCode::CANCELED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] Action result: Cancelled", this->getName().c_str());
|
||||
LOG_INFO("[{}] Action result: Cancelled", this->getName().c_str());
|
||||
onActionCancelled_(result_msg);
|
||||
postCancelledEvent(result_msg);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
getLogger(), "[%s] Action result: Unhandled type: %d", this->getName().c_str(),
|
||||
(int)resultType);
|
||||
LOG_WARN("[{}] Action result: Unhandled type: {}", this->getName().c_str(), (int)resultType);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -275,9 +268,7 @@ private:
|
||||
void postResultEvent(const WrappedResult & /* result */)
|
||||
{
|
||||
auto * ev = new EvType();
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[%s] Posting event: %s", this->getName().c_str(),
|
||||
smacc2::demangleSymbol(typeid(ev).name()).c_str());
|
||||
LOG_INFO("[{}] Posting event: {}", this->getName().c_str(), smacc2::demangleSymbol(typeid(ev).name()).c_str());
|
||||
this->postEvent(ev);
|
||||
}
|
||||
};
|
||||
|
||||
@@ -15,8 +15,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include <optional>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/common.hpp>
|
||||
#include <smacc2/component.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
@@ -47,9 +49,8 @@ public:
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Initializing ROS2 Timer with duration: "
|
||||
<< duration_.seconds() << "s, oneshot: " << (oneshot_ ? "true" : "false"));
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Initializing ROS2 Timer with duration: "
|
||||
<< duration_.seconds() << "s, oneshot: " << (oneshot_ ? "true" : "false"); return _oss.str(); }());
|
||||
|
||||
auto clock = this->getNode()->get_clock();
|
||||
timer_ = rclcpp::create_timer(
|
||||
@@ -65,7 +66,7 @@ public:
|
||||
{
|
||||
if (timer_ && timer_->is_canceled())
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Starting timer");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Starting timer"; return _oss.str(); }());
|
||||
timer_->reset();
|
||||
}
|
||||
}
|
||||
@@ -74,7 +75,7 @@ public:
|
||||
{
|
||||
if (timer_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Stopping timer");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Stopping timer"; return _oss.str(); }());
|
||||
timer_->cancel();
|
||||
}
|
||||
}
|
||||
@@ -83,7 +84,7 @@ public:
|
||||
{
|
||||
if (timer_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Cancelling timer");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Cancelling timer"; return _oss.str(); }());
|
||||
timer_->cancel();
|
||||
}
|
||||
}
|
||||
@@ -97,7 +98,7 @@ public:
|
||||
private:
|
||||
void timerCallback()
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(getLogger(), "[" << this->getName() << "] Timer tick");
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Timer tick"; return _oss.str(); }());
|
||||
|
||||
if (!onTimerTick_.empty())
|
||||
{
|
||||
@@ -106,8 +107,7 @@ private:
|
||||
|
||||
if (oneshot_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Oneshot timer completed, cancelling");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Oneshot timer completed, cancelling"; return _oss.str(); }());
|
||||
timer_->cancel();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -20,7 +20,9 @@
|
||||
|
||||
#pragma once
|
||||
#include <optional>
|
||||
#include <sstream>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/common.hpp>
|
||||
#include <smacc2/component.hpp>
|
||||
|
||||
@@ -73,8 +75,7 @@ void CpTopicPublisher<T>::onInitialize()
|
||||
qos.durability(*durability);
|
||||
qos.reliability(*reliability);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Publisher to topic: " << topicName_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Publisher to topic: " << topicName_; return _oss.str(); }());
|
||||
|
||||
auto nh = this->getNode();
|
||||
pub_ = nh->template create_publisher<T>(this->topicName_, qos);
|
||||
|
||||
@@ -20,6 +20,8 @@
|
||||
|
||||
#pragma once
|
||||
#include <optional>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/client_bases/smacc_subscriber_client.hpp>
|
||||
#include <smacc2/component.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
@@ -93,8 +95,7 @@ public:
|
||||
|
||||
if (!queueSize) queueSize = 1;
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Subscribing to topic: " << topicName_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << this->getName() << "] Subscribing to topic: " << topicName_; return _oss.str(); }());
|
||||
|
||||
rclcpp::SensorDataQoS qos;
|
||||
if (queueSize) qos.keep_last(*queueSize);
|
||||
|
||||
@@ -20,6 +20,7 @@
|
||||
|
||||
#pragma once
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -35,9 +36,7 @@ void SmaccAsyncClientBehavior::onOrthogonalAllocation()
|
||||
{
|
||||
if (postFinishEventFn_ || postSuccessEventFn_ || postFailureEventFn_)
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
getLogger(),
|
||||
"SmaccAsyncClientBehavior already has event posting functions assigned. Skipping "
|
||||
LOG_WARN("SmaccAsyncClientBehavior already has event posting functions assigned. Skipping "
|
||||
"re-assignment. This could be a problem if you are using the same behavior in multiple "
|
||||
"states. This may be related with the deprecation of onOrthogonalAllocation in favor of "
|
||||
"onStateOrthogonalAllocation.");
|
||||
|
||||
@@ -21,6 +21,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/smacc_client_behavior.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -30,9 +31,7 @@ void ISmaccClientBehavior::postEvent(const EventType & ev)
|
||||
{
|
||||
if (stateMachine_ == nullptr)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
|
||||
LOG_ERROR("The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
|
||||
"post event call.");
|
||||
}
|
||||
else
|
||||
@@ -46,9 +45,7 @@ void ISmaccClientBehavior::postEvent()
|
||||
{
|
||||
if (stateMachine_ == nullptr)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
|
||||
LOG_ERROR("The client behavior cannot post events before being assigned to an orthogonal. Ignoring "
|
||||
"post event call.");
|
||||
}
|
||||
else
|
||||
@@ -75,9 +72,7 @@ void ISmaccClientBehavior::requiresComponent(
|
||||
{
|
||||
if (stateMachine_ == nullptr)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"Cannot use the requiresComponent funcionality before assigning the client behavior to an "
|
||||
LOG_ERROR("Cannot use the requiresComponent funcionality before assigning the client behavior to an "
|
||||
"orthogonal. Try using the OnEntry method to capture required components.");
|
||||
}
|
||||
else
|
||||
|
||||
@@ -21,6 +21,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/impl/smacc_state_machine_impl.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -99,11 +100,8 @@ SmaccComponentType * ISmaccClient::createNamedComponent(std::string name, TArgs.
|
||||
if (it == this->components_.end())
|
||||
{
|
||||
auto tname = demangledTypeName<SmaccComponentType>();
|
||||
RCLCPP_INFO(
|
||||
getLogger(),
|
||||
"Creating a new component of type: %s smacc component is required by client '%s'. Creating a "
|
||||
"new instance %s",
|
||||
this->getName().c_str(), demangledTypeName<SmaccComponentType>().c_str(), tname.c_str());
|
||||
LOG_INFO("Creating a new component of type: {} smacc component is required by client '{}'. Creating a "
|
||||
"new instance {}", this->getName().c_str(), demangledTypeName<SmaccComponentType>().c_str(), tname.c_str());
|
||||
|
||||
ret = std::shared_ptr<SmaccComponentType>(new SmaccComponentType(targs...));
|
||||
ret->setStateMachine(this->getStateMachine());
|
||||
@@ -114,13 +112,11 @@ SmaccComponentType * ISmaccClient::createNamedComponent(std::string name, TArgs.
|
||||
|
||||
this->components_[componentkey] =
|
||||
ret; //std::dynamic_pointer_cast<smacc2::ISmaccComponent>(ret);
|
||||
RCLCPP_DEBUG(getLogger(), "%s resource is required. Done.", tname.c_str());
|
||||
LOG_DEBUG("{} resource is required. Done.", tname.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "%s resource is required. Found resource in cache.",
|
||||
demangledTypeName<SmaccComponentType>().c_str());
|
||||
LOG_INFO("{} resource is required. Found resource in cache.", demangledTypeName<SmaccComponentType>().c_str());
|
||||
ret = dynamic_pointer_cast<SmaccComponentType>(it->second);
|
||||
}
|
||||
|
||||
|
||||
@@ -21,6 +21,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
#include <smacc2/component.hpp>
|
||||
#include <smacc2/impl/smacc_state_machine_impl.hpp>
|
||||
@@ -48,17 +50,16 @@ void ISmaccComponent::requiresComponent(
|
||||
|
||||
if (requiredComponentStorage == nullptr && throwExceptionIfNotExist)
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
this->getLogger(), std::string("Required component ") +
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << std::string("Required component ") +
|
||||
demangleSymbol(typeid(TComponent).name()) +
|
||||
" not found. Available components:");
|
||||
" not found. Available components:"; return _oss.str(); }());
|
||||
|
||||
std::vector<std::shared_ptr<ISmaccComponent>> components;
|
||||
this->owner_->getComponents(components);
|
||||
|
||||
for (auto c : components)
|
||||
{
|
||||
RCLCPP_DEBUG(this->getLogger(), "- Component %s", c->getName().c_str());
|
||||
LOG_DEBUG("- Component {}", c->getName().c_str());
|
||||
}
|
||||
|
||||
throw std::runtime_error(
|
||||
@@ -74,17 +75,16 @@ void ISmaccComponent::requiresComponent(
|
||||
|
||||
if (requiredComponentStorage == nullptr && throwExceptionIfNotExist)
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
this->getLogger(), std::string("Required component with name: '") + name + "'" +
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << std::string("Required component with name: '") + name + "'" +
|
||||
demangleSymbol(typeid(TComponent).name()) +
|
||||
" not found. Available components:");
|
||||
" not found. Available components:"; return _oss.str(); }());
|
||||
|
||||
std::vector<std::shared_ptr<ISmaccComponent>> components;
|
||||
this->owner_->getComponents(components);
|
||||
|
||||
for (auto c : components)
|
||||
{
|
||||
RCLCPP_DEBUG(this->getLogger(), " - Component %s", c->getName().c_str());
|
||||
LOG_DEBUG(" - Component {}", c->getName().c_str());
|
||||
}
|
||||
|
||||
throw std::runtime_error(
|
||||
|
||||
@@ -20,6 +20,8 @@
|
||||
|
||||
#pragma once
|
||||
#include <cassert>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
#include <smacc2/smacc_orthogonal.hpp>
|
||||
|
||||
@@ -35,10 +37,9 @@ bool ISmaccOrthogonal::requiresClient(SmaccClientType *& storage)
|
||||
}
|
||||
|
||||
auto requiredClientName = demangledTypeName<SmaccClientType>();
|
||||
RCLCPP_WARN_STREAM(
|
||||
getLogger(), "Required client ["
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Required client ["
|
||||
<< requiredClientName
|
||||
<< "] not found in current orthogonal. Searching in other orthogonals.");
|
||||
<< "] not found in current orthogonal. Searching in other orthogonals."; return _oss.str(); }());
|
||||
|
||||
for (auto & orthoentry : this->getStateMachine()->getOrthogonals())
|
||||
{
|
||||
@@ -47,19 +48,16 @@ bool ISmaccOrthogonal::requiresClient(SmaccClientType *& storage)
|
||||
storage = dynamic_cast<SmaccClientType *>(client.get());
|
||||
if (storage != nullptr)
|
||||
{
|
||||
RCLCPP_WARN_STREAM(
|
||||
getLogger(),
|
||||
"Required client [" << requiredClientName << "] found in other orthogonal.");
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Required client [" << requiredClientName << "] found in other orthogonal."; return _oss.str(); }());
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_ERROR_STREAM(
|
||||
getLogger(), "Required client ["
|
||||
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "Required client ["
|
||||
<< requiredClientName
|
||||
<< "] not found even in other orthogonals. Returning null pointer. If the "
|
||||
"requested client is used may result in a segmentation fault.");
|
||||
"requested client is used may result in a segmentation fault."; return _oss.str(); }());
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -68,9 +66,7 @@ void ISmaccOrthogonal::requiresComponent(SmaccComponentType *& storage)
|
||||
{
|
||||
if (stateMachine_ == nullptr)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"Cannot use the requiresComponent funcionality from an orthogonal before onInitialize");
|
||||
LOG_ERROR("Cannot use the requiresComponent funcionality from an orthogonal before onInitialize");
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -177,10 +173,7 @@ public:
|
||||
// return nullptr;
|
||||
// }
|
||||
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[%s] Creating client object, type:'%s' object tag: '%s'",
|
||||
demangleType(typeid(*this)).c_str(), demangledTypeName<TClient>().c_str(),
|
||||
demangledTypeName<TOrthogonal>().c_str());
|
||||
LOG_INFO("[{}] Creating client object, type:'{}' object tag: '{}'", demangleType(typeid(*this)).c_str(), demangledTypeName<TClient>().c_str(), demangledTypeName<TOrthogonal>().c_str());
|
||||
|
||||
auto client = std::make_shared<ClientHandler<TOrthogonal, TClient>>(args...);
|
||||
this->template assignClientToOrthogonal<TOrthogonal, TClient>(client.get());
|
||||
|
||||
@@ -20,6 +20,7 @@
|
||||
|
||||
#pragma once
|
||||
#include <smacc2/introspection/introspection.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_client_behavior.hpp>
|
||||
#include <smacc2/smacc_orthogonal.hpp>
|
||||
#include <smacc2/smacc_state.hpp>
|
||||
@@ -37,8 +38,7 @@ template <typename TOrthogonal, typename TBehavior, typename... Args>
|
||||
std::shared_ptr<TBehavior> ISmaccState::configure(Args &&... args)
|
||||
{
|
||||
std::string orthogonalkey = demangledTypeName<TOrthogonal>();
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[%s] Configuring orthogonal: %s", THIS_STATE_NAME, orthogonalkey.c_str());
|
||||
LOG_INFO("[{}] Configuring orthogonal: {}", THIS_STATE_NAME, orthogonalkey.c_str());
|
||||
|
||||
TOrthogonal * orthogonal = this->getOrthogonal<TOrthogonal>();
|
||||
if (orthogonal != nullptr)
|
||||
@@ -53,9 +53,7 @@ std::shared_ptr<TBehavior> ISmaccState::configure(Args &&... args)
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(), "[%s] Skipping client behavior creation in orthogonal [%s]. It does not exist.",
|
||||
THIS_STATE_NAME, orthogonalkey.c_str());
|
||||
LOG_ERROR("[{}] Skipping client behavior creation in orthogonal [{}]. It does not exist.", THIS_STATE_NAME, orthogonalkey.c_str());
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
@@ -80,11 +78,8 @@ void ISmaccState::requiresClient(SmaccClientType *& storage)
|
||||
if (storage != nullptr) return;
|
||||
}
|
||||
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[%s] Client of type '%s' not found in any orthogonal of the current state machine. This may "
|
||||
"produce a segmentation fault if the returned reference is used.",
|
||||
sname, demangleSymbol<SmaccClientType>().c_str());
|
||||
LOG_ERROR("[{}] Client of type '{}' not found in any orthogonal of the current state machine. This may "
|
||||
"produce a segmentation fault if the returned reference is used.", sname, demangleSymbol<SmaccClientType>().c_str());
|
||||
}
|
||||
//-------------------------------------------------------------------------------------------------------
|
||||
|
||||
|
||||
@@ -21,6 +21,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
@@ -63,12 +64,9 @@ TOrthogonal * ISmaccStateMachine::getOrthogonal()
|
||||
|
||||
if (it != orthogonals_.end())
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(),
|
||||
"Orthogonal %s resource is being required from some state, client or component. Found "
|
||||
LOG_DEBUG("Orthogonal {} resource is being required from some state, client or component. Found "
|
||||
"resource in "
|
||||
"cache.",
|
||||
orthogonalkey.c_str());
|
||||
"cache.", orthogonalkey.c_str());
|
||||
ret = dynamic_cast<TOrthogonal *>(it->second.get());
|
||||
return ret;
|
||||
}
|
||||
@@ -82,7 +80,7 @@ TOrthogonal * ISmaccStateMachine::getOrthogonal()
|
||||
ss << " - " << orthogonal.first << std::endl;
|
||||
}
|
||||
|
||||
RCLCPP_WARN_STREAM(getLogger(), ss.str());
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << ss.str(); return _oss.str(); }());
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
@@ -104,20 +102,19 @@ void ISmaccStateMachine::createOrthogonal()
|
||||
|
||||
ret->setStateMachine(this);
|
||||
|
||||
RCLCPP_INFO(getLogger(), "%s Orthogonal is created", orthogonalkey.c_str());
|
||||
LOG_INFO("{} Orthogonal is created", orthogonalkey.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN_STREAM(
|
||||
getLogger(), "There were already one existing orthogonal of type "
|
||||
<< orthogonalkey.c_str() << ". Skipping creation orthogonal request. ");
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "There were already one existing orthogonal of type "
|
||||
<< orthogonalkey.c_str() << ". Skipping creation orthogonal request. "; return _oss.str(); }());
|
||||
std::stringstream ss;
|
||||
ss << "The existing orthogonals are the following: " << std::endl;
|
||||
for (auto & orthogonal : orthogonals_)
|
||||
{
|
||||
ss << " - " << orthogonal.first << std::endl;
|
||||
}
|
||||
RCLCPP_WARN_STREAM(getLogger(), ss.str());
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << ss.str(); return _oss.str(); }());
|
||||
}
|
||||
//this->unlockStateMachine("create orthogonal");
|
||||
}
|
||||
@@ -126,9 +123,7 @@ void ISmaccStateMachine::createOrthogonal()
|
||||
template <typename SmaccComponentType>
|
||||
void ISmaccStateMachine::requiresComponent(SmaccComponentType *& storage, bool throwsException)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "component %s is required",
|
||||
demangleSymbol(typeid(SmaccComponentType).name()).c_str());
|
||||
LOG_DEBUG("component {} is required", demangleSymbol(typeid(SmaccComponentType).name()).c_str());
|
||||
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
|
||||
|
||||
for (auto ortho : this->orthogonals_)
|
||||
@@ -143,9 +138,7 @@ void ISmaccStateMachine::requiresComponent(SmaccComponentType *& storage, bool t
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_WARN(
|
||||
getLogger(), "component %s is required but it was not found in any orthogonal",
|
||||
demangleSymbol(typeid(SmaccComponentType).name()).c_str());
|
||||
LOG_WARN("component {} is required but it was not found in any orthogonal", demangleSymbol(typeid(SmaccComponentType).name()).c_str());
|
||||
|
||||
if (throwsException)
|
||||
throw std::runtime_error("component is required but it was not found in any orthogonal");
|
||||
@@ -188,10 +181,8 @@ void ISmaccStateMachine::postEvent(EventType * ev, EventLifeTime evlifetime)
|
||||
(stateMachineCurrentAction == StateMachineInternalAction::STATE_EXITING ||
|
||||
stateMachineCurrentAction == StateMachineInternalAction::TRANSITIONING))
|
||||
{
|
||||
RCLCPP_WARN_STREAM(
|
||||
getLogger(),
|
||||
"[ISmaccStateMachine] CURRENT STATE SCOPED EVENT DISCARDED, state is exiting/transitioning "
|
||||
<< eventtypename);
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ISmaccStateMachine] CURRENT STATE SCOPED EVENT DISCARDED, state is exiting/transitioning "
|
||||
<< eventtypename; return _oss.str(); }());
|
||||
return;
|
||||
// in this case we may lose/skip events, if this is not right for some cases we should create a
|
||||
// queue to lock the events during the transitions. This issues appeared when a client
|
||||
@@ -203,7 +194,7 @@ void ISmaccStateMachine::postEvent(EventType * ev, EventLifeTime evlifetime)
|
||||
// we reach this place. Now, we propagate the events to all the state state reactors to generate
|
||||
// some more events
|
||||
|
||||
RCLCPP_DEBUG_STREAM(getLogger(), "[PostEvent entry point] " << eventtypename);
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[PostEvent entry point] " << eventtypename; return _oss.str(); }());
|
||||
for (auto currentState : currentState_)
|
||||
{
|
||||
propagateEventToStateReactors(currentState, ev);
|
||||
@@ -216,7 +207,7 @@ template <typename EventType>
|
||||
void ISmaccStateMachine::postEvent(EventLifeTime evlifetime)
|
||||
{
|
||||
auto evname = smacc2::introspection::demangleSymbol<EventType>();
|
||||
RCLCPP_INFO_STREAM(getLogger(), "Event: " << evname);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Event: " << evname; return _oss.str(); }());
|
||||
auto * ev = new EventType();
|
||||
this->postEvent(ev, evlifetime);
|
||||
}
|
||||
@@ -247,7 +238,7 @@ bool ISmaccStateMachine::getGlobalSMData(std::string name, T & ret)
|
||||
}
|
||||
catch (boost::bad_any_cast & ex)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "bad any cast: %s", ex.what());
|
||||
LOG_ERROR("bad any cast: {}", ex.what());
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
@@ -282,9 +273,7 @@ void ISmaccStateMachine::mapBehavior()
|
||||
{
|
||||
std::string stateFieldName = demangleSymbol(typeid(StateField).name());
|
||||
std::string behaviorType = demangleSymbol(typeid(BehaviorType).name());
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "Mapping state field '%s' to stateReactor '%s'", stateFieldName.c_str(),
|
||||
behaviorType.c_str());
|
||||
LOG_INFO("Mapping state field '{}' to stateReactor '{}'", stateFieldName.c_str(), behaviorType.c_str());
|
||||
SmaccClientBehavior * globalreference;
|
||||
if (!this->getGlobalSMData(stateFieldName, globalreference))
|
||||
{
|
||||
@@ -433,12 +422,8 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
|
||||
std::is_base_of<ISmaccOrthogonal, TSmaccObjectType>::value ||
|
||||
std::is_base_of<ISmaccStateMachine, TSmaccObjectType>::value)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(),
|
||||
"[StateMachine] Long life-time SMACC signal subscription created. Subscriber is %s. Callback "
|
||||
"is: %s",
|
||||
demangledTypeName<TSmaccObjectType>().c_str(),
|
||||
demangleSymbol(typeid(callback).name()).c_str());
|
||||
LOG_INFO("[StateMachine] Long life-time SMACC signal subscription created. Subscriber is {}. Callback "
|
||||
"is: {}", demangledTypeName<TSmaccObjectType>().c_str(), demangleSymbol(typeid(callback).name()).c_str());
|
||||
|
||||
connection = binder.bindaux(signal, callback, object, nullptr);
|
||||
}
|
||||
@@ -447,10 +432,7 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
|
||||
std::is_base_of<StateReactor, TSmaccObjectType>::value ||
|
||||
std::is_base_of<ISmaccClientBehavior, TSmaccObjectType>::value)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(),
|
||||
"[StateMachine] Life-time constrained SMACC signal subscription created. Subscriber is %s",
|
||||
demangledTypeName<TSmaccObjectType>().c_str());
|
||||
LOG_INFO("[StateMachine] Life-time constrained SMACC signal subscription created. Subscriber is {}", demangledTypeName<TSmaccObjectType>().c_str());
|
||||
|
||||
std::shared_ptr<CallbackCounterSemaphore> callbackCounterSemaphore;
|
||||
if (stateCallbackConnections.count(object))
|
||||
@@ -469,9 +451,7 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
|
||||
}
|
||||
else // state life-time objects
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
getLogger(),
|
||||
"[StateMachine] Connecting signal to an unknown object with unknown lifetime "
|
||||
LOG_WARN("[StateMachine] Connecting signal to an unknown object with unknown lifetime "
|
||||
"behavior. An exception may occur if the object is destroyed during the execution.");
|
||||
|
||||
connection = binder.bindaux(signal, callback, object, nullptr);
|
||||
@@ -485,11 +465,8 @@ void ISmaccStateMachine::notifyOnStateEntryStart(StateType * state)
|
||||
{
|
||||
std::lock_guard<std::recursive_mutex> lock(m_mutex_);
|
||||
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(),
|
||||
"[State Machine] Initializing a new state '%s' and updating current state. Getting state "
|
||||
"meta-information. number of orthogonals: %ld",
|
||||
demangleSymbol(typeid(StateType).name()).c_str(), this->orthogonals_.size());
|
||||
LOG_DEBUG("[State Machine] Initializing a new state '{}' and updating current state. Getting state "
|
||||
"meta-information. number of orthogonals: {}", demangleSymbol(typeid(StateType).name()).c_str(), this->orthogonals_.size());
|
||||
|
||||
stateSeqCounter_++;
|
||||
currentState_.push_back(state);
|
||||
@@ -499,14 +476,12 @@ void ISmaccStateMachine::notifyOnStateEntryStart(StateType * state)
|
||||
template <typename StateType>
|
||||
void ISmaccStateMachine::notifyOnStateEntryEnd(StateType *)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[%s] State OnEntry code finished.",
|
||||
demangleSymbol(typeid(StateType).name()).c_str());
|
||||
LOG_INFO("[{}] State OnEntry code finished.", demangleSymbol(typeid(StateType).name()).c_str());
|
||||
|
||||
auto currentState = this->currentState_.back();
|
||||
for (auto pair : this->orthogonals_)
|
||||
{
|
||||
RCLCPP_DEBUG(getLogger(), "Orthogonal onEntry: %s.", pair.second->getName().c_str());
|
||||
LOG_DEBUG("Orthogonal onEntry: {}.", pair.second->getName().c_str());
|
||||
auto & orthogonal = pair.second;
|
||||
try
|
||||
{
|
||||
@@ -514,46 +489,37 @@ void ISmaccStateMachine::notifyOnStateEntryEnd(StateType *)
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[Orthogonal %s] Exception on Entry - continuing with next orthogonal. Exception info: %s",
|
||||
pair.second->getName().c_str(), e.what());
|
||||
LOG_ERROR("[Orthogonal {}] Exception on Entry - continuing with next orthogonal. Exception info: {}", pair.second->getName().c_str(), e.what());
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & sr : currentState->getStateReactors())
|
||||
{
|
||||
auto srname = smacc2::demangleSymbol(typeid(*sr).name());
|
||||
RCLCPP_INFO_STREAM(getLogger(), "State reactor onEntry: " << srname);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "State reactor onEntry: " << srname; return _oss.str(); }());
|
||||
try
|
||||
{
|
||||
sr->onEntry();
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[State Reactor %s] Exception on Entry - continuing with next state reactor. Exception "
|
||||
"info: %s",
|
||||
srname.c_str(), e.what());
|
||||
LOG_ERROR("[State Reactor {}] Exception on Entry - continuing with next state reactor. Exception "
|
||||
"info: {}", srname.c_str(), e.what());
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & eg : currentState->getEventGenerators())
|
||||
{
|
||||
auto egname = smacc2::demangleSymbol(typeid(*eg).name());
|
||||
RCLCPP_INFO_STREAM(getLogger(), "Event generator onEntry: " << egname);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Event generator onEntry: " << egname; return _oss.str(); }());
|
||||
try
|
||||
{
|
||||
eg->onEntry();
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[Event generator %s] Exception on Entry - continuing with next state reactor. Exception "
|
||||
"info: %s",
|
||||
egname.c_str(), e.what());
|
||||
LOG_ERROR("[Event generator {}] Exception on Entry - continuing with next state reactor. Exception "
|
||||
"info: {}", egname.c_str(), e.what());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -594,10 +560,10 @@ void ISmaccStateMachine::notifyOnStateExitting(StateType * state)
|
||||
{
|
||||
stateMachineCurrentAction = StateMachineInternalAction::STATE_EXITING;
|
||||
auto fullname = demangleSymbol(typeid(StateType).name());
|
||||
RCLCPP_WARN_STREAM(getLogger(), "Exiting state: " << fullname);
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Exiting state: " << fullname; return _oss.str(); }());
|
||||
// this->set_parameter("destroyed", true);
|
||||
|
||||
RCLCPP_INFO_STREAM(getLogger(), "Notification state exit: leaving state " << state);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Notification state exit: leaving state " << state; return _oss.str(); }());
|
||||
for (auto pair : this->orthogonals_)
|
||||
{
|
||||
auto & orthogonal = pair.second;
|
||||
@@ -607,46 +573,37 @@ void ISmaccStateMachine::notifyOnStateExitting(StateType * state)
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[Orthogonal %s] Exception onExit - continuing with next orthogonal. Exception info: %s",
|
||||
pair.second->getName().c_str(), e.what());
|
||||
LOG_ERROR("[Orthogonal {}] Exception onExit - continuing with next orthogonal. Exception info: {}", pair.second->getName().c_str(), e.what());
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & sr : state->getStateReactors())
|
||||
{
|
||||
auto srname = smacc2::demangleSymbol(typeid(*sr).name());
|
||||
RCLCPP_INFO_STREAM(getLogger(), "State reactor OnExit: " << srname);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "State reactor OnExit: " << srname; return _oss.str(); }());
|
||||
try
|
||||
{
|
||||
sr->onExit();
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
|
||||
"info: %s",
|
||||
srname.c_str(), e.what());
|
||||
LOG_ERROR("[State Reactor {}] Exception on OnExit - continuing with next state reactor. Exception "
|
||||
"info: {}", srname.c_str(), e.what());
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & eg : state->getEventGenerators())
|
||||
{
|
||||
auto egname = smacc2::demangleSymbol(typeid(*eg).name());
|
||||
RCLCPP_INFO_STREAM(getLogger(), "Event generator OnExit: " << egname);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Event generator OnExit: " << egname; return _oss.str(); }());
|
||||
try
|
||||
{
|
||||
eg->onExit();
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[State Reactor %s] Exception on OnExit - continuing with next state reactor. Exception "
|
||||
"info: %s",
|
||||
egname.c_str(), e.what());
|
||||
LOG_ERROR("[State Reactor {}] Exception on OnExit - continuing with next state reactor. Exception "
|
||||
"info: {}", egname.c_str(), e.what());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -659,9 +616,9 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
|
||||
signalDetector_->notifyStateExited(state);
|
||||
|
||||
auto fullname = demangleSymbol(typeid(StateType).name());
|
||||
RCLCPP_WARN_STREAM(getLogger(), "Exiting state: " << fullname);
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Exiting state: " << fullname; return _oss.str(); }());
|
||||
|
||||
RCLCPP_INFO_STREAM(getLogger(), "Notification state disposing: leaving state" << state);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Notification state disposing: leaving state" << state; return _oss.str(); }());
|
||||
for (auto pair : this->orthogonals_)
|
||||
{
|
||||
auto & orthogonal = pair.second;
|
||||
@@ -671,46 +628,37 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[Orthogonal %s] Exception onDispose - continuing with next orthogonal. Exception info: %s",
|
||||
pair.second->getName().c_str(), e.what());
|
||||
LOG_ERROR("[Orthogonal {}] Exception onDispose - continuing with next orthogonal. Exception info: {}", pair.second->getName().c_str(), e.what());
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & sr : state->getStateReactors())
|
||||
{
|
||||
auto srname = smacc2::demangleSymbol(typeid(*sr).name()).c_str();
|
||||
RCLCPP_INFO(getLogger(), "State reactor disposing: %s", srname);
|
||||
LOG_INFO("State reactor disposing: {}", srname);
|
||||
try
|
||||
{
|
||||
this->disconnectSmaccSignalObject(sr.get());
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
|
||||
"info: %s",
|
||||
srname, e.what());
|
||||
LOG_ERROR("[State Reactor {}] Exception on OnDispose - continuing with next state reactor. Exception "
|
||||
"info: {}", srname, e.what());
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & eg : state->getEventGenerators())
|
||||
{
|
||||
auto egname = smacc2::demangleSymbol(typeid(*eg).name()).c_str();
|
||||
RCLCPP_INFO(getLogger(), "Event generator disposing: %s", egname);
|
||||
LOG_INFO("Event generator disposing: {}", egname);
|
||||
try
|
||||
{
|
||||
this->disconnectSmaccSignalObject(eg.get());
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[State Reactor %s] Exception on OnDispose - continuing with next state reactor. Exception "
|
||||
"info: %s",
|
||||
egname, e.what());
|
||||
LOG_ERROR("[State Reactor {}] Exception on OnDispose - continuing with next state reactor. Exception "
|
||||
"info: {}", egname, e.what());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -718,7 +666,7 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
|
||||
currentState_.pop_back();
|
||||
|
||||
// then call exit state
|
||||
RCLCPP_WARN_STREAM(getLogger(), "State exit: " << fullname);
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "State exit: " << fullname; return _oss.str(); }());
|
||||
|
||||
stateMachineCurrentAction = StateMachineInternalAction::TRANSITIONING;
|
||||
this->unlockStateMachine("state exit");
|
||||
@@ -727,9 +675,7 @@ void ISmaccStateMachine::notifyOnStateExited(StateType * state)
|
||||
template <typename EventType>
|
||||
void ISmaccStateMachine::propagateEventToStateReactors(ISmaccState * st, EventType * ev)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "PROPAGATING EVENT [%s] TO SRs [%s]: ", demangleSymbol<EventType>().c_str(),
|
||||
st->getClassName().c_str());
|
||||
LOG_DEBUG("PROPAGATING EVENT [{}] TO SRs [{}]: ", demangleSymbol<EventType>().c_str(), st->getClassName().c_str());
|
||||
for (auto & sb : st->getStateReactors())
|
||||
{
|
||||
sb->notifyEvent(ev);
|
||||
|
||||
@@ -20,6 +20,8 @@
|
||||
|
||||
#pragma once
|
||||
#include <smacc2/introspection/introspection.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_state_reactor.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -41,8 +43,7 @@ void StateReactor::setOutputEvent()
|
||||
{
|
||||
this->postEventFn = [this]()
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->getLogger(), "[State Reactor Base] postingfn posting event: " << demangleSymbol<TEv>());
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[State Reactor Base] postingfn posting event: " << demangleSymbol<TEv>(); return _oss.str(); }());
|
||||
auto * ev = new TEv();
|
||||
this->ownerState->getStateMachine().postEvent(ev);
|
||||
};
|
||||
@@ -84,9 +85,7 @@ void StateReactorHandler::addInputEvent()
|
||||
StateReactorCallbackFunctor functor;
|
||||
functor.fn = [this](std::shared_ptr<smacc2::StateReactor> sr)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
nh_->get_logger(), "[%s] State Reactor adding input event: %s",
|
||||
srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
|
||||
LOG_INFO("[{}] State Reactor adding input event: {}", srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
|
||||
sr->addInputEvent<TEv>();
|
||||
};
|
||||
|
||||
@@ -105,9 +104,7 @@ void StateReactorHandler::setOutputEvent()
|
||||
StateReactorCallbackFunctor functor;
|
||||
functor.fn = [this](std::shared_ptr<smacc2::StateReactor> sr)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
nh_->get_logger(), "[%s] State Reactor setting output event: %s",
|
||||
srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
|
||||
LOG_INFO("[{}] State Reactor setting output event: {}", srInfo_->stateReactorType->getFullName().c_str(), demangledTypeName<TEv>().c_str());
|
||||
sr->setOutputEvent<TEv>();
|
||||
};
|
||||
|
||||
|
||||
@@ -21,6 +21,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
@@ -160,9 +162,7 @@ template <typename T>
|
||||
typename enable_if<boost::mpl::is_sequence<T>>::type processTransitions(
|
||||
std::shared_ptr<SmaccStateInfo> & sourceState)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
globalNh_->get_logger(), "State %s Walker has transition list",
|
||||
sourceState->fullStateName.c_str());
|
||||
LOG_INFO("State {} Walker has transition list", sourceState->fullStateName.c_str());
|
||||
using boost::mpl::_1;
|
||||
using wrappedList = typename boost::mpl::transform<T, add_type_wrapper<_1>>::type;
|
||||
boost::mpl::for_each<wrappedList>(AddTransition(sourceState));
|
||||
@@ -184,9 +184,7 @@ void processTransition(
|
||||
smacc2::Transition<Ev, Dst, Tag> * t, std::shared_ptr<SmaccStateInfo> & sourceState)
|
||||
{
|
||||
auto transitionTypeInfo = TypeInfo::getTypeInfoFromType<smacc2::Transition<Ev, Dst, Tag>>();
|
||||
RCLCPP_INFO(
|
||||
globalNh_->get_logger(), "State %s Walker transition: %s", sourceState->toShortName().c_str(),
|
||||
demangleSymbol(typeid(Ev).name()).c_str());
|
||||
LOG_INFO("State {} Walker transition: {}", sourceState->toShortName().c_str(), demangleSymbol(typeid(Ev).name()).c_str());
|
||||
processTransitionAux(t, sourceState, false, transitionTypeInfo);
|
||||
}
|
||||
|
||||
@@ -195,9 +193,7 @@ void processTransitionAux(
|
||||
smacc2::Transition<Ev, Dst, Tag> *, std::shared_ptr<SmaccStateInfo> & sourceState, bool history,
|
||||
TypeInfo::Ptr & transitionTypeInfo)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
globalNh_->get_logger(), "State %s Walker transition: %s", sourceState->toShortName().c_str(),
|
||||
demangleSymbol(typeid(Ev).name()).c_str());
|
||||
LOG_INFO("State {} Walker transition: {}", sourceState->toShortName().c_str(), demangleSymbol(typeid(Ev).name()).c_str());
|
||||
std::string transitionTag;
|
||||
std::string transitionType;
|
||||
|
||||
@@ -205,7 +201,7 @@ void processTransitionAux(
|
||||
{
|
||||
transitionTag = demangleSymbol<Tag>();
|
||||
transitionType = getTransitionType<Tag>();
|
||||
RCLCPP_DEBUG_STREAM(globalNh_->get_logger(), "TRANSITION TYPE:" << transitionType);
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "TRANSITION TYPE:" << transitionType; return _oss.str(); }());
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -214,7 +210,7 @@ void processTransitionAux(
|
||||
automaticTransitionType<Ev>(transitionType);
|
||||
}
|
||||
|
||||
RCLCPP_INFO_STREAM(globalNh_->get_logger(), "Transition tag: " << transitionTag);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Transition tag: " << transitionTag; return _oss.str(); }());
|
||||
|
||||
if (!sourceState->stateMachine_->containsState<Dst>())
|
||||
{
|
||||
@@ -350,17 +346,14 @@ template <typename T>
|
||||
typename std::enable_if<HasOnDefinition<T>::value, void>::type CallOnDefinition()
|
||||
{
|
||||
/* something when T has toString ... */
|
||||
RCLCPP_INFO_STREAM(
|
||||
globalNh_->get_logger(), "EXECUTING ONDEFINITION: " << demangleSymbol(typeid(T).name()));
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "EXECUTING ONDEFINITION: " << demangleSymbol(typeid(T).name()); return _oss.str(); }());
|
||||
T::staticConfigure();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
typename std::enable_if<!HasOnDefinition<T>::value, void>::type CallOnDefinition()
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
globalNh_->get_logger(),
|
||||
"static OnDefinition: dont exist for " << demangleSymbol(typeid(T).name()));
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "static OnDefinition: dont exist for " << demangleSymbol(typeid(T).name()); return _oss.str(); }());
|
||||
/* something when T has toString ... */
|
||||
}
|
||||
|
||||
@@ -371,7 +364,7 @@ template <typename> typename EvType, typename Tag, typename DestinyState >
|
||||
typename disable_if<boost::mpl::is_sequence<TTransition<EvType<TevSource>,DestinyState, Tag>>>::type
|
||||
processTransitions(std::shared_ptr<SmaccStateInfo> &sourceState)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(),"DETECTED COMPLEX TRANSITION **************");
|
||||
LOG_INFO("DETECTED COMPLEX TRANSITION **************");
|
||||
// RCLCPP_INFO_STREAM(getLogger(),"state transition from: " << sourceState->demangledStateName
|
||||
<< " of type: " << demangledTypeName<T>());
|
||||
TTransition<EvType<TevSource>,DestinyState, Tag> *dummy;
|
||||
@@ -383,7 +376,7 @@ template <typename> typename EvType, typename DestinyState >
|
||||
typename disable_if<boost::mpl::is_sequence<TTransition<EvType<TevSource>,DestinyState>>>::type
|
||||
processTransitions(std::shared_ptr<SmaccStateInfo> &sourceState)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(),"DETECTED COMPLEX TRANSITION **************");
|
||||
LOG_INFO("DETECTED COMPLEX TRANSITION **************");
|
||||
// RCLCPP_INFO_STREAM(getLogger(),"state transition from: " << sourceState->demangledStateName
|
||||
<< " of type: " << demangledTypeName<T>());
|
||||
TTransition<EvType<TevSource>,DestinyState> *dummy;
|
||||
@@ -480,7 +473,7 @@ std::shared_ptr<SmaccStateInfo> SmaccStateMachineInfo::createState(
|
||||
auto * statetid = &(typeid(StateType));
|
||||
|
||||
auto demangledName = demangledTypeName<StateType>();
|
||||
RCLCPP_INFO_STREAM(getLogger(), "Creating State Info: " << demangledName);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Creating State Info: " << demangledName; return _oss.str(); }());
|
||||
|
||||
auto state = std::make_shared<SmaccStateInfo>(statetid, parent, thisptr);
|
||||
state->demangledStateName = demangledName;
|
||||
@@ -510,8 +503,7 @@ std::shared_ptr<SmaccStateInfo> SmaccStateInfo::createChildState()
|
||||
|
||||
auto childState = this->stateMachine_->createState<StateType>(realparentState);
|
||||
|
||||
RCLCPP_WARN_STREAM(
|
||||
getLogger(), "Real parent state> " << demangleSymbol<typename StateType::TContext>());
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "Real parent state> " << demangleSymbol<typename StateType::TContext>(); return _oss.str(); }());
|
||||
|
||||
/*auto contextInfo = TypeInfo::getTypeInfoFromType<InitialStateType>();
|
||||
auto parentState2= getState<InitialStateType::TContext>();
|
||||
|
||||
@@ -20,6 +20,8 @@
|
||||
|
||||
#pragma once
|
||||
#include <smacc2/introspection/state_traits.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_event_generator.hpp>
|
||||
#include <smacc2/smacc_state.hpp>
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
@@ -76,7 +78,7 @@ public:
|
||||
logger_.reset(new rclcpp::Logger(
|
||||
rclcpp::get_logger(smacc2::utils::cleanShortTypeName(typeid(MostDerived)))));
|
||||
|
||||
RCLCPP_INFO(getLogger(), "[%s] Creating state ", STATE_NAME);
|
||||
LOG_INFO("[{}] Creating state ", STATE_NAME);
|
||||
this->set_context(ctx.pContext_);
|
||||
|
||||
node_ = this->getStateMachine().getNode();
|
||||
@@ -345,7 +347,7 @@ public:
|
||||
{
|
||||
this->postEvent<EvLoopEnd<MostDerived>>();
|
||||
}
|
||||
RCLCPP_INFO(getLogger(), "[%s] POST THROW CONDITION", STATE_NAME);
|
||||
LOG_INFO("[{}] POST THROW CONDITION", STATE_NAME);
|
||||
}
|
||||
|
||||
void throwSequenceFinishedEvent() { this->postEvent<EvSequenceFinished<MostDerived>>(); }
|
||||
@@ -406,38 +408,32 @@ private:
|
||||
SmaccStateInfo::staticBehaviorInfo[tindex] = std::vector<ClientBehaviorInfoEntry>();
|
||||
|
||||
SmaccStateInfo::staticBehaviorInfo[tindex].push_back(bhinfo);
|
||||
RCLCPP_INFO_STREAM(
|
||||
rclcpp::get_logger("static"), "[states walking] State "
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[states walking] State "
|
||||
<< smacc2::utils::cleanShortTypeName(*tindex)
|
||||
<< "client behavior count: "
|
||||
<< SmaccStateInfo::staticBehaviorInfo[tindex].size());
|
||||
<< SmaccStateInfo::staticBehaviorInfo[tindex].size(); return _oss.str(); }());
|
||||
}
|
||||
|
||||
void entryStateInternal()
|
||||
{
|
||||
// finally we go to the derived state onEntry Function
|
||||
|
||||
RCLCPP_DEBUG(getLogger(), "[%s] State object created. Initializating...", STATE_NAME);
|
||||
LOG_DEBUG("[{}] State object created. Initializating...", STATE_NAME);
|
||||
this->getStateMachine().notifyOnStateEntryStart(static_cast<MostDerived *>(this));
|
||||
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(), "[" << smacc2::utils::cleanShortTypeName(typeid(MostDerived)).c_str()
|
||||
<< "] creating ros subnode");
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << smacc2::utils::cleanShortTypeName(typeid(MostDerived)).c_str()
|
||||
<< "] creating ros subnode"; return _oss.str(); }());
|
||||
|
||||
// before dynamic runtimeConfigure, we execute the staticConfigure behavior configurations
|
||||
{
|
||||
RCLCPP_DEBUG(getLogger(), "[%s] -- STATIC STATE DESCRIPTION --", STATE_NAME);
|
||||
LOG_DEBUG("[{}] -- STATIC STATE DESCRIPTION --", STATE_NAME);
|
||||
|
||||
for (const auto & clientBehavior : SmaccStateInfo::staticBehaviorInfo)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] client behavior info: %s", STATE_NAME,
|
||||
demangleSymbol(clientBehavior.first->name()).c_str());
|
||||
LOG_DEBUG("[{}] client behavior info: {}", STATE_NAME, demangleSymbol(clientBehavior.first->name()).c_str());
|
||||
for (auto & cbinfo : clientBehavior.second)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] client behavior: %s", STATE_NAME,
|
||||
demangleSymbol(cbinfo.behaviorType->name()).c_str());
|
||||
LOG_DEBUG("[{}] client behavior: {}", STATE_NAME, demangleSymbol(cbinfo.behaviorType->name()).c_str());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -448,45 +444,36 @@ private:
|
||||
|
||||
for (auto & ortho : this->getStateMachine().getOrthogonals())
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[%s] Initializing orthogonal: %s", STATE_NAME,
|
||||
demangleSymbol(typeid(*ortho.second).name()).c_str());
|
||||
LOG_INFO("[{}] Initializing orthogonal: {}", STATE_NAME, demangleSymbol(typeid(*ortho.second).name()).c_str());
|
||||
ortho.second->initState(this);
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(), "Finding static client behaviors. State Database: "
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Finding static client behaviors. State Database: "
|
||||
<< SmaccStateInfo::staticBehaviorInfo.size() << ". Current state "
|
||||
<< cleanShortTypeName(*tindex)
|
||||
<< " cbs: " << SmaccStateInfo::staticBehaviorInfo[tindex].size());
|
||||
<< " cbs: " << SmaccStateInfo::staticBehaviorInfo[tindex].size(); return _oss.str(); }());
|
||||
for (auto & bhinfo : staticDefinedBehaviors)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] Creating static client behavior: %s", STATE_NAME,
|
||||
demangleSymbol(bhinfo.behaviorType->name()).c_str());
|
||||
LOG_DEBUG("[{}] Creating static client behavior: {}", STATE_NAME, demangleSymbol(bhinfo.behaviorType->name()).c_str());
|
||||
bhinfo.factoryFunction(this);
|
||||
}
|
||||
|
||||
for (auto & sr : staticDefinedStateReactors)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] Creating static state reactor: %s", STATE_NAME,
|
||||
sr->stateReactorType->getFullName().c_str());
|
||||
LOG_DEBUG("[{}] Creating static state reactor: {}", STATE_NAME, sr->stateReactorType->getFullName().c_str());
|
||||
sr->factoryFunction(this);
|
||||
}
|
||||
|
||||
for (auto & eg : staticDefinedEventGenerators)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] Creating static event generator: %s", STATE_NAME,
|
||||
eg->eventGeneratorType->getFullName().c_str());
|
||||
LOG_DEBUG("[{}] Creating static event generator: {}", STATE_NAME, eg->eventGeneratorType->getFullName().c_str());
|
||||
eg->factoryFunction(this);
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(getLogger(), "[%s] ---- END STATIC DESCRIPTION", STATE_NAME);
|
||||
LOG_DEBUG("[{}] ---- END STATIC DESCRIPTION", STATE_NAME);
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(getLogger(), "[%s] State runtime configuration", STATE_NAME);
|
||||
LOG_DEBUG("[{}] State runtime configuration", STATE_NAME);
|
||||
|
||||
auto * derivedthis = static_cast<MostDerived *>(this);
|
||||
|
||||
@@ -500,7 +487,7 @@ private:
|
||||
|
||||
this->getStateMachine().notifyOnRuntimeConfigurationFinished(derivedthis);
|
||||
|
||||
RCLCPP_DEBUG(getLogger(), "[%s] State OnEntry", STATE_NAME);
|
||||
LOG_DEBUG("[{}] State OnEntry", STATE_NAME);
|
||||
|
||||
static_cast<MostDerived *>(this)->onEntry();
|
||||
|
||||
|
||||
@@ -19,6 +19,7 @@
|
||||
******************************************************************************************************************/
|
||||
#pragma once
|
||||
#include <smacc2/common.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
#include <smacc2/smacc_state_base.hpp>
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
@@ -74,15 +75,15 @@ public:
|
||||
// this is before because this creates orthogonals
|
||||
this->onInitialize();
|
||||
|
||||
RCLCPP_INFO(getLogger(), "[SmaccStateMachine] Introspecting state machine via typeWalker");
|
||||
LOG_INFO("[SmaccStateMachine] Introspecting state machine via typeWalker");
|
||||
this->buildStateMachineInfo<InitialStateType>();
|
||||
|
||||
RCLCPP_INFO(getLogger(), "[SmaccStateMachine] initiate_impl");
|
||||
LOG_INFO("[SmaccStateMachine] initiate_impl");
|
||||
auto shortname = smacc2::utils::cleanShortTypeName(typeid(DerivedStateMachine));
|
||||
|
||||
this->initializeROS(shortname);
|
||||
|
||||
RCLCPP_INFO(getLogger(), "[SmaccStateMachine] Initializing ROS communication mechanisms");
|
||||
LOG_INFO("[SmaccStateMachine] Initializing ROS communication mechanisms");
|
||||
this->onInitialized();
|
||||
|
||||
// publish startup state machine transition info
|
||||
@@ -90,7 +91,7 @@ public:
|
||||
transitionInfo->destinyState = this->stateMachineInfo_->getState<InitialStateType>();
|
||||
this->publishTransition(*transitionInfo);
|
||||
|
||||
RCLCPP_INFO(getLogger(), "[SmaccStateMachine] Initializing state machine");
|
||||
LOG_INFO("[SmaccStateMachine] Initializing state machine");
|
||||
sc::state_machine<DerivedStateMachine, InitialStateType, SmaccAllocator>::initiate();
|
||||
}
|
||||
};
|
||||
|
||||
@@ -21,6 +21,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/introspection/introspection.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/introspection/state_traits.hpp>
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -40,7 +41,7 @@ private:
|
||||
{
|
||||
static boost::statechart::result react_without_action(State & stt)
|
||||
{
|
||||
RCLCPP_DEBUG(stt.getLogger(), "[Smacc Transition] REACT WITHOUT ACTION");
|
||||
LOG_DEBUG("[Smacc Transition] REACT WITHOUT ACTION");
|
||||
typedef smacc2::Transition<Event, Destination, Tag, TransitionContext, pTransitionAction>
|
||||
Transtype;
|
||||
TRANSITION_TAG mock;
|
||||
@@ -52,7 +53,7 @@ private:
|
||||
|
||||
static boost::statechart::result react_with_action(State & stt, const Event & evt)
|
||||
{
|
||||
RCLCPP_DEBUG(stt.getLogger(), "[Smacc Transition] REACT WITH ACTION AND EVENT");
|
||||
LOG_DEBUG("[Smacc Transition] REACT WITH ACTION AND EVENT");
|
||||
typedef smacc2::Transition<Event, Destination, Tag, TransitionContext, pTransitionAction>
|
||||
Transtype;
|
||||
TRANSITION_TAG mock;
|
||||
|
||||
@@ -22,6 +22,7 @@
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/callback_counter_semaphore.hpp>
|
||||
#include <thread>
|
||||
|
||||
@@ -35,24 +36,18 @@ CallbackCounterSemaphore::CallbackCounterSemaphore(std::string name, int count)
|
||||
bool CallbackCounterSemaphore::acquire()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(mutex_);
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] acquire callback %s %ld", name_.c_str(),
|
||||
(long)this);
|
||||
LOG_DEBUG("[CallbackCounterSemaphore] acquire callback {} {}", name_.c_str(), (long)this);
|
||||
|
||||
if (finalized)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callback rejected %s %ld",
|
||||
name_.c_str(), (long)this);
|
||||
LOG_DEBUG("[CallbackCounterSemaphore] callback rejected {} {}", name_.c_str(), (long)this);
|
||||
return false;
|
||||
}
|
||||
|
||||
++count_;
|
||||
cv_.notify_one();
|
||||
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callback accepted %s %ld", name_.c_str(),
|
||||
(long)this);
|
||||
LOG_DEBUG("[CallbackCounterSemaphore] callback accepted {} {}", name_.c_str(), (long)this);
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -62,9 +57,7 @@ void CallbackCounterSemaphore::release()
|
||||
--count_;
|
||||
cv_.notify_one();
|
||||
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callback finished %s %ld", name_.c_str(),
|
||||
(long)this);
|
||||
LOG_DEBUG("[CallbackCounterSemaphore] callback finished {} {}", name_.c_str(), (long)this);
|
||||
}
|
||||
|
||||
void CallbackCounterSemaphore::finalize()
|
||||
@@ -83,9 +76,7 @@ void CallbackCounterSemaphore::finalize()
|
||||
}
|
||||
|
||||
connections_.clear();
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger(name_), "[CallbackCounterSemaphore] callbacks finalized %s %ld",
|
||||
name_.c_str(), (long)this);
|
||||
LOG_DEBUG("[CallbackCounterSemaphore] callbacks finalized {} {}", name_.c_str(), (long)this);
|
||||
}
|
||||
|
||||
void CallbackCounterSemaphore::addConnection(boost::signals2::connection conn)
|
||||
@@ -94,10 +85,7 @@ void CallbackCounterSemaphore::addConnection(boost::signals2::connection conn)
|
||||
|
||||
if (finalized)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger(name_),
|
||||
"[CallbackCounterSemaphore] ignoring adding callback, already finalized %s %ld",
|
||||
name_.c_str(), (long)this);
|
||||
LOG_DEBUG("[CallbackCounterSemaphore] ignoring adding callback, already finalized {} {}", name_.c_str(), (long)this);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/client_bases/smacc_ros_launch_client.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -46,8 +48,7 @@ std::future<std::string> ClRosLaunch::executeRosLaunch(
|
||||
std::launch::async,
|
||||
[=]()
|
||||
{
|
||||
RCLCPP_WARN_STREAM(
|
||||
rclcpp::get_logger("smacc2"), "[ClRosLaunch static] starting ros launch thread ");
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch static] starting ros launch thread "; return _oss.str(); }());
|
||||
|
||||
std::stringstream cmd;
|
||||
cmd << "ros2 launch " << packageName << " " << launchFileName;
|
||||
@@ -70,8 +71,7 @@ std::future<std::string> ClRosLaunch::executeRosLaunch(
|
||||
ss << buffer.data();
|
||||
if (cancelled) // break_pipe
|
||||
{
|
||||
RCLCPP_WARN_STREAM(
|
||||
rclcpp::get_logger("smacc2"), "[ClRosLaunch static] cancelling ros launch thread ");
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch static] cancelling ros launch thread "; return _oss.str(); }());
|
||||
|
||||
// break pipe
|
||||
pclose(pipe.release());
|
||||
@@ -86,8 +86,7 @@ std::future<std::string> ClRosLaunch::executeRosLaunch(
|
||||
}
|
||||
|
||||
result = ss.str();
|
||||
RCLCPP_WARN_STREAM(
|
||||
rclcpp::get_logger("smacc2"), "[ClRosLaunch static]] RESULT = \n " << ss.str());
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch static]] RESULT = \n " << ss.str(); return _oss.str(); }());
|
||||
return result;
|
||||
});
|
||||
}
|
||||
|
||||
@@ -18,6 +18,8 @@
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#include <errno.h> // Agrega esta inclusión
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <fcntl.h> // Agrega esta inclusión
|
||||
#include <poll.h>
|
||||
#include <signal.h>
|
||||
@@ -75,7 +77,7 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
|
||||
std::launch::async,
|
||||
[packageName, launchFileName, cancelCondition, client]()
|
||||
{
|
||||
RCLCPP_WARN_STREAM(rclcpp::get_logger("smacc2"), "[ClRosLaunch2] Starting ros launch thread");
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch2] Starting ros launch thread"; return _oss.str(); }());
|
||||
|
||||
std::stringstream cmd;
|
||||
cmd << "ros2 launch " << packageName << " " << launchFileName;
|
||||
@@ -119,7 +121,7 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
|
||||
else
|
||||
{
|
||||
// Error de lectura
|
||||
RCLCPP_ERROR(rclcpp::get_logger("smacc2"), "Error de lectura en pipe");
|
||||
LOG_ERROR("Error de lectura en pipe");
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -138,25 +140,23 @@ std::future<std::string> ClRosLaunch2::executeRosLaunch(
|
||||
if (WIFEXITED(status))
|
||||
{
|
||||
int exit_status = WEXITSTATUS(status);
|
||||
RCLCPP_INFO(
|
||||
rclcpp::get_logger("smacc2"), "Child process exited with status: %d", exit_status);
|
||||
LOG_INFO("Child process exited with status: {}", exit_status);
|
||||
}
|
||||
else if (WIFSIGNALED(status))
|
||||
{
|
||||
int term_signal = WTERMSIG(status);
|
||||
RCLCPP_WARN(
|
||||
rclcpp::get_logger("smacc2"), "Child process terminated by signal: %d", term_signal);
|
||||
LOG_WARN("Child process terminated by signal: {}", term_signal);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(rclcpp::get_logger("smacc2"), "Error waiting for child process.");
|
||||
LOG_ERROR("Error waiting for child process.");
|
||||
}
|
||||
|
||||
pclose(child.pipe);
|
||||
close(child.pipe->_fileno); // Close pipe file descriptor but not processes
|
||||
|
||||
RCLCPP_WARN_STREAM(rclcpp::get_logger("smacc2"), "[ClRosLaunch2] RESULT:\n" << result);
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[ClRosLaunch2] RESULT:\n" << result; return _oss.str(); }());
|
||||
|
||||
// Devuelve una std::future con el resultado
|
||||
// return std::async(std::launch::async, [result]() { return result; });
|
||||
@@ -251,7 +251,7 @@ void killProcessesRecursive(pid_t pid)
|
||||
int res = kill(pid, SIGTERM);
|
||||
if (res == 0)
|
||||
{
|
||||
RCLCPP_FATAL(rclcpp::get_logger("smacc2"), "Killed process %d", pid);
|
||||
LOG_FATAL("Killed process {}", pid);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -18,6 +18,8 @@
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#include <smacc2/client_behaviors/cb_ros_launch.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -47,7 +49,7 @@ void onOrthogonalAllocation()
|
||||
|
||||
void CbRosLaunch::onEntry()
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch] OnEntry");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] OnEntry"; return _oss.str(); }());
|
||||
|
||||
std::string packageName, launchFileName;
|
||||
if (launchFileName_ && packageName_)
|
||||
@@ -63,9 +65,8 @@ void CbRosLaunch::onEntry()
|
||||
breakfunction = []() -> bool { return false; };
|
||||
}
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[CbRosLaunch] launching: " << *packageName_ << " , " << *launchFileName_
|
||||
<< "LaunchMode: " << (int)launchMode_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] launching: " << *packageName_ << " , " << *launchFileName_
|
||||
<< "LaunchMode: " << (int)launchMode_; return _oss.str(); }());
|
||||
|
||||
auto fut = smacc2::client_bases::ClRosLaunch::executeRosLaunch(
|
||||
*packageName_, *launchFileName_, breakfunction);
|
||||
@@ -77,22 +78,19 @@ void CbRosLaunch::onEntry()
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch] finding Ros Launch client");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] finding Ros Launch client"; return _oss.str(); }());
|
||||
|
||||
this->requiresClient(client_);
|
||||
if (client_ != nullptr)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[CbRosLaunch] launching from client: " << client_->packageName_ << " , "
|
||||
<< client_->launchFileName_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch] launching from client: " << client_->packageName_ << " , "
|
||||
<< client_->launchFileName_; return _oss.str(); }());
|
||||
|
||||
client_->launch();
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[CbRosLaunch] Inccorrect ros launch operation. No Ros Launch client specified neither "
|
||||
LOG_ERROR("[CbRosLaunch] Inccorrect ros launch operation. No Ros Launch client specified neither "
|
||||
"package/roslaunch file path.");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -18,6 +18,8 @@
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#include <smacc2/client_behaviors/cb_ros_launch_2.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -48,7 +50,7 @@ void onOrthogonalAllocation()
|
||||
|
||||
void CbRosLaunch2::onEntry()
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch2] OnEntry");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] OnEntry"; return _oss.str(); }());
|
||||
|
||||
std::string packageName, launchFileName;
|
||||
if (
|
||||
@@ -59,9 +61,8 @@ void CbRosLaunch2::onEntry()
|
||||
|
||||
breakfunction = std::bind(&CbRosLaunch2::isShutdownRequested, this);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[CbRosLaunch2] launching: " << *packageName_ << " , " << *launchFileName_
|
||||
<< "LaunchMode: " << (int)launchMode_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] launching: " << *packageName_ << " , " << *launchFileName_
|
||||
<< "LaunchMode: " << (int)launchMode_; return _oss.str(); }());
|
||||
|
||||
auto result_ = smacc2::client_bases::ClRosLaunch2::executeRosLaunch(
|
||||
// this->result_ = smacc2::client_bases::ClRosLaunch2::executeRosLaunch(
|
||||
@@ -76,30 +77,25 @@ void CbRosLaunch2::onEntry()
|
||||
client_->launchFileName_ = *launchFileName_;
|
||||
}
|
||||
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[CbRosLaunch2] finding Ros Launch client");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] finding Ros Launch client"; return _oss.str(); }());
|
||||
|
||||
// this->requiresClient(client_);
|
||||
if (client_ != nullptr)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[CbRosLaunch2] launching from client: " << client_->packageName_ << " , "
|
||||
<< client_->launchFileName_);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[CbRosLaunch2] launching from client: " << client_->packageName_ << " , "
|
||||
<< client_->launchFileName_; return _oss.str(); }());
|
||||
|
||||
client_->launch();
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[CbRosLaunch2] Inccorrect ros launch operation. No Ros Launch client specified neither "
|
||||
LOG_ERROR("[CbRosLaunch2] Inccorrect ros launch operation. No Ros Launch client specified neither "
|
||||
"package/roslaunch file path.");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[CbRosLaunch2] Inccorrect ros launch operation. Not supported case. Invalid argument.");
|
||||
LOG_ERROR("[CbRosLaunch2] Inccorrect ros launch operation. Not supported case. Invalid argument.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -27,7 +27,7 @@ std::vector<std::future<std::string>> CbRosStop2::detached_futures_;
|
||||
|
||||
CbRosStop2::CbRosStop2() {}
|
||||
|
||||
CbRosStop2::CbRosStop2(pid_t launchPid) {}
|
||||
CbRosStop2::CbRosStop2(pid_t launchPid) { (void)launchPid; }
|
||||
|
||||
CbRosStop2::~CbRosStop2() {}
|
||||
|
||||
|
||||
@@ -20,6 +20,7 @@
|
||||
|
||||
#include <functional>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/client_behaviors/cb_sequence.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
@@ -31,30 +32,24 @@ CbSequence::CbSequence() {}
|
||||
|
||||
void CbSequence::recursiveConsumeNext()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[SequenceNode %ld] next onEntry: %ld", (long)this, sequenceNodes_.size());
|
||||
LOG_INFO("[SequenceNode {}] next onEntry: {}", (long)this, sequenceNodes_.size());
|
||||
|
||||
auto first = sequenceNodes_.front();
|
||||
auto onDelayedConfigureFn = first;
|
||||
|
||||
RCLCPP_INFO(getLogger(), "[SequenceNode %ld] Behavior on delayed sequence configure", (long)this);
|
||||
LOG_INFO("[SequenceNode {}] Behavior on delayed sequence configure", (long)this);
|
||||
bh_ = onDelayedConfigureFn();
|
||||
std::string currentNodeName = bh_->getName();
|
||||
|
||||
RCLCPP_INFO(getLogger(), "[SequenceNode %ld] Subscribing OnSuccess", (long)this);
|
||||
LOG_INFO("[SequenceNode {}] Subscribing OnSuccess", (long)this);
|
||||
this->conn_ = bh_->onSuccess(&CbSequence::onSubNodeSuccess, this);
|
||||
this->conn2_ = bh_->onFailure(&CbSequence::onSubNodeAbort, this);
|
||||
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[SequenceNode %ld] subnode %s on entry", (long)this, currentNodeName.c_str());
|
||||
LOG_INFO("[SequenceNode {}] subnode {} on entry", (long)this, currentNodeName.c_str());
|
||||
bh_->executeOnEntry();
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[SequenceNode %ld] subnode %s on entry finished", (long)this,
|
||||
currentNodeName.c_str());
|
||||
LOG_INFO("[SequenceNode {}] subnode {} on entry finished", (long)this, currentNodeName.c_str());
|
||||
bh_->waitOnEntryThread(false); // we do not request to finish to keep on subscriptions
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[SequenceNode %ld] subnode %s thread finished", (long)this,
|
||||
currentNodeName.c_str());
|
||||
LOG_INFO("[SequenceNode {}] subnode {} thread finished", (long)this, currentNodeName.c_str());
|
||||
}
|
||||
|
||||
void CbSequence::onEntry()
|
||||
@@ -69,10 +64,7 @@ void CbSequence::onEntry()
|
||||
}
|
||||
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(200));
|
||||
RCLCPP_INFO_THROTTLE(
|
||||
getLogger(), *(getNode()->get_clock()), 1000,
|
||||
"[CbSequence %ld] Waiting for subnodes to finish %ld. Current head Behavior: %s ", (long)this,
|
||||
sequenceNodes_.size(), demangleType(&typeid(*bh_)).c_str());
|
||||
LOG_INFO_THROTTLE(1000, "[CbSequence {}] Waiting for subnodes to finish {}. Current head Behavior: {} ", (long)this, sequenceNodes_.size(), demangleType(&typeid(*bh_)).c_str());
|
||||
|
||||
if (consume_)
|
||||
{
|
||||
@@ -92,34 +84,31 @@ void CbSequence::onEntry()
|
||||
|
||||
if (sequenceNodes_.empty())
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[CbSequence %ld] All subnodes finished", (long)this);
|
||||
LOG_INFO("[CbSequence {}] All subnodes finished", (long)this);
|
||||
this->postSuccessEvent();
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[CbSequence %ld] Aborting", (long)this);
|
||||
LOG_INFO("[CbSequence {}] Aborting", (long)this);
|
||||
this->postFailureEvent();
|
||||
}
|
||||
}
|
||||
|
||||
void CbSequence::onSubNodeSuccess()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[CbSequence %ld] Success NextCbSequence %ld", (long)this, sequenceNodes_.size());
|
||||
LOG_INFO("[CbSequence {}] Success NextCbSequence {}", (long)this, sequenceNodes_.size());
|
||||
consume_++;
|
||||
}
|
||||
|
||||
void CbSequence::onSubNodeAbort()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[CbSequence %ld] Abort NextCbSequence %ld", (long)this, sequenceNodes_.size());
|
||||
LOG_INFO("[CbSequence {}] Abort NextCbSequence {}", (long)this, sequenceNodes_.size());
|
||||
// bh_->waitOnEntryThread();
|
||||
this->conn_.disconnect();
|
||||
this->conn2_.disconnect();
|
||||
|
||||
this->postFailureEvent();
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[CbSequence %ld] Abort NextCbSequence requesting for force finish", (long)this);
|
||||
LOG_INFO("[CbSequence {}] Abort NextCbSequence requesting for force finish", (long)this);
|
||||
this->requestForceFinish();
|
||||
consume_++;
|
||||
}
|
||||
|
||||
@@ -19,6 +19,7 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/client_behaviors/cb_wait_action_server.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -37,24 +38,24 @@ void CbWaitActionServer::onEntry()
|
||||
while (!this->isShutdownRequested() && !found && (getNode()->now() - starttime) < timeout_)
|
||||
{
|
||||
std::shared_ptr<rclcpp_action::ClientBase> client_base = client_->getClientBase();
|
||||
RCLCPP_INFO(getLogger(), "[CbWaitActionServer] waiting action server..");
|
||||
LOG_INFO("[CbWaitActionServer] waiting action server..");
|
||||
found = client_base->wait_for_action_server(std::chrono::milliseconds(1000));
|
||||
}
|
||||
|
||||
if (found)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[CbWaitActionServer] action server already available");
|
||||
LOG_INFO("[CbWaitActionServer] action server already available");
|
||||
this->postSuccessEvent();
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[CbWaitActionServer] action server not found, timeout");
|
||||
LOG_INFO("[CbWaitActionServer] action server not found, timeout");
|
||||
this->postFailureEvent();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[CbWaitActionServer] there is no action client in this orthogonal");
|
||||
LOG_INFO("[CbWaitActionServer] there is no action client in this orthogonal");
|
||||
this->postFailureEvent();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/client_behaviors/cb_wait_node.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -42,10 +44,9 @@ void CbWaitNode::onEntry()
|
||||
}
|
||||
|
||||
auto totalstr = ss.str();
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] on entry, listing nodes (" << nodenames.size() << ")"
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] on entry, listing nodes (" << nodenames.size() << ")"
|
||||
<< std::endl
|
||||
<< totalstr);
|
||||
<< totalstr; return _oss.str(); }());
|
||||
|
||||
rate_.sleep();
|
||||
}
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/client_behaviors/cb_wait_topic.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -33,9 +35,7 @@ void CbWaitTopic::onEntry()
|
||||
bool found = false;
|
||||
while (!this->isShutdownRequested() && !found)
|
||||
{
|
||||
RCLCPP_INFO_STREAM_THROTTLE(
|
||||
getLogger(), *(getNode()->get_clock()), 1000,
|
||||
"[" << getName() << "] waiting topic: " << topicName_);
|
||||
LOG_INFO_THROTTLE(1000, "{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] waiting topic: " << topicName_; return _oss.str(); }());
|
||||
std::stringstream ss;
|
||||
auto topicnames = getNode()->get_topic_names_and_types();
|
||||
|
||||
@@ -49,11 +49,9 @@ void CbWaitTopic::onEntry()
|
||||
}
|
||||
|
||||
auto totalstr = ss.str();
|
||||
RCLCPP_INFO_STREAM_THROTTLE(
|
||||
getLogger(), *(getNode()->get_clock()), 5000,
|
||||
"[" << getName() << "] still waiting topic " << topicName_ << ", listing topics ("
|
||||
LOG_INFO_THROTTLE(5000, "{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] still waiting topic " << topicName_ << ", listing topics ("
|
||||
<< topicnames.size() << ")" << std::endl
|
||||
<< totalstr);
|
||||
<< totalstr; return _oss.str(); }());
|
||||
|
||||
rate_.sleep();
|
||||
}
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/introspection/smacc_type_info.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
@@ -243,7 +245,7 @@ TypeInfo::Ptr TypeInfo::getTypeInfoFromString(std::string inputtext)
|
||||
<< " - " << originalinputtext << std::endl;
|
||||
|
||||
auto strmsg = ss.str();
|
||||
RCLCPP_WARN_STREAM(globalNh_->get_logger(), strmsg);
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << strmsg; return _oss.str(); }());
|
||||
}
|
||||
|
||||
// std::sort(types.begin(), types.end(),[](auto& a, auto& b)
|
||||
@@ -333,7 +335,7 @@ TypeInfo::Ptr TypeInfo::getTypeInfoFromString(std::string inputtext)
|
||||
//RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Current Database");
|
||||
for (auto & en : typeInfoDatabase)
|
||||
{
|
||||
if (globalNh_ != nullptr) RCLCPP_DEBUG_STREAM(globalNh_->get_logger(), "- " << en.first);
|
||||
if (globalNh_ != nullptr) LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "- " << en.first; return _oss.str(); }());
|
||||
}
|
||||
|
||||
typeInfoDatabase[originalinputtext] = roottype;
|
||||
@@ -369,7 +371,7 @@ TypeInfo::Ptr TypeInfo::getTypeInfoFromString(std::string inputtext)
|
||||
}
|
||||
|
||||
auto strmsg = ss.str();
|
||||
RCLCPP_WARN_STREAM(globalNh_->get_logger(), strmsg);
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << strmsg; return _oss.str(); }());
|
||||
}
|
||||
|
||||
return roottype;
|
||||
|
||||
@@ -19,6 +19,7 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/impl/smacc_state_machine_impl.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_client_behavior.hpp>
|
||||
#include <smacc2/smacc_orthogonal.hpp>
|
||||
#include <smacc2/smacc_tracing/smacc_tracing.hpp>
|
||||
@@ -52,9 +53,7 @@ void ISmaccOrthogonal::addClientBehavior(std::shared_ptr<smacc2::ISmaccClientBeh
|
||||
|
||||
if (clBehavior != nullptr)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[Orthogonal: %s] Adding client behavior: %s.", this->getName().c_str(),
|
||||
clBehavior->getName().c_str());
|
||||
LOG_INFO("[Orthogonal: {}] Adding client behavior: {}.", this->getName().c_str(), clBehavior->getName().c_str());
|
||||
clBehavior->stateMachine_ = this->getStateMachine();
|
||||
clBehavior->currentOrthogonal = this;
|
||||
clBehavior->currentState = clBehavior->stateMachine_->getCurrentState();
|
||||
@@ -63,8 +62,7 @@ void ISmaccOrthogonal::addClientBehavior(std::shared_ptr<smacc2::ISmaccClientBeh
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[Orthogonal: %s] No client behaviors in this state.", this->getName().c_str());
|
||||
LOG_INFO("[Orthogonal: {}] No client behaviors in this state.", this->getName().c_str());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -72,9 +70,7 @@ void ISmaccOrthogonal::onInitialize() {}
|
||||
|
||||
void ISmaccOrthogonal::initState(ISmaccState * state)
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[Orthogonal: %s] InitState: %s.", this->getName().c_str(),
|
||||
state->getClassName().c_str());
|
||||
LOG_INFO("[Orthogonal: {}] InitState: {}.", this->getName().c_str(), state->getClassName().c_str());
|
||||
clientBehaviors_.push_back(std::vector<std::shared_ptr<smacc2::ISmaccClientBehavior>>());
|
||||
}
|
||||
|
||||
@@ -86,9 +82,7 @@ void ISmaccOrthogonal::runtimeConfigure()
|
||||
|
||||
for (auto & clBehavior : clientBehaviors_.back())
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[Orthogonal: %s] runtimeConfigure(), current behavior: %s.",
|
||||
this->getName().c_str(), clBehavior->getName().c_str());
|
||||
LOG_INFO("[Orthogonal: {}] runtimeConfigure(), current behavior: {}.", this->getName().c_str(), clBehavior->getName().c_str());
|
||||
|
||||
clBehavior->runtimeConfigure();
|
||||
}
|
||||
@@ -102,8 +96,7 @@ void ISmaccOrthogonal::onEntry()
|
||||
{
|
||||
for (auto & clBehavior : clientBehaviors_.back())
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[Orthogonal: %s] OnEntry, current Behavior: %s", orthogonalName, cbName);
|
||||
LOG_INFO("[Orthogonal: {}] OnEntry, current Behavior: {}", orthogonalName, cbName);
|
||||
|
||||
try
|
||||
{
|
||||
@@ -112,21 +105,16 @@ void ISmaccOrthogonal::onEntry()
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[ClientBehavior: %s] Exception on Entry - continuing with next client behavior. "
|
||||
LOG_ERROR("[ClientBehavior: {}] Exception on Entry - continuing with next client behavior. "
|
||||
"Exception info: "
|
||||
"%s",
|
||||
cbName, e.what());
|
||||
"{}", cbName, e.what());
|
||||
}
|
||||
TRACEPOINT(smacc2_client_behavior_on_entry_end, statename, orthogonalName, cbName);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[Orthogonal: %s] OnEntry -> empty orthogonal (no client behavior).",
|
||||
orthogonalName);
|
||||
LOG_INFO("[Orthogonal: {}] OnEntry -> empty orthogonal (no client behavior).", orthogonalName);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -138,8 +126,7 @@ void ISmaccOrthogonal::onExit()
|
||||
{
|
||||
for (auto & clBehavior : clientBehaviors_.back())
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[Orthogonal: %s] OnExit, current Behavior: %s", orthogonalName, cbName);
|
||||
LOG_INFO("[Orthogonal: {}] OnExit, current Behavior: {}", orthogonalName, cbName);
|
||||
try
|
||||
{
|
||||
TRACEPOINT(smacc2_client_behavior_on_exit_start, statename, orthogonalName, cbName);
|
||||
@@ -147,18 +134,15 @@ void ISmaccOrthogonal::onExit()
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
getLogger(),
|
||||
"[ClientBehavior: %s] Exception onExit - continuing with next client behavior. Exception "
|
||||
"info: %s",
|
||||
cbName, e.what());
|
||||
LOG_ERROR("[ClientBehavior: {}] Exception onExit - continuing with next client behavior. Exception "
|
||||
"info: {}", cbName, e.what());
|
||||
}
|
||||
TRACEPOINT(smacc2_client_behavior_on_exit_end, statename, orthogonalName, cbName);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[Orthogonal %s] OnExit().", orthogonalName);
|
||||
LOG_INFO("[Orthogonal {}] OnExit().", orthogonalName);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <signal.h>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
@@ -87,8 +89,7 @@ void SignalDetector::findUpdatableClientsAndComponents()
|
||||
|
||||
if (updatableClient != nullptr)
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(), "Adding updatable client: " << demangleType(typeid(updatableClient)));
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable client: " << demangleType(typeid(updatableClient)); return _oss.str(); }());
|
||||
this->updatableClients_.push_back(updatableClient);
|
||||
}
|
||||
|
||||
@@ -100,9 +101,7 @@ void SignalDetector::findUpdatableClientsAndComponents()
|
||||
auto updatableComponent = dynamic_cast<ISmaccUpdatable *>(componententry.get());
|
||||
if (updatableComponent != nullptr)
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(),
|
||||
"Adding updatable component: " << demangleType(typeid(*updatableComponent)));
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable component: " << demangleType(typeid(*updatableComponent)); return _oss.str(); }());
|
||||
this->updatableClients_.push_back(updatableComponent);
|
||||
}
|
||||
}
|
||||
@@ -133,9 +132,7 @@ void SignalDetector::findUpdatableStateElements(ISmaccState * currentState)
|
||||
|
||||
if (updatableClientBehavior != nullptr)
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(),
|
||||
"Adding updatable behavior: " << demangleType(typeid(updatableClientBehavior)));
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable behavior: " << demangleType(typeid(updatableClientBehavior)); return _oss.str(); }());
|
||||
updatableElements.push_back(updatableClientBehavior);
|
||||
}
|
||||
}
|
||||
@@ -153,9 +150,7 @@ void SignalDetector::findUpdatableStateElements(ISmaccState * currentState)
|
||||
ISmaccUpdatable * updatableStateReactor = dynamic_cast<ISmaccUpdatable *>(sr.get());
|
||||
if (updatableStateReactor != nullptr)
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(),
|
||||
"Adding updatable stateReactorr: " << demangleType(typeid(updatableStateReactor)));
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable stateReactorr: " << demangleType(typeid(updatableStateReactor)); return _oss.str(); }());
|
||||
updatableElements.push_back(updatableStateReactor);
|
||||
}
|
||||
}
|
||||
@@ -166,9 +161,7 @@ void SignalDetector::findUpdatableStateElements(ISmaccState * currentState)
|
||||
ISmaccUpdatable * updatableEventGenerator = dynamic_cast<ISmaccUpdatable *>(eg.get());
|
||||
if (updatableEventGenerator != nullptr)
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(),
|
||||
"Adding updatable eventGenerator: " << demangleType(typeid(updatableEventGenerator)));
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Adding updatable eventGenerator: " << demangleType(typeid(updatableEventGenerator)); return _oss.str(); }());
|
||||
updatableElements.push_back(updatableEventGenerator);
|
||||
}
|
||||
}
|
||||
@@ -236,7 +229,7 @@ void SignalDetector::pollOnce()
|
||||
//smaccStateMachine_->lockStateMachine("update behaviors");
|
||||
|
||||
this->findUpdatableClientsAndComponents();
|
||||
RCLCPP_DEBUG_STREAM(getLogger(), "Updatable clients: " << this->updatableClients_.size());
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Updatable clients: " << this->updatableClients_.size(); return _oss.str(); }());
|
||||
|
||||
if (this->updatableClients_.size())
|
||||
{
|
||||
@@ -246,9 +239,7 @@ void SignalDetector::pollOnce()
|
||||
auto updatableElementName = demangleType(typeid(*updatableClient)).c_str();
|
||||
try
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
node->get_logger(),
|
||||
"[PollOnce] update client call: " << demangleType(typeid(*updatableClient)));
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[PollOnce] update client call: " << demangleType(typeid(*updatableClient)); return _oss.str(); }());
|
||||
|
||||
TRACEPOINT(smacc2_state_update_start, updatableElementName);
|
||||
updatableClient->executeUpdate(smaccStateMachine_->getNode());
|
||||
@@ -256,9 +247,7 @@ void SignalDetector::pollOnce()
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_ERROR_STREAM(
|
||||
node->get_logger(),
|
||||
"Error in updatable elemnent " << updatableElementName << ": " << e.what() << '\n');
|
||||
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "Error in updatable elemnent " << updatableElementName << ": " << e.what() << '\n'; return _oss.str(); }());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -274,8 +263,7 @@ void SignalDetector::pollOnce()
|
||||
this->smaccStateMachine_->stateMachineCurrentAction !=
|
||||
StateMachineInternalAction::STATE_EXITING)
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(), "Updatable states: " << this->updatableStateElements_.size());
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "Updatable states: " << this->updatableStateElements_.size(); return _oss.str(); }());
|
||||
|
||||
for (auto stateElement : this->updatableStateElements_)
|
||||
{
|
||||
@@ -284,9 +272,8 @@ void SignalDetector::pollOnce()
|
||||
std::string updatableElementName = demangleType(typeid(*udpatableStateElement));
|
||||
auto updatableElementNameCstr = updatableElementName.c_str();
|
||||
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(), "pollOnce update client behavior call: "
|
||||
<< demangleType(typeid(*udpatableStateElement)));
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "pollOnce update client behavior call: "
|
||||
<< demangleType(typeid(*udpatableStateElement)); return _oss.str(); }());
|
||||
TRACEPOINT(smacc2_state_update_start, updatableElementNameCstr);
|
||||
|
||||
udpatableStateElement->executeUpdate(smaccStateMachine_->getNode());
|
||||
@@ -297,7 +284,7 @@ void SignalDetector::pollOnce()
|
||||
}
|
||||
catch (std::exception & ex)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "Exception during Signal Detector update loop. %s.", ex.what());
|
||||
LOG_ERROR("Exception during Signal Detector update loop. {}.", ex.what());
|
||||
}
|
||||
|
||||
auto nh = this->getNode();
|
||||
@@ -325,33 +312,27 @@ void SignalDetector::pollingLoop()
|
||||
|
||||
if (!nh->get_parameter("signal_detector_loop_freq", this->loop_rate_hz))
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
getLogger(),
|
||||
"Signal Detector frequency (ros param signal_detector_loop_freq) was not set, using default "
|
||||
LOG_WARN("Signal Detector frequency (ros param signal_detector_loop_freq) was not set, using default "
|
||||
"frequency: "
|
||||
"%lf",
|
||||
this->loop_rate_hz);
|
||||
"{}", this->loop_rate_hz);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
getLogger(), "Signal Detector frequency (ros param signal_detector_loop_freq): %lf",
|
||||
this->loop_rate_hz);
|
||||
LOG_WARN("Signal Detector frequency (ros param signal_detector_loop_freq): {}", this->loop_rate_hz);
|
||||
}
|
||||
|
||||
nh->set_parameter(rclcpp::Parameter("signal_detector_loop_freq", this->loop_rate_hz));
|
||||
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] Loop rate hz:" << loop_rate_hz);
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[SignalDetector] Loop rate hz:" << loop_rate_hz; return _oss.str(); }());
|
||||
|
||||
if (this->executionModel_ == ExecutionModel::SINGLE_THREAD_SPINNER)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] Running in single-threaded mode.");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[SignalDetector] Running in single-threaded mode."; return _oss.str(); }());
|
||||
|
||||
rclcpp::Rate r(loop_rate_hz);
|
||||
while (rclcpp::ok() && !end_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM_THROTTLE(
|
||||
getLogger(), *getNode()->get_clock(), 10000, "[SignalDetector] Heartbeat");
|
||||
LOG_INFO_THROTTLE(10000, "{}", [&]{ std::ostringstream _oss; _oss << "[SignalDetector] Heartbeat"; return _oss.str(); }());
|
||||
pollOnce();
|
||||
rclcpp::spin_some(nh);
|
||||
r.sleep();
|
||||
@@ -359,7 +340,7 @@ void SignalDetector::pollingLoop()
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] Running in multi-threaded mode.");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[SignalDetector] Running in multi-threaded mode."; return _oss.str(); }());
|
||||
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(nh);
|
||||
@@ -369,7 +350,7 @@ void SignalDetector::pollingLoop()
|
||||
|
||||
void onSigQuit(int)
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger("SMACC"), "SignalDetector: SIGQUIT received.");
|
||||
LOG_INFO("SignalDetector: SIGQUIT received.");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
@@ -26,14 +28,14 @@ namespace smacc2
|
||||
{
|
||||
void SmaccAsyncClientBehavior::executeOnEntry()
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] asynchronous onEntry thread started");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] asynchronous onEntry thread started"; return _oss.str(); }());
|
||||
this->onEntryThread_ = std::async(
|
||||
std::launch::async,
|
||||
[=]
|
||||
{
|
||||
this->onEntry();
|
||||
this->postFinishEventFn_();
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] asynchronous onEntry thread finished");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] asynchronous onEntry thread finished"; return _oss.str(); }());
|
||||
return 0;
|
||||
});
|
||||
}
|
||||
@@ -61,39 +63,30 @@ void SmaccAsyncClientBehavior::waitFutureIfNotFinished(
|
||||
else
|
||||
{
|
||||
// in progress
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(),
|
||||
"[" << getName()
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName()
|
||||
<< "] fut valid but waiting for asynchronous onEntry thread to finish: state: "
|
||||
<< (int)status);
|
||||
<< (int)status; return _oss.str(); }());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(),
|
||||
"[" << getName()
|
||||
<< "] waiting future onEntryThread. It was not even created. Skipping wait.");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName()
|
||||
<< "] waiting future onEntryThread. It was not even created. Skipping wait."; return _oss.str(); }());
|
||||
break;
|
||||
}
|
||||
|
||||
// r.sleep();
|
||||
rclcpp::sleep_for(100ms);
|
||||
// rclcpp::spin_some(getNode());
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
getLogger(), *(getNode()->get_clock()), 1000,
|
||||
"[%s] waiting for finishing client behavior, before leaving the state. Is the client "
|
||||
"behavior stuck? requesting force finish",
|
||||
this->getName().c_str());
|
||||
LOG_WARN_THROTTLE(1000, "[{}] waiting for finishing client behavior, before leaving the state. Is the client "
|
||||
"behavior stuck? requesting force finish", this->getName().c_str());
|
||||
|
||||
if (requestFinish) requestForceFinish();
|
||||
}
|
||||
}
|
||||
catch (const std::exception & e)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(),
|
||||
"[SmaccAsyncClientBehavior] trying to join function, but it was already finished.");
|
||||
LOG_DEBUG("[SmaccAsyncClientBehavior] trying to join function, but it was already finished.");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -104,27 +97,25 @@ void SmaccAsyncClientBehavior::waitOnEntryThread(bool requestFinish)
|
||||
|
||||
void SmaccAsyncClientBehavior::executeOnExit()
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] onExit - join async onEntry thread");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] onExit - join async onEntry thread"; return _oss.str(); }());
|
||||
this->waitOnEntryThread(true);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] onExit - Creating asynchronous onExit thread");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] onExit - Creating asynchronous onExit thread"; return _oss.str(); }());
|
||||
this->onExitThread_ = std::async(
|
||||
std::launch::async,
|
||||
[=]
|
||||
{
|
||||
this->onExit();
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] asynchronous onExit done.");
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] asynchronous onExit done."; return _oss.str(); }());
|
||||
return 0;
|
||||
});
|
||||
}
|
||||
|
||||
void SmaccAsyncClientBehavior::dispose()
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(), "[" << getName()
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName()
|
||||
<< "] Destroying client behavior- Waiting finishing of asynchronous onExit "
|
||||
"thread");
|
||||
"thread"; return _oss.str(); }());
|
||||
try
|
||||
{
|
||||
if (this->onExitThread_)
|
||||
@@ -136,16 +127,13 @@ void SmaccAsyncClientBehavior::dispose()
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(),
|
||||
"[SmaccAsyncClientBehavior] trying to Join onExit function, but it was already "
|
||||
LOG_DEBUG("[SmaccAsyncClientBehavior] trying to Join onExit function, but it was already "
|
||||
"finished.");
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
getLogger(), "[" << getName()
|
||||
LOG_DEBUG("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName()
|
||||
<< "] Destroying client behavior- onExit thread finished. Proccedding "
|
||||
"destruction.");
|
||||
"destruction."; return _oss.str(); }());
|
||||
}
|
||||
|
||||
SmaccAsyncClientBehavior::~SmaccAsyncClientBehavior() {}
|
||||
@@ -156,9 +144,7 @@ void SmaccAsyncClientBehavior::postFailureEvent() { postFailureEventFn_(); }
|
||||
|
||||
void SmaccAsyncClientBehavior::requestForceFinish()
|
||||
{
|
||||
RCLCPP_FATAL_STREAM_THROTTLE(
|
||||
getLogger(), *(getNode()->get_clock()), 1000,
|
||||
"[" << getName() << "] " << ((uint64_t)this) << " requestForceFinish");
|
||||
LOG_WARN_THROTTLE(1000, "{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "] " << ((uint64_t)this) << " requestForceFinish"; return _oss.str(); }());
|
||||
isShutdownRequested_ = true;
|
||||
}
|
||||
|
||||
|
||||
@@ -19,18 +19,17 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/smacc_client_behavior.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
void SmaccClientBehavior::onEntry()
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] Default empty SmaccClientBehavior onEntry", this->getName().c_str());
|
||||
LOG_DEBUG("[{}] Default empty SmaccClientBehavior onEntry", this->getName().c_str());
|
||||
}
|
||||
|
||||
void SmaccClientBehavior::onExit()
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] Default empty SmaccClientBehavior onExit", this->getName().c_str());
|
||||
LOG_DEBUG("[{}] Default empty SmaccClientBehavior onExit", this->getName().c_str());
|
||||
}
|
||||
} // namespace smacc2
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/smacc_client_behavior.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -30,9 +32,8 @@ ISmaccClientBehavior::ISmaccClientBehavior()
|
||||
|
||||
ISmaccClientBehavior::~ISmaccClientBehavior()
|
||||
{
|
||||
RCLCPP_WARN_STREAM(
|
||||
getLogger(), "[" << getName() << "]"
|
||||
<< " Client behavior deallocated");
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[" << getName() << "]"
|
||||
<< " Client behavior deallocated"; return _oss.str(); }());
|
||||
}
|
||||
|
||||
std::string ISmaccClientBehavior::getName() const { return demangleSymbol(typeid(*this).name()); }
|
||||
@@ -57,22 +58,18 @@ rclcpp::Logger ISmaccClientBehavior::getLogger() const
|
||||
|
||||
void ISmaccClientBehavior::runtimeConfigure()
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] Default empty SmaccClientBehavior runtimeConfigure()",
|
||||
this->getName().c_str());
|
||||
LOG_DEBUG("[{}] Default empty SmaccClientBehavior runtimeConfigure()", this->getName().c_str());
|
||||
}
|
||||
|
||||
void ISmaccClientBehavior::executeOnEntry()
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] Default empty SmaccClientBehavior onEntry()", this->getName().c_str());
|
||||
LOG_DEBUG("[{}] Default empty SmaccClientBehavior onEntry()", this->getName().c_str());
|
||||
this->onEntry();
|
||||
}
|
||||
|
||||
void ISmaccClientBehavior::executeOnExit()
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
getLogger(), "[%s] Default empty SmaccClientBehavior onExit()", this->getName().c_str());
|
||||
LOG_DEBUG("[{}] Default empty SmaccClientBehavior onExit()", this->getName().c_str());
|
||||
this->onExit();
|
||||
}
|
||||
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/smacc_state.hpp>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -27,7 +29,7 @@ std::string ISmaccState::getClassName() { return demangleSymbol(typeid(*this).na
|
||||
|
||||
void ISmaccState::notifyTransitionFromTransitionTypeInfo(TypeInfo::Ptr & transitionType)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "TRANSITION RULE TRIGGERED: " << transitionType->getFullName());
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "TRANSITION RULE TRIGGERED: " << transitionType->getFullName(); return _oss.str(); }());
|
||||
|
||||
//auto currstateinfo = this->getStateMachine().getCurrentStateInfo();
|
||||
auto currstateinfo = this->stateInfo_;
|
||||
@@ -48,12 +50,10 @@ void ISmaccState::notifyTransitionFromTransitionTypeInfo(TypeInfo::Ptr & transit
|
||||
}
|
||||
|
||||
// debug information if not found
|
||||
RCLCPP_ERROR_STREAM(
|
||||
getLogger(),
|
||||
"Transition happened, from current state "
|
||||
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "Transition happened, from current state "
|
||||
<< currstateinfo->getDemangledFullName()
|
||||
<< " but there is not any transitioninfo match available to publish transition: "
|
||||
<< transitionType->getFullName());
|
||||
<< transitionType->getFullName(); return _oss.str(); }());
|
||||
std::stringstream ss;
|
||||
|
||||
auto stateinfo = currstateinfo;
|
||||
@@ -61,32 +61,31 @@ void ISmaccState::notifyTransitionFromTransitionTypeInfo(TypeInfo::Ptr & transit
|
||||
for (auto & transition : currstateinfo->transitions_)
|
||||
{
|
||||
std::string transitionCandidateName = transition.transitionTypeInfo->getFullName();
|
||||
RCLCPP_ERROR_STREAM(getLogger(), "- candidate transition: " << transitionCandidateName);
|
||||
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "- candidate transition: " << transitionCandidateName; return _oss.str(); }());
|
||||
}
|
||||
|
||||
RCLCPP_ERROR(getLogger(), "Ancestors candidates: ");
|
||||
LOG_ERROR("Ancestors candidates: ");
|
||||
|
||||
std::list<const SmaccStateInfo *> ancestors;
|
||||
stateinfo->getAncestors(ancestors);
|
||||
|
||||
for (auto & ancestor : ancestors)
|
||||
{
|
||||
RCLCPP_ERROR_STREAM(getLogger(), " * Ancestor " << ancestor->getDemangledFullName() << ":");
|
||||
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << " * Ancestor " << ancestor->getDemangledFullName() << ":"; return _oss.str(); }());
|
||||
for (auto & transition : ancestor->transitions_)
|
||||
{
|
||||
std::string transitionCandidateName = transition.transitionTypeInfo->getFullName();
|
||||
RCLCPP_ERROR_STREAM(getLogger(), "- candidate transition: " << transitionCandidateName);
|
||||
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "- candidate transition: " << transitionCandidateName; return _oss.str(); }());
|
||||
if (transitionType->getFullName() == transitionCandidateName)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "GOTCHA");
|
||||
LOG_ERROR("GOTCHA");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR_STREAM(
|
||||
getLogger(), "Transition happened, but current state was not set. Transition candidates:");
|
||||
LOG_ERROR("{}", [&]{ std::ostringstream _oss; _oss << "Transition happened, but current state was not set. Transition candidates:"; return _oss.str(); }());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -19,6 +19,8 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <rcl/time.h>
|
||||
#include <sstream>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <smacc2/client_bases/smacc_action_client.hpp>
|
||||
@@ -43,8 +45,7 @@ ISmaccStateMachine::ISmaccStateMachine(
|
||||
// node_options.automatically_declare_parameters_from_overrides(true);
|
||||
|
||||
nh_ = rclcpp::Node::make_shared(stateMachineName, nodeOptions); //
|
||||
RCLCPP_INFO_STREAM(
|
||||
nh_->get_logger(), "Creating state machine base: " << nh_->get_fully_qualified_name());
|
||||
LOG_INFO("{}", [&]{ std::ostringstream _oss; _oss << "Creating state machine base: " << nh_->get_fully_qualified_name(); return _oss.str(); }());
|
||||
|
||||
signalDetector_ = signalDetector;
|
||||
signalDetector_->initialize(this);
|
||||
@@ -62,7 +63,7 @@ ISmaccStateMachine::ISmaccStateMachine(
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(nh_->get_logger(), "Incorrect run_mode value: %s", runMode.c_str());
|
||||
LOG_ERROR("Incorrect run_mode value: {}", runMode.c_str());
|
||||
}
|
||||
}
|
||||
else
|
||||
@@ -73,12 +74,12 @@ ISmaccStateMachine::ISmaccStateMachine(
|
||||
|
||||
ISmaccStateMachine::~ISmaccStateMachine()
|
||||
{
|
||||
RCLCPP_INFO(nh_->get_logger(), "Finishing State Machine");
|
||||
LOG_INFO("Finishing State Machine");
|
||||
}
|
||||
|
||||
void ISmaccStateMachine::disconnectSmaccSignalObject(void * object_ptr)
|
||||
{
|
||||
RCLCPP_INFO(nh_->get_logger(), "[SmaccSignal] Object signal disconnecting %ld", (long)object_ptr);
|
||||
LOG_INFO("[SmaccSignal] Object signal disconnecting {}", (long)object_ptr);
|
||||
if (stateCallbackConnections.count(object_ptr) > 0)
|
||||
{
|
||||
auto callbackSemaphore = stateCallbackConnections[object_ptr];
|
||||
@@ -87,7 +88,7 @@ void ISmaccStateMachine::disconnectSmaccSignalObject(void * object_ptr)
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(nh_->get_logger(), "[SmaccSignal] No signals found %ld", (long)object_ptr);
|
||||
LOG_INFO("[SmaccSignal] No signals found {}", (long)object_ptr);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -111,9 +112,7 @@ void ISmaccStateMachine::updateStatusMessage()
|
||||
|
||||
if (currentStateInfo_ != nullptr)
|
||||
{
|
||||
RCLCPP_WARN_STREAM(
|
||||
nh_->get_logger(),
|
||||
"[StateMachine] Setting state active: " << currentStateInfo_->getFullPath());
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "[StateMachine] Setting state active: " << currentStateInfo_->getFullPath(); return _oss.str(); }());
|
||||
|
||||
if (runMode_ == SMRunMode::DEBUG)
|
||||
{
|
||||
@@ -163,7 +162,7 @@ void ISmaccStateMachine::onInitialized()
|
||||
|
||||
void ISmaccStateMachine::initializeROS(std::string shortname)
|
||||
{
|
||||
RCLCPP_WARN_STREAM(nh_->get_logger(), "State machine base creation: " << shortname);
|
||||
LOG_WARN("{}", [&]{ std::ostringstream _oss; _oss << "State machine base creation: " << shortname; return _oss.str(); }());
|
||||
// STATE MACHINE TOPICS
|
||||
stateMachinePub_ = nh_->create_publisher<smacc2_msgs::msg::SmaccStateMachine>(
|
||||
shortname + "/smacc/state_machine_description", rclcpp::QoS(1));
|
||||
@@ -185,9 +184,7 @@ void ISmaccStateMachine::getTransitionLogHistory(
|
||||
const std::shared_ptr<smacc2_msgs::srv::SmaccGetTransitionHistory::Request> /*req*/,
|
||||
std::shared_ptr<smacc2_msgs::srv::SmaccGetTransitionHistory::Response> res)
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
nh_->get_logger(), "Requesting transition log history, current size: %ld",
|
||||
this->transitionLogHistory_.size());
|
||||
LOG_WARN("Requesting transition log history, current size: {}", this->transitionLogHistory_.size());
|
||||
res->history = this->transitionLogHistory_;
|
||||
}
|
||||
|
||||
@@ -209,13 +206,13 @@ void ISmaccStateMachine::state_machine_visualization()
|
||||
|
||||
void ISmaccStateMachine::lockStateMachine(std::string msg)
|
||||
{
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "-- locking SM: %s", msg.c_str());
|
||||
LOG_DEBUG("-- locking SM: {}", msg.c_str());
|
||||
m_mutex_.lock();
|
||||
}
|
||||
|
||||
void ISmaccStateMachine::unlockStateMachine(std::string msg)
|
||||
{
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "-- unlocking SM: %s", msg.c_str());
|
||||
LOG_DEBUG("-- unlocking SM: {}", msg.c_str());
|
||||
m_mutex_.unlock();
|
||||
}
|
||||
|
||||
|
||||
@@ -19,6 +19,7 @@
|
||||
******************************************************************************************************************/
|
||||
|
||||
#include <smacc2/introspection/introspection.hpp>
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
|
||||
namespace smacc2
|
||||
@@ -212,7 +213,7 @@ void SmaccStateMachineInfo::assembleSMStructureMessage(ISmaccStateMachine * sm)
|
||||
ss << "----------------------------------------------------------" << std::endl;
|
||||
|
||||
auto resumeMsg = ss.str();
|
||||
RCLCPP_DEBUG(getLogger(), "%s", resumeMsg.c_str());
|
||||
LOG_DEBUG("{}", resumeMsg.c_str());
|
||||
stateMsgs.push_back(stateMsg);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -23,6 +23,7 @@
|
||||
#include <smacc2/smacc_state.hpp>
|
||||
#include <smacc2/smacc_state_reactor.hpp>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "hivecore_logger/logger.hpp"
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
@@ -48,7 +49,7 @@ void StateReactor::update()
|
||||
{
|
||||
if (this->triggers())
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "State reactor base REALLY TRIGGERS!!");
|
||||
LOG_INFO("State reactor base REALLY TRIGGERS!!");
|
||||
this->postEventFn();
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user