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

Refactor vision hooks and add dual arm motion type

Browse Source
This commit is contained in:
NuoDaJia02
2026-02-05 12:12:36 +08:00
parent 527c243617
commit 0e7b8570fd
10 changed files with 448 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/brain/config/brain_node_params.yaml
View File

@@ -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 # 夹爪默认模式

16
src/brain/config/bt_vision_grasp_dual_arm.params.yaml
View File

@@ -37,12 +37,12 @@
- name: s2_right_arm_cam_take_photo
params: &right_arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s2_left_arm_cam_take_photo
params: &left_arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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
@@ -59,22 +59,22 @@
- name: s3_right_arm_vision_pre_grasp
params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s3_left_arm_vision_pre_grasp
params: &pre_grasp_left "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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
@@ -91,12 +91,12 @@
- 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s4_left_arm_take_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s5_right_arm_take_box_off
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \

16
src/brain/config/bt_vision_grasp_dual_arm_demo1.params.yaml
View File

@@ -37,12 +37,12 @@
- name: s2_right_arm_cam_take_photo
params: &right_arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s2_left_arm_cam_take_photo
params: &left_arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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
@@ -59,22 +59,22 @@
- name: s3_right_arm_vision_pre_grasp
params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s3_left_arm_vision_pre_grasp
params: &pre_grasp_left "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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
@@ -91,12 +91,12 @@
- 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s4_left_arm_take_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [360, 360, 360, 360, 360, 360]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s5_right_arm_take_box_off
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \

54
src/brain/config/bt_vision_grasp_left_arm.params.yaml
View File

@@ -22,15 +22,15 @@
mode: 2
'
- name: s1_left_arm_pre_cam_take_photo
params: &pre_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
- 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: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s1_right_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
- 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: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 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
@@ -41,12 +41,12 @@
- 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: [120, 20, 80, 0, 80, 120]\n blend_radius:\
\ 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: [60, -20, -80, 0, -80, 60]\n blend_radius:\
\ 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
@@ -55,7 +55,7 @@
- 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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
@@ -64,12 +64,12 @@
- 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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
@@ -84,7 +84,7 @@
- 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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 \
@@ -134,18 +134,28 @@
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

30
src/brain/config/bt_vision_grasp_left_arm.xml
View File

@@ -51,15 +51,16 @@
<RetryUntilSuccessful name="retry_vision" num_attempts="1">
<Sequence>
<Parallel name="parallel_action_1">
<DualArm_H name="s1_left_arm_pre_cam_take_photo" />
<DualArm_H name="s1_right_arm_pre_cam_take_photo" />
</Parallel>
<Parallel name="parallel_action_2">
<!-- <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" />
<DualArm_H name="s2_left_arm_pre_cam_take_photo" />
<DualArm_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" /> -->
<!-- <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" />
@@ -123,7 +124,8 @@
<Parallel name="parallel_action_3">
<!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
<DualArm_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>
@@ -135,7 +137,8 @@
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
<Sequence>
<GripperCmd0_H name="s12_gripper_open" />
<DualArm_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>
@@ -147,7 +150,8 @@
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
<Sequence>
<!-- <GripperCmd0_H name="s14_gripper_open" /> -->
<DualArm_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>
@@ -161,7 +165,8 @@
<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_pre_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>
@@ -175,7 +180,8 @@
<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_pre_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>

54
src/brain/config/bt_vision_grasp_right_arm.params.yaml
View File

@@ -22,15 +22,15 @@
mode: 2
'
- name: s1_left_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
- 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: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ 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: &pre_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\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: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 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
@@ -41,12 +41,12 @@
- 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: [120, 20, 80, 0, 80, 120]\n blend_radius:\
\ 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: [60, -20, -80, 0, -80, 60]\n blend_radius:\
\ 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
@@ -55,7 +55,7 @@
- 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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
@@ -64,12 +64,12 @@
- 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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
@@ -84,7 +84,7 @@
- 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: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 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 \
@@ -134,18 +134,28 @@
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

30
src/brain/config/bt_vision_grasp_right_arm.xml
View File

@@ -51,15 +51,16 @@
<RetryUntilSuccessful name="retry_vision" num_attempts="1">
<Sequence>
<Parallel name="parallel_action_1">
<DualArm_H name="s1_left_arm_pre_cam_take_photo" />
<DualArm_H name="s1_right_arm_pre_cam_take_photo" />
</Parallel>
<Parallel name="parallel_action_2">
<!-- <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" />
<DualArm_H name="s2_left_arm_pre_cam_take_photo" />
<DualArm_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"/>
<DualArm_H name="s2_right_arm_cam_take_photo" />
@@ -123,7 +124,8 @@
<Parallel name="parallel_action_3">
<!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
<DualArm_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>
@@ -135,7 +137,8 @@
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
<Sequence>
<GripperCmd1_H name="s12_gripper_open" />
<DualArm_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>
@@ -147,7 +150,8 @@
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
<Sequence>
<!-- <GripperCmd1_H name="s14_gripper_open" /> -->
<DualArm_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>
@@ -161,7 +165,8 @@
<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_pre_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>
@@ -175,7 +180,8 @@
<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_pre_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>

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

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

649
src/brain/src/cerebellum_hooks.cpp
View File

@@ -83,7 +83,7 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
}
} else if (goal.data_type >= node->arm_cmd_type_custom_min_ && goal.data_type <= node->arm_cmd_type_custom_max_) {
geometry_msgs::msg::Pose pose;
std::vector<double> angle{360.0, 360.0, 360.0, 360.0, 360.0, 360.0}; //invalid angles
std::vector<double> angle(6, 360.0); //invalid angles
const std::string side = (goal.body_id == RIGHT_ARM) ? "right_arm_" : "left_arm_";
const char * action = nullptr;
@@ -236,6 +236,8 @@ void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
} else if (p.motion_type == node->arm_cmd_type_take_object_) {
action = "take_object";
}
RCLCPP_INFO(node->get_logger(), "[%s] param[%zu]: action=%s, motion_type=%u",
skill_name.c_str(), i, action, p.motion_type);
if (action) {
geometry_msgs::msg::Pose pose;
@@ -248,7 +250,9 @@ void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
if (!angle.empty()) {
p.target_joints = angle;
}
p.motion_type = interfaces::msg::ArmMotionParams::MOVEJ; //MOVEJ or MOVEL
p.motion_type = (node->dual_arm_motion_type_ == "MOVEL")
? interfaces::msg::ArmMotionParams::MOVEL
: interfaces::msg::ArmMotionParams::MOVEJ;
}
RCLCPP_INFO(node->get_logger(),
@@ -681,36 +685,107 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
{
using VisionSrv = interfaces::srv::VisionObjectRecognition;
auto UpdateTargetPose = [node](const interfaces::msg::PoseClassAndID & obj, const std_msgs::msg::Header & header) -> bool {
for (auto &tf: node->target_frames_) {
if (tf == obj.class_name) {
node->target_pose_[tf].header = header;
if (node->camera_position_ == "left") {
node->target_pose_[tf].header.frame_id = node->left_camera_frame_;
} else if (node->camera_position_ == "right") {
node->target_pose_[tf].header.frame_id = node->right_camera_frame_;
}
node->target_pose_[tf].pose = obj.pose;
node->right_hand_grab_width_.clear();
int size = (int)(obj.grab_width.size());
for (int i = 0; i < size; i++) {
node->right_hand_grab_width_.push_back(obj.grab_width[i]);
RCLCPP_INFO(node->get_logger(), "grab_width : %.4f", obj.grab_width[i]);
}
const auto & p = obj.pose;
RCLCPP_INFO(node->get_logger(), "target_frame: %s, right_hand_grab_width_size: %zu, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
tf.c_str(), node->right_hand_grab_width_.size(),
p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
return true;
} else {
RCLCPP_WARN(node->get_logger(), "object class_name: %s, target_frame: %s not match", obj.class_name.c_str(), tf.c_str());
}
auto normalize_camera_position = [node](std::string & camera_position, const std::string & skill_name) {
if (camera_position != "left" && camera_position != "right" && camera_position != "top") {
RCLCPP_ERROR(node->get_logger(), "[%s] Invalid camera_position: %s", skill_name.c_str(), camera_position.c_str());
camera_position = "top";
}
RCLCPP_WARN(node->get_logger(), "object class_name not match target frames");
return false;
};
auto set_target_frame_id = [node](std_msgs::msg::Header & header) {
if (node->camera_position_ == "left") {
header.frame_id = node->left_camera_frame_;
} else if (node->camera_position_ == "right") {
header.frame_id = node->right_camera_frame_;
}
};
auto update_grab_width = [node](const std::vector<double> & widths) {
node->right_hand_grab_width_.clear();
node->right_hand_grab_width_.reserve(widths.size());
for (const auto w : widths) {
node->right_hand_grab_width_.push_back(static_cast<float>(w));
RCLCPP_INFO(node->get_logger(), "grab_width : %.4f", w);
}
};
auto UpdateTargetPose = [node, set_target_frame_id, update_grab_width](
const interfaces::msg::PoseClassAndID & obj,
const std_msgs::msg::Header & header) -> bool {
for (auto & tf : node->target_frames_) {
if (tf != obj.class_name) {
RCLCPP_WARN(node->get_logger(), "object class_name: %s, target_frame: %s not match", obj.class_name.c_str(), tf.c_str());
continue;
}
node->target_pose_[tf].header = header;
set_target_frame_id(node->target_pose_[tf].header);
node->target_pose_[tf].pose = obj.pose;
update_grab_width(obj.grab_width);
const auto & p = obj.pose;
RCLCPP_INFO(node->get_logger(), "target_frame: %s, right_hand_grab_width_size: %zu, target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
tf.c_str(), node->right_hand_grab_width_.size(),
p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
return true;
}
RCLCPP_WARN(node->get_logger(), "object class_name not match target frames");
return false;
};
auto CalculateGraspPoses = [node](const std::string & skill_name) -> bool {
auto apply_orientation = [](const std::vector<double> & orientation, geometry_msgs::msg::Pose & pose) {
if (orientation.size() == 4) {
pose.orientation.x = orientation[0];
pose.orientation.y = orientation[1];
pose.orientation.z = orientation[2];
pose.orientation.w = orientation[3];
}
};
auto log_joint_angles = [node](const std::string & label, const std::vector<double> & joint_angles) {
if (joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "%s joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
label.c_str(), joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "%s joint angles size invalid: %ld", label.c_str(), joint_angles.size());
}
};
auto push_top_cam_results = [node](
const std::string & target_arm,
const geometry_msgs::msg::Pose & pose,
const std::vector<double> & joint_angles) {
if (target_arm == "right_arm") {
node->arm_poses_["right_arm_take_photo"].push_back(pose);
node->arm_angles_["right_arm_take_photo"].push_back(joint_angles);
} else if (target_arm == "left_arm") {
node->arm_poses_["left_arm_take_photo"].push_back(pose);
node->arm_angles_["left_arm_take_photo"].push_back(joint_angles);
}
};
auto apply_side_cam_offsets = [node](brain::GraspResult & result) {
if (node->camera_position_ == "right") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + node->side_cam_right_arm_z_offset_); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + node->side_cam_right_arm_z_offset_); //补偿抓取的偏差
result.grasp_pose.position.setY(result.grasp_pose.position.y() + node->side_cam_right_arm_y_offset_); //补偿抓取的长度
} else if (node->camera_position_ == "left") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + node->side_cam_left_arm_z_offset_); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + node->side_cam_left_arm_z_offset_); //补偿抓取的偏差
result.grasp_pose.position.setY(result.grasp_pose.position.y() + node->side_cam_left_arm_y_offset_); //补偿抓取的长度
}
};
auto to_pose_msg = [](const tf2::Vector3 & position, const tf2::Quaternion & orientation) {
geometry_msgs::msg::Pose msg;
msg.position.x = position.x();
msg.position.y = position.y();
msg.position.z = position.z();
msg.orientation.x = orientation.x();
msg.orientation.y = orientation.y();
msg.orientation.z = orientation.z();
msg.orientation.w = orientation.w();
return msg;
};
//top cam
if (node->camera_position_ == "top") {
@@ -723,13 +798,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
target_arm = "right_arm";
target_pose.pose.position.x += node->top_cam_right_arm_x_offset_;
target_pose.pose.position.y = node->top_cam_right_arm_y_;
if (node->top_cam_right_arm_orientation_.size() == 4) {
target_pose.pose.orientation.x = node->top_cam_right_arm_orientation_[0];
target_pose.pose.orientation.y = node->top_cam_right_arm_orientation_[1];
target_pose.pose.orientation.z = node->top_cam_right_arm_orientation_[2];
target_pose.pose.orientation.w = node->top_cam_right_arm_orientation_[3];
}
apply_orientation(node->top_cam_right_arm_orientation_, target_pose.pose);
} else if (target_pose.pose.position.z <= node->top_cam_left_arm_z_threshold_) { //left arm
//transfer to left arm base
target_arm = "left_arm";
@@ -743,12 +812,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
target_pose.pose = left_arm_pose.pose;
target_pose.pose.position.x += node->top_cam_left_arm_x_offset_;
target_pose.pose.position.y = node->top_cam_left_arm_y_;
if (node->top_cam_left_arm_orientation_.size() == 4) {
target_pose.pose.orientation.x = node->top_cam_left_arm_orientation_[0];
target_pose.pose.orientation.y = node->top_cam_left_arm_orientation_[1];
target_pose.pose.orientation.z = node->top_cam_left_arm_orientation_[2];
target_pose.pose.orientation.w = node->top_cam_left_arm_orientation_[3];
}
apply_orientation(node->top_cam_left_arm_orientation_, target_pose.pose);
}
auto pose = target_pose.pose;
@@ -763,7 +827,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// return false;
// }
std::vector<double> joint_angles(6, 0.0);
std::vector<double> joint_angles(6, 360.0);
tf2::Vector3 target_pos = {pose.position.x, pose.position.y, pose.position.z};
tf2::Quaternion target_quat = {pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
int arm_id = (target_arm == "left_arm") ? 0 : 1;
@@ -772,28 +836,15 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
return false;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
if (joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "%s take_photo joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
target_arm.c_str(), joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "%s take_photo joint angles size invalid: %ld", target_arm.c_str(), joint_angles.size());
}
log_joint_angles(target_arm + " take_photo", joint_angles);
#endif
if(target_arm == "right_arm") {
node->arm_poses_["right_arm_take_photo"].push_back(pose);
node->arm_angles_["right_arm_take_photo"].push_back(joint_angles);
} else if (target_arm == "left_arm") {
node->arm_poses_["left_arm_take_photo"].push_back(pose);
node->arm_angles_["left_arm_take_photo"].push_back(joint_angles);
}
push_top_cam_results(target_arm, pose, joint_angles);
return true;
}
//left & right cam
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if (node->camera_position_ == "right") {
node->arm_target_frame_ = "base_link_rightarm";
} else if (node->camera_position_ == "left") {
@@ -807,298 +858,240 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
return false;
}
{
RCLCPP_INFO(node->get_logger(), "[%s] target pose transformed to %s frame", skill_name.c_str(), node->arm_target_frame_.c_str());
RCLCPP_INFO(node->get_logger(), "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z,
pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z);
tf2::Quaternion target_quat(pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
RCLCPP_INFO(node->get_logger(), "[%s] target pose transformed to %s frame", skill_name.c_str(), node->arm_target_frame_.c_str());
RCLCPP_INFO(node->get_logger(), "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z,
pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
// std::vector<double> best_angles = brain::GraspPoseCalculator::find_optimal_grasp_angles(target_pos, node->arm_target_frame_, 3);
// if (best_angles.empty()) {
// RCLCPP_WARN(node->get_logger(), "[%s]no valid grasp angles found", skill_name.c_str());
// return false;
// }
tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z);
tf2::Quaternion target_quat(pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
// best_angles.clear();
std::vector<double> best_angles = node->grasp_best_angles_;
int arm_id = ((node->camera_position_ == "left") ? 0 : 1);
// std::vector<double> best_angles = brain::GraspPoseCalculator::find_optimal_grasp_angles(target_pos, node->arm_target_frame_, 3);
// if (best_angles.empty()) {
// RCLCPP_WARN(node->get_logger(), "[%s]no valid grasp angles found", skill_name.c_str());
// return false;
// }
{
std::vector<std::string> palm_facings = node->grasp_palm_facings_;
// best_angles.clear();
std::vector<double> best_angles = node->grasp_best_angles_;
int arm_id = ((node->camera_position_ == "left") ? 0 : 1);
std::vector<std::string> palm_facings = node->grasp_palm_facings_;
for (double angle : best_angles) {
for (const auto& palm : palm_facings) {
brain::GraspResult result = brain::GraspPoseCalculator::calculate(target_pos,
target_quat, node->grasp_type_, angle, palm, node->grasp_safety_dist_, node->grasp_finger_length_, node->arm_target_frame_);
for (double angle : best_angles) {
for (const auto & palm : palm_facings) {
brain::GraspResult result = brain::GraspPoseCalculator::calculate(target_pos,
target_quat, node->grasp_type_, angle, palm, node->grasp_safety_dist_, node->grasp_finger_length_, node->arm_target_frame_);
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, pre_grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(), result.pre_grasp_pose.orientation.w());
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, pre_grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(), result.pre_grasp_pose.orientation.w());
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
#endif
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
#endif
if (node->camera_position_ == "right") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + node->side_cam_right_arm_z_offset_); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + node->side_cam_right_arm_z_offset_); //补偿抓取的偏差
result.grasp_pose.position.setY(result.grasp_pose.position.y() + node->side_cam_right_arm_y_offset_); //补偿抓取的长度
} else if (node->camera_position_ == "left") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + node->side_cam_left_arm_z_offset_); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + node->side_cam_left_arm_z_offset_); //补偿抓取的偏差
result.grasp_pose.position.setY(result.grasp_pose.position.y() + node->side_cam_left_arm_y_offset_); //补偿抓取的长度
}
//pre grasp
std::vector<double> pre_grasp_joint_angles(6, 0.0);
if (node->CallInverseKinematics(result.pre_grasp_pose.position,
result.pre_grasp_pose.orientation, pre_grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
//grasp
std::vector<double> grasp_joint_angles(6, 0.0);
if (node->CallInverseKinematics(result.grasp_pose.position,
result.grasp_pose.orientation, grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
//take object
auto take_object_pose = result.grasp_pose;
// take_object_pose.position.setX(take_object_pose.position.x() - 0.10); //向上拿起物体
take_object_pose.position.setX(node->take_object_arm_x_); //向上拿起物体
std::vector<double> take_object_joint_angles(6, 0.0);
if (node->CallInverseKinematics(take_object_pose.position,
take_object_pose.orientation, take_object_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s take_object joint_angles rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
if (pre_grasp_joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "pre_grasp joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
pre_grasp_joint_angles[0], pre_grasp_joint_angles[1], pre_grasp_joint_angles[2],
pre_grasp_joint_angles[3], pre_grasp_joint_angles[4], pre_grasp_joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "pre_grasp joint angles size invalid: %ld", pre_grasp_joint_angles.size());
}
if (grasp_joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "grasp joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
grasp_joint_angles[0], grasp_joint_angles[1], grasp_joint_angles[2],
grasp_joint_angles[3], grasp_joint_angles[4], grasp_joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "grasp joint angles size invalid: %ld", grasp_joint_angles.size());
}
if (take_object_joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "take_object joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
take_object_joint_angles[0], take_object_joint_angles[1], take_object_joint_angles[2],
take_object_joint_angles[3], take_object_joint_angles[4], take_object_joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "take_object joint angles size invalid: %ld", take_object_joint_angles.size());
}
#endif
geometry_msgs::msg::Pose pre_grasp_msg;
pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
pre_grasp_msg.position.z = result.pre_grasp_pose.position.z();
pre_grasp_msg.orientation.x = result.pre_grasp_pose.orientation.x();
pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
geometry_msgs::msg::Pose grasp_msg;
grasp_msg.position.x = result.grasp_pose.position.x();
grasp_msg.position.y = result.grasp_pose.position.y();
grasp_msg.position.z = result.grasp_pose.position.z();
grasp_msg.orientation.x = result.grasp_pose.orientation.x();
grasp_msg.orientation.y = result.grasp_pose.orientation.y();
grasp_msg.orientation.z = result.grasp_pose.orientation.z();
grasp_msg.orientation.w = result.grasp_pose.orientation.w();
geometry_msgs::msg::Pose take_object_msg;
take_object_msg.position.x = take_object_pose.position.x();
take_object_msg.position.y = take_object_pose.position.y();
take_object_msg.position.z = take_object_pose.position.z();
take_object_msg.orientation.x = take_object_pose.orientation.x();
take_object_msg.orientation.y = take_object_pose.orientation.y();
take_object_msg.orientation.z = take_object_pose.orientation.z();
take_object_msg.orientation.w = take_object_pose.orientation.w();
if (node->camera_position_ == "right") {
node->arm_poses_["right_arm_pre_grasp"].push_back(pre_grasp_msg);
node->arm_poses_["right_arm_grasp"].push_back(grasp_msg);
node->arm_poses_["right_arm_take_object"].push_back(take_object_msg);
node->arm_angles_["right_arm_pre_grasp"].push_back(pre_grasp_joint_angles);
node->arm_angles_["right_arm_grasp"].push_back(grasp_joint_angles);
node->arm_angles_["right_arm_take_object"].push_back(take_object_joint_angles);
} else if (node->camera_position_ == "left") {
node->arm_poses_["left_arm_pre_grasp"].push_back(pre_grasp_msg);
node->arm_poses_["left_arm_grasp"].push_back(grasp_msg);
node->arm_poses_["left_arm_take_object"].push_back(take_object_msg);
node->arm_angles_["left_arm_pre_grasp"].push_back(pre_grasp_joint_angles);
node->arm_angles_["left_arm_grasp"].push_back(grasp_joint_angles);
node->arm_angles_["left_arm_take_object"].push_back(take_object_joint_angles);
}
RCLCPP_INFO(node->get_logger(), "Generated Pose Success (grasp_type: %s, Angle: %.1f, Palm: %s)", result.name.c_str(), angle, palm.c_str());
}
apply_side_cam_offsets(result);
//pre grasp
std::vector<double> pre_grasp_joint_angles(6, 360.0);
if (node->CallInverseKinematics(result.pre_grasp_pose.position,
result.pre_grasp_pose.orientation, pre_grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
//grasp
std::vector<double> grasp_joint_angles(6, 360.0);
if (node->CallInverseKinematics(result.grasp_pose.position,
result.grasp_pose.orientation, grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
//take object
auto take_object_pose = result.grasp_pose;
// take_object_pose.position.setX(take_object_pose.position.x() - 0.10); //向上拿起物体
take_object_pose.position.setX(node->take_object_arm_x_); //向上拿起物体
std::vector<double> take_object_joint_angles(6, 360.0);
if (node->CallInverseKinematics(take_object_pose.position,
take_object_pose.orientation, take_object_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s take_object joint_angles rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
log_joint_angles("pre_grasp", pre_grasp_joint_angles);
log_joint_angles("grasp", grasp_joint_angles);
log_joint_angles("take_object", take_object_joint_angles);
#endif
geometry_msgs::msg::Pose pre_grasp_msg = to_pose_msg(result.pre_grasp_pose.position, result.pre_grasp_pose.orientation);
geometry_msgs::msg::Pose grasp_msg = to_pose_msg(result.grasp_pose.position, result.grasp_pose.orientation);
geometry_msgs::msg::Pose take_object_msg = to_pose_msg(take_object_pose.position, take_object_pose.orientation);
if (node->camera_position_ == "right") {
node->arm_poses_["right_arm_pre_grasp"].push_back(pre_grasp_msg);
node->arm_poses_["right_arm_grasp"].push_back(grasp_msg);
node->arm_poses_["right_arm_take_object"].push_back(take_object_msg);
node->arm_angles_["right_arm_pre_grasp"].push_back(pre_grasp_joint_angles);
node->arm_angles_["right_arm_grasp"].push_back(grasp_joint_angles);
node->arm_angles_["right_arm_take_object"].push_back(take_object_joint_angles);
} else if (node->camera_position_ == "left") {
node->arm_poses_["left_arm_pre_grasp"].push_back(pre_grasp_msg);
node->arm_poses_["left_arm_grasp"].push_back(grasp_msg);
node->arm_poses_["left_arm_take_object"].push_back(take_object_msg);
node->arm_angles_["left_arm_pre_grasp"].push_back(pre_grasp_joint_angles);
node->arm_angles_["left_arm_grasp"].push_back(grasp_joint_angles);
node->arm_angles_["left_arm_take_object"].push_back(take_object_joint_angles);
}
RCLCPP_INFO(node->get_logger(), "Generated Pose Success (grasp_type: %s, Angle: %.1f, Palm: %s)", result.name.c_str(), angle, palm.c_str());
}
}
return true;
};
{
SkillManager::ServiceHookOptions<VisionSrv> hooks;
hooks.wait_timeout = [node](const std::string &) {
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(node->vision_object_recognition_wait_timeout_sec_));
};
hooks.call_timeout = [node](const std::string &) {
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(node->vision_object_recognition_call_timeout_sec_));
};
hooks.make_request = [node](const std::string & skill_name) {
auto req = std::make_shared<VisionSrv::Request>();
// Prefer payload_yaml from calls
if (!node->TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
RCLCPP_ERROR(node->get_logger(), "[%s] Payload Parse failed", skill_name.c_str());
return req;
}
// fallback to parameter
// const std::string param = "vision_object_recognition.camera_position";
// if (!node->has_parameter(param)) {
// node->declare_parameter<std::string>(param, std::string("right"));
// }
// req->camera_position = node->get_parameter(param).as_string();
auto clear_cached_grasp_data = [node]() {
if (node->camera_position_ == "left") {
node->arm_poses_["left_arm_pre_grasp"].clear();
node->arm_poses_["left_arm_grasp"].clear();
node->arm_poses_["left_arm_take_object"].clear();
node->arm_angles_["left_arm_pre_grasp"].clear();
node->arm_angles_["left_arm_grasp"].clear();
node->arm_angles_["left_arm_take_object"].clear();
} else if (node->camera_position_ == "right") {
node->arm_poses_["right_arm_pre_grasp"].clear();
node->arm_poses_["right_arm_grasp"].clear();
node->arm_poses_["right_arm_take_object"].clear();
node->arm_angles_["right_arm_pre_grasp"].clear();
node->arm_angles_["right_arm_grasp"].clear();
node->arm_angles_["right_arm_take_object"].clear();
} else if (node->camera_position_ == "top") {
node->arm_poses_["left_arm_take_photo"].clear();
node->arm_angles_["left_arm_take_photo"].clear();
node->arm_poses_["right_arm_take_photo"].clear();
node->arm_angles_["right_arm_take_photo"].clear();
}
};
if (req->camera_position != "left" && req->camera_position != "right" && req->camera_position != "top") {
RCLCPP_ERROR(node->get_logger(), "[%s] Invalid camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
req->camera_position = "top";
auto validate_generated_results = [node](const std::string & skill_name) -> bool {
if (node->camera_position_ == "top") {
if (node->arm_poses_["right_arm_take_photo"].empty() && node->arm_poses_["left_arm_take_photo"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo poses generated", skill_name.c_str());
return false;
}
if (node->arm_angles_["right_arm_take_photo"].empty() && node->arm_angles_["left_arm_take_photo"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo angles generated", skill_name.c_str());
return false;
}
return true;
}
const bool is_right = (node->camera_position_ == "right");
const std::string pose_prefix = is_right ? "right_arm_" : "left_arm_";
const auto & pre_grasp_pose = node->arm_poses_[pose_prefix + "pre_grasp"];
const auto & grasp_pose = node->arm_poses_[pose_prefix + "grasp"];
const auto & pre_grasp_angle = node->arm_angles_[pose_prefix + "pre_grasp"];
const auto & grasp_angle = node->arm_angles_[pose_prefix + "grasp"];
const auto & take_object_angle = node->arm_angles_[pose_prefix + "take_object"];
if (pre_grasp_pose.empty() || grasp_pose.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (pre_grasp_pose.size() != grasp_pose.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] %s_pre_grasp size %zu not match %s_grasp size %zu",
skill_name.c_str(), pose_prefix.c_str(), pre_grasp_pose.size(), pose_prefix.c_str(), grasp_pose.size());
return false;
}
if (pre_grasp_angle.empty() || grasp_angle.empty() || take_object_angle.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (pre_grasp_angle.size() != grasp_angle.size() ||
grasp_angle.size() != take_object_angle.size() ||
pre_grasp_angle.size() != take_object_angle.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
skill_name.c_str(), pre_grasp_angle.size(), grasp_angle.size(), take_object_angle.size());
return false;
}
return true;
};
SkillManager::ServiceHookOptions<VisionSrv> hooks;
hooks.wait_timeout = [node](const std::string &) {
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(node->vision_object_recognition_wait_timeout_sec_));
};
hooks.call_timeout = [node](const std::string &) {
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(node->vision_object_recognition_call_timeout_sec_));
};
hooks.make_request = [node, normalize_camera_position](const std::string & skill_name) {
auto req = std::make_shared<VisionSrv::Request>();
// Prefer payload_yaml from calls
if (!node->TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
RCLCPP_ERROR(node->get_logger(), "[%s] Payload Parse failed", skill_name.c_str());
return req;
}
// fallback to parameter
// const std::string param = "vision_object_recognition.camera_position";
// if (!node->has_parameter(param)) {
// node->declare_parameter<std::string>(param, std::string("right"));
// }
// req->camera_position = node->get_parameter(param).as_string();
node->camera_position_ = req->camera_position;
RCLCPP_INFO(node->get_logger(), "[%s] camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
return req;
};
hooks.on_response = [node, UpdateTargetPose, CalculateGraspPoses](
const std::string & skill_name,
const SkillManager::ServiceHookOptions<VisionSrv>::ClientPtr &,
const SkillManager::ServiceHookOptions<VisionSrv>::ResponsePtr & resp,
std::string & detail) {
if (!resp->success) {
detail = resp->info.empty() ? std::string("Not success") : resp->info;
RCLCPP_WARN(node->get_logger(), "[%s] VisionObjectRecognition failed: %s",
skill_name.c_str(), detail.c_str());
return false;
normalize_camera_position(req->camera_position, skill_name);
node->camera_position_ = req->camera_position;
RCLCPP_INFO(node->get_logger(), "[%s] camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
return req;
};
hooks.on_response = [node, UpdateTargetPose, CalculateGraspPoses, clear_cached_grasp_data, validate_generated_results](
const std::string & skill_name,
const SkillManager::ServiceHookOptions<VisionSrv>::ClientPtr &,
const SkillManager::ServiceHookOptions<VisionSrv>::ResponsePtr & resp,
std::string & detail) {
if (!resp->success) {
detail = resp->info.empty() ? std::string("Not success") : resp->info;
RCLCPP_WARN(node->get_logger(), "[%s] VisionObjectRecognition failed: %s",
skill_name.c_str(), detail.c_str());
return false;
}
detail = resp->info.empty() ? std::string("success pose") : resp->info;
const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
RCLCPP_INFO(node->get_logger(), "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
skill_name.c_str(), resp->success, resp->objects.size(), resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
clear_cached_grasp_data();
for (size_t i = 0; i < resp->objects.size(); ++i) {
const auto & obj = resp->objects[i];
// RCLCPP_INFO(node->get_logger(), " [%zu] class='%s' id=%d", i, obj.class_name.c_str(), obj.class_id);
if (!UpdateTargetPose(obj, resp->header)) {
continue;
}
detail = resp->info.empty() ? std::string("success pose") : resp->info;
const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
RCLCPP_INFO(node->get_logger(), "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
skill_name.c_str(), resp->success, resp->objects.size(), resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
if (node->camera_position_ == "left") {
node->arm_poses_["left_arm_pre_grasp"].clear();
node->arm_poses_["left_arm_grasp"].clear();
node->arm_poses_["left_arm_take_object"].clear();
node->arm_angles_["left_arm_pre_grasp"].clear();
node->arm_angles_["left_arm_grasp"].clear();
node->arm_angles_["left_arm_take_object"].clear();
} else if (node->camera_position_ == "right") {
node->arm_poses_["right_arm_pre_grasp"].clear();
node->arm_poses_["right_arm_grasp"].clear();
node->arm_poses_["right_arm_take_object"].clear();
node->arm_angles_["right_arm_pre_grasp"].clear();
node->arm_angles_["right_arm_grasp"].clear();
node->arm_angles_["right_arm_take_object"].clear();
} else if (node->camera_position_ == "top") {
node->arm_poses_["left_arm_take_photo"].clear();
node->arm_angles_["left_arm_take_photo"].clear();
node->arm_poses_["right_arm_take_photo"].clear();
node->arm_angles_["right_arm_take_photo"].clear();
}
for (size_t i = 0; i < resp->objects.size(); ++i) {
const auto & obj = resp->objects[i];
// RCLCPP_INFO(node->get_logger(), " [%zu] class='%s' id=%d", i, obj.class_name.c_str(), obj.class_id);
if (!UpdateTargetPose(obj, resp->header)) {
continue;
}
#ifdef ENABLE_CAL_GRASP_POSE
if (!CalculateGraspPoses(skill_name)) {
continue;
}
if (!CalculateGraspPoses(skill_name)) {
continue;
}
#endif
}
}
if (node->camera_position_ == "top") {
if (node->arm_poses_["right_arm_take_photo"].empty() && node->arm_poses_["left_arm_take_photo"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo poses generated", skill_name.c_str());
return false;
}
if (node->arm_angles_["right_arm_take_photo"].empty() && node->arm_angles_["left_arm_take_photo"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid hand take photo angles generated", skill_name.c_str());
return false;
}
} else if (node->camera_position_ == "right") {
if (node->arm_poses_["right_arm_pre_grasp"].empty() || node->arm_poses_["right_arm_grasp"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->arm_poses_["right_arm_pre_grasp"].size() != node->arm_poses_["right_arm_grasp"].size()) {
RCLCPP_WARN(node->get_logger(), "[%s] right_arm_pre_grasp size %zu not match right_arm_grasp size %zu",
skill_name.c_str(), node->arm_poses_["right_arm_pre_grasp"].size(), node->arm_poses_["right_arm_grasp"].size());
return false;
}
if (node->arm_angles_["right_arm_pre_grasp"].empty() || node->arm_angles_["right_arm_grasp"].empty() || node->arm_angles_["right_arm_take_object"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->arm_angles_["right_arm_pre_grasp"].size() != node->arm_angles_["right_arm_grasp"].size() ||
node->arm_angles_["right_arm_grasp"].size() != node->arm_angles_["right_arm_take_object"].size() ||
node->arm_angles_["right_arm_pre_grasp"].size() != node->arm_angles_["right_arm_take_object"].size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
skill_name.c_str(), node->arm_angles_["right_arm_pre_grasp"].size(), node->arm_angles_["right_arm_grasp"].size(), node->arm_angles_["right_arm_take_object"].size());
return false;
}
} else if (node->camera_position_ == "left") {
if (node->arm_poses_["left_arm_pre_grasp"].empty() || node->arm_poses_["left_arm_grasp"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->arm_poses_["left_arm_pre_grasp"].size() != node->arm_poses_["left_arm_grasp"].size()) {
RCLCPP_WARN(node->get_logger(), "[%s] left_arm_pre_grasp size %zu not match left_arm_grasp size %zu",
skill_name.c_str(), node->arm_poses_["left_arm_pre_grasp"].size(), node->arm_poses_["left_arm_grasp"].size());
return false;
}
if (node->arm_angles_["left_arm_pre_grasp"].empty() || node->arm_angles_["left_arm_grasp"].empty() || node->arm_angles_["left_arm_take_object"].empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->arm_angles_["left_arm_pre_grasp"].size() != node->arm_angles_["left_arm_grasp"].size() ||
node->arm_angles_["left_arm_grasp"].size() != node->arm_angles_["left_arm_take_object"].size() ||
node->arm_angles_["left_arm_pre_grasp"].size() != node->arm_angles_["left_arm_take_object"].size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
skill_name.c_str(), node->arm_angles_["left_arm_pre_grasp"].size(), node->arm_angles_["left_arm_grasp"].size(), node->arm_angles_["left_arm_take_object"].size());
return false;
}
}
return true;
};
node->skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));
}
return validate_generated_results(skill_name);
};
node->skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));
}
void CerebellumHooks::ConfigureMapLoadHooks(CerebellumNode * node)

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

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