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

modify sch action and params

Browse Source
This commit is contained in:
david
2025-10-29 10:51:23 +08:00
parent 6e2f06b26a
commit 5b83a865cc
5 changed files with 202 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

156
src/brain/config/bt_carry_boxes_sch1.params.yaml
View File

@@ -6,13 +6,7 @@
params: '{}
'
- name: s1_waist_bend_down
params: 'move_pitch_degree: 2.5
move_yaw_degree: -10.0
'
- name: s2_arm_stretch_out
- name: s1_arm_move_origin_position
params: 'body_id: 0
data_type: 2
@@ -23,16 +17,10 @@
frame_time_stamp: 0
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
'
- name: s3_hand_pickup
params: 'mode: 0
effort: 0.0
'
- name: s4_arm_lift
- name: s2_arm_move_to_snapshot_pose
params: 'body_id: 0
data_type: 2
@@ -43,38 +31,102 @@
frame_time_stamp: 0
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
data_array: [-85, -35, -40, 1, -70, -180, -95, 35, 40, -1, 70, -10]
'
- name: s5_waist_bend_up
params: 'move_pitch_degree: 1.0
- name: s3_wheel_move_to_pickup_position
params: 'move_distance: 0.5
move_yaw_degree: 3.0
move_angle: 0.0
'
- name: s6_waist_turn_around
- name: s4_arm_grasp_box
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-80, -65, -36, 1, -80, -170, -100, 65, 36, -1, 80, -10]
'
- name: s5_arm_pickup_box
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-98, -65, -36, 1, -80, -170, -82, 65, 36, -1, 80, -10]
'
- name: s6_wheel_move_back_to_origin
params: 'move_distance: -0.5
move_angle: 0.0
'
- name: s7_waist_turn_around
params: 'move_pitch_degree: 180.0
move_yaw_degree: 0.0
'
- name: s7_leg_move_back
params: 'move_up_distance: 0.5
- name: s8_waist_bend_down
params: 'move_pitch_degree: 0.0
move_yaw_degree: 15.0
'
- name: s8_wheel_move_back
params: 'move_distance: 1.0
- name: s9_arm_put_down_box
params: 'body_id: 0
move_angle: 0.0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-80, -65, -36, 1, -80, -170, -100, 65, 36, -1, 80, -10]
'
- name: s9_waist_bend_down
params: 'move_pitch_degree: 5.0
- name: s10_arm_release_box
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-80, -60, -1, 1, -80, -170, -100, 60, 1, -1, 80, -10]
'
- name: s11_waist_bend_up
params: 'move_pitch_degree: 0.0
move_yaw_degree: -15.0
'
- name: s12_waist_turn_around
params: 'move_pitch_degree: -180.0
move_yaw_degree: 0.0
'
- name: s10_arm_stretch_out
- name: s13_arm_move_origin_position
params: 'body_id: 0
data_type: 2
@@ -85,52 +137,6 @@
frame_time_stamp: 0
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'
- name: s11_hand_release
params: 'mode: 1
effort: 0.0
'
- name: s12_arm_move_to_snapshot_pose
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
'
- name: s13_camera_take_photo
params: 'mode: 1
effort: 0.0
'
- name: s14_waist_bend_up
params: 'move_pitch_degree: -5.0
move_yaw_degree: 4.0
'
- name: s15_arm_retract
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
'

29
src/brain/config/bt_carry_boxes_sch1.xml
View File

@@ -4,22 +4,19 @@
<!-- Retry waist + arm + hand until HandControl_H succeeds -->
<RetryUntilSuccessful name="retry_all_action" num_attempts="5">
<Sequence>
<!-- <MoveHome_H name="s0_move_home" /> -->
<MoveWaist_H name="s1_waist_bend_down" />
<Arm_H name="s2_arm_stretch_out" />
<HandControl_H name="s3_hand_pickup" />
<Arm_H name="s4_arm_lift" />
<MoveWaist_H name="s5_waist_bend_up" />
<MoveWaist_H name="s6_waist_turn_around" />
<MoveLeg_H name="s7_leg_move_back" />
<MoveWheel_H name="s8_wheel_move_back" />
<MoveWaist_H name="s9_waist_bend_down" />
<Arm_H name="s10_arm_stretch_out" />
<HandControl_H name="s11_hand_release" />
<Arm_H name="s12_arm_move_to_snapshot_pose" />
<CameraTakePhoto_H name="s13_camera_take_photo" />
<MoveWaist_H name="s14_waist_bend_up" />
<Arm_H name="s15_arm_retract" />
<Arm_H name="s1_arm_move_origin_position" />
<Arm_H name="s2_arm_move_to_snapshot_pose" />
<MoveWheel_H name="s3_wheel_move_to_pickup_position" />
<Arm_H name="s4_arm_grasp_box" />
<Arm_H name="s5_arm_pickup_box" />
<MoveWheel_H name="s6_wheel_move_back_to_origin" />
<MoveWaist_H name="s7_waist_turn_around" />
<MoveWaist_H name="s8_waist_bend_down" />
<Arm_H name="s9_arm_put_down_box" />
<Arm_H name="s10_arm_release_box" />
<MoveWaist_H name="s11_waist_bend_up" />
<MoveWaist_H name="s12_waist_turn_around" />
<Arm_H name="s13_arm_move_origin_position" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>

124
src/brain/config/bt_carry_boxes_sch2.params.yaml
View File

@@ -6,13 +6,7 @@
params: '{}
'
- name: s1_waist_bend_down
params: 'move_pitch_degree: 13.0
move_yaw_degree: -30.0
'
- name: s2_arm_stretch_out
- name: s1_arm_move_origin_position
params: 'body_id: 0
data_type: 2
@@ -23,16 +17,10 @@
frame_time_stamp: 0
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
'
- name: s3_hand_pickup
params: 'mode: 1
effort: 0.0
'
- name: s4_arm_lift
- name: s2_arm_move_to_snapshot_pose
params: 'body_id: 0
data_type: 2
@@ -43,38 +31,62 @@
frame_time_stamp: 0
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
data_array: [-85, -35, -40, 1, -70, -180, -95, 35, 40, -1, 70, -10]
'
- name: s5_waist_bend_up
params: 'move_pitch_degree: 3.0
move_yaw_degree: 5.0
'
- name: s6_waist_turn_around
params: 'move_pitch_degree: 120.0
move_yaw_degree: 30.0
'
- name: s7_leg_move_back
params: 'move_up_distance: -0.5
'
- name: s8_wheel_move_back
params: 'move_distance: -2.0
- name: s3_wheel_move_to_pickup_position
params: 'move_distance: 0.5
move_angle: 0.0
'
- name: s9_waist_bend_down
params: 'move_pitch_degree: -5.0
- name: s4_arm_grasp_box
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-80, -65, -36, 1, -80, -170, -100, 65, 36, -1, 80, -10]
'
- name: s5_arm_pickup_box
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-98, -65, -36, 1, -80, -170, -82, 65, 36, -1, 80, -10]
'
- name: s6_wheel_move_back_to_origin
params: 'move_distance: -0.5
move_angle: 0.0
'
- name: s7_waist_turn_around
params: 'move_pitch_degree: 180.0
move_yaw_degree: 0.0
'
- name: s10_arm_stretch_out
- name: s8_waist_bend_down
params: 'move_pitch_degree: 0.0
move_yaw_degree: 15.0
'
- name: s9_arm_put_down_box
params: 'body_id: 0
data_type: 2
@@ -85,16 +97,10 @@
frame_time_stamp: 0
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
data_array: [-80, -65, -36, 1, -80, -170, -100, 65, 36, -1, 80, -10]
'
- name: s11_hand_release
params: 'mode: 0
effort: 0.0
'
- name: s15_arm_retract
- name: s10_arm_release_box
params: 'body_id: 0
data_type: 2
@@ -105,6 +111,32 @@
frame_time_stamp: 0
data_array: [-76.15, -59.43, -39.243, 0.134, -77.463, -168.251, -102.251, 55.548, 33.71, -1.496, 92.311, -9.148]
data_array: [-80, -60, -1, 1, -80, -170, -100, 60, 1, -1, 80, -10]
'
- name: s11_waist_bend_up
params: 'move_pitch_degree: 0.0
move_yaw_degree: -15.0
'
- name: s12_waist_turn_around
params: 'move_pitch_degree: -180.0
move_yaw_degree: 0.0
'
- name: s13_arm_move_origin_position
params: 'body_id: 0
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
'

29
src/brain/config/bt_carry_boxes_sch2.xml
View File

@@ -4,22 +4,19 @@
<!-- Retry waist + arm + hand until HandControl_H succeeds -->
<RetryUntilSuccessful name="retry_all_action" num_attempts="5">
<Sequence>
<!-- <MoveHome_H name="s0_move_home" /> -->
<MoveWaist_H name="s1_waist_bend_down" />
<Arm_H name="s2_arm_stretch_out" />
<HandControl_H name="s3_hand_pickup" />
<Arm_H name="s4_arm_lift" />
<MoveWaist_H name="s5_waist_bend_up" />
<MoveWaist_H name="s6_waist_turn_around" />
<MoveLeg_H name="s7_leg_move_back" />
<MoveWheel_H name="s8_wheel_move_back" />
<MoveWaist_H name="s9_waist_bend_down" />
<Arm_H name="s10_arm_stretch_out" />
<HandControl_H name="s11_hand_release" />
<!-- <Arm_H name="s12_arm_move_to_snapshot_pose" />
<CameraTakePhoto_H name="s13_camera_take_photo" />
<MoveWaist_H name="s14_waist_bend_up" /> -->
<Arm_H name="s15_arm_retract" />
<Arm_H name="s1_arm_move_origin_position" />
<Arm_H name="s2_arm_move_to_snapshot_pose" />
<MoveWheel_H name="s3_wheel_move_to_pickup_position" />
<Arm_H name="s4_arm_grasp_box" />
<Arm_H name="s5_arm_pickup_box" />
<MoveWheel_H name="s6_wheel_move_back_to_origin" />
<MoveWaist_H name="s7_waist_turn_around" />
<MoveWaist_H name="s8_waist_bend_down" />
<Arm_H name="s9_arm_put_down_box" />
<Arm_H name="s10_arm_release_box" />
<MoveWaist_H name="s11_waist_bend_up" />
<MoveWaist_H name="s12_waist_turn_around" />
<Arm_H name="s13_arm_move_origin_position" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>

17
src/brain/src/cerebrum_node.cpp
View File

@@ -72,6 +72,11 @@ struct UpdatingFlagGuard
~UpdatingFlagGuard() {flag = false;}
};
// Simple, file-local cooldown between successive BT leaf actions.
// We use steady_clock to avoid ROS time/clock-type complications.
static std::chrono::steady_clock::time_point g_bt_resume_time_steady{};
static constexpr std::chrono::milliseconds kInterActionDelayMs{3000};
} // namespace
/**
@@ -351,6 +356,14 @@ void CerebrumNode::ExecuteBehaviorTree()
if (bt_updating_ || !bt_pending_run_) {
return;
}
// Enforce a fixed cooldown between actions if set.
if (g_bt_resume_time_steady.time_since_epoch().count() != 0) {
if (std::chrono::steady_clock::now() < g_bt_resume_time_steady) {
return;
} else {
g_bt_resume_time_steady = std::chrono::steady_clock::time_point{};
}
}
if (!tree_.rootNode()) {
bt_pending_run_ = false;
return;
@@ -544,9 +557,13 @@ void CerebrumNode::RegisterSkillBtActions()
info.done_instances.insert(instance_name);
}
info.current_instance.reset();
// Set a fixed cooldown between actions on SUCCESS
g_bt_resume_time_steady = std::chrono::steady_clock::now() + kInterActionDelayMs;
return BT::NodeStatus::SUCCESS;
}
info.current_instance.reset();
// Set a fixed cooldown between actions on FAILURE as well
g_bt_resume_time_steady = std::chrono::steady_clock::now() + kInterActionDelayMs;
return BT::NodeStatus::FAILURE;
};
handlers.on_halted = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {