Compare commits
9 Commits
feature-vi
...
feature-ne
| Author | SHA1 | Date | |
|---|---|---|---|
| ce03b2c6c5 | |||
|
a02bc399b7 | ||
|
|
0e7b8570fd | ||
|
|
527c243617 | ||
|
|
32e15a7d9a | ||
|
|
aa74fc2397 | ||
|
|
97363f1e32 | ||
|
|
742e5ec67e | ||
|
|
7d7f602a16 |
@@ -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:\
|
||||
\ 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
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -143,6 +143,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_;
|
||||
|
||||
@@ -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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
{
|
||||
@@ -199,6 +199,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 +261,20 @@ 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") {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(), "Invalid dual_arm_motion_type '%s', 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);
|
||||
@@ -739,6 +748,7 @@ 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)) {
|
||||
@@ -756,6 +766,8 @@ bool CerebellumNode::ExecuteActionSkill(
|
||||
return false;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
return RunActionSkillWithProgress(skill, interface_base, topic, effective_timeout,
|
||||
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
|
||||
}
|
||||
@@ -872,7 +884,7 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
time_steady = std::chrono::steady_clock::now();
|
||||
RCLCPP_WARN(this->get_logger(), "left_arm_time_steady2");
|
||||
};
|
||||
std::thread left_arm_thread(left_arm_worker, LEFT_ARM); // LEFT_ARM=0
|
||||
std::thread 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) {
|
||||
@@ -886,7 +898,7 @@ bool CerebellumNode::ExecuteBilateralArmAction(
|
||||
time_steady = std::chrono::steady_clock::now();
|
||||
RCLCPP_WARN(this->get_logger(), "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();
|
||||
|
||||
|
||||
@@ -1226,6 +1226,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 +1252,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 {
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user