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

add left arm bt xml

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-08 17:10:53 +08:00
parent 68563e2162
commit 1c7605e397
6 changed files with 388 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

214
src/brain/config/bt_vision_grasp_left_arm.params.yaml Normal file
View File

@@ -0,0 +1,214 @@
- 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_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_wheel_move_to_origin_pickup_position
params: 'move_distance: 0.5
move_angle: 0.0
'
- 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]
'
- name: s2_top_camera_vision_recg
params: 'camera_position: top
'
- name: s2_left_arm_cam_take_photo
params: 'body_id: 1
data_type: 100
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s3_left_camera_vision_recg
params: 'camera_position: left
'
- name: s3_left_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: []
'
- name: s4_left_arm_vision_grasp
params: 'body_id: 1
data_type: 102
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s4_gripper_close
params: 'loc: 200
speed: 150
torque: 100
mode: 1
'
- name: s4_left_arm_take_box
params: 'body_id: 1
data_type: 110
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- 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]
'
- name: s6_move_waist_turn_left_90
params: 'move_pitch_degree: 0.0
move_yaw_degree: 90.0
'
- name: s6_wheel_move_to_putdown_position
params: 'move_distance: 0.0
move_angle: 0.0
'
- name: s7_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]
'
- 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]
'
- 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: 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: 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

165
src/brain/config/bt_vision_grasp_left_arm.xml Normal file
View File

@@ -0,0 +1,165 @@
<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>
<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" /> -->
<GripperCmd0_H name="s1_gripper_open" />
<Arm_H name="s2_left_arm_pre_cam_take_photo" />
</Parallel>
<SubTree ID="vision_top_cam_subtree"/>
<Arm_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>
<Arm_H name="s3_left_arm_vision_pre_grasp" />
<Arm_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" />
</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_left_90" />
</Parallel> -->
<Arm_H name="s7_left_arm_grasp_box" />
<Arm_H name="s7_left_arm_put_down_box" />
<GripperCmd0_H name="s8_gripper_open" />
<Arm_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" />
</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" />
<Arm_H name="s13_left_arm_pre_cam_take_photo" />
<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" /> -->
<Arm_H name="s14_left_arm_pre_cam_take_photo" />
<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" />
<Arm_H name="s15_arm_pre_grasp" />
<Arm_H name="s15_left_arm_pre_cam_take_photo" />
<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" />
<Arm_H name="s16_arm_pre_grasp" />
<Arm_H name="s16_left_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_putdown" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
</BehaviorTree>
</root>

0
src/brain/config/bt_vision_grasp.params.yaml → src/brain/config/bt_vision_grasp_right_arm.params.yaml
View File

0
src/brain/config/bt_vision_grasp.xml → src/brain/config/bt_vision_grasp_right_arm.xml
View File

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

@@ -7,7 +7,7 @@
config_params_path: [
{config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
{config: /config/bt_vision_grasp.xml, param: /config/bt_vision_grasp.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}
]
@@ -16,6 +16,6 @@
config_params_path: [
{config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
{config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
{config: /config/bt_vision_grasp.xml, param: /config/bt_vision_grasp.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}
]

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

@@ -913,6 +913,11 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// }
// req->camera_position = node->get_parameter(param).as_string();
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";
}
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;
@@ -969,11 +974,11 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
}
if (node->camera_position_ == "top") {
if (node->right_arm_take_photo_poses_.empty() || node->left_arm_take_photo_poses_.empty()) {
if (node->right_arm_take_photo_poses_.empty() && node->left_arm_take_photo_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo poses generated", skill_name.c_str());
return false;
}
if (node->right_arm_take_photo_angles_.empty() || node->left_arm_take_photo_angles_.empty()) {
if (node->right_arm_take_photo_angles_.empty() && node->left_arm_take_photo_angles_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo angles generated", skill_name.c_str());
return false;
}
@@ -1022,9 +1027,6 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
return false;
}
}
// node->camera_position_.clear();
return true;
};
node->skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));