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

optimize sch&params

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-05 17:20:21 +08:00
parent 4ec5ff7a05
commit 1ee0c96bb6
8 changed files with 109 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,8 +9,8 @@ cerebrum_node:
stats_publish_interval_ms: 10000
# Configuration paths
robot_config_path: ""
skill_file_path: ""
# robot_config_path: ""
# skill_file_path: ""
# Behavior tree parameters
default_bt_timeout: 300.0
@@ -59,8 +59,8 @@ cerebellum_node:
slam_enable_mapping_default: true
# Configuration paths
robot_config_path: ""
robot_skill_file_path: ""
# robot_config_path: ""
# robot_skill_file_path: ""
# Statistics parameters
enable_stats_publisher: true
@@ -70,5 +70,5 @@ cerebellum_node:
skill_timeouts: ""
# Arm-specific parameters
arm_target_frame: "base_link"
arm_target_frame: "base_link_rightarm"
arm_transform_timeout_sec: 2.0

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

@@ -4,6 +4,7 @@
<!-- Retry waist + arm + hand until HandControl_H succeeds -->
<RetryUntilSuccessful name="retry_all_action" num_attempts="3">
<Sequence>
<ClearDoneInstances_H name="clear_done_instances_retry" />
<!-- <MoveHome_H name="s0_motion_move_home" /> -->
<Arm_H name="s1_arm_move_origin_position" />
<MoveWheel_H name="s3_wheel_move_to_pickup_position" />

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

@@ -4,6 +4,7 @@
<!-- Retry waist + arm + hand until HandControl_H succeeds -->
<RetryUntilSuccessful name="retry_all_action" num_attempts="3">
<Sequence>
<ClearDoneInstances_H name="clear_done_instances_retry" />
<Arm_H name="s1_arm_move_origin_position" />
<MoveWheel_H name="s2_wheel_move_to_origin_pickup_position" />
<Arm_H name="s4_arm_move_pre_origin_position" />

88
src/brain/config/bt_vision_grasp.params.yaml
View File

@@ -7,13 +7,13 @@
'
- name: s1_move_waist_turn_right_90
params: 'move_pitch_degree: 0.0
params: &waist_turn_right_90 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
- name: s1_gripper_open
params: 'loc: 0
params: &gripper_open 'loc: 0
speed: 150
@@ -23,7 +23,7 @@
'
- name: s1_right_arm_pre_cam_take_photo
params: 'body_id: 2
params: &pre_cam_take_photo 'body_id: 2
data_type: 2
@@ -157,7 +157,7 @@
'
- name: s7_right_arm_grasp_box
params: 'body_id: 2
params: &grasp_box_s7_s9 'body_id: 2
data_type: 2
@@ -185,70 +185,26 @@
'
- name: s8_gripper_open
params: 'loc: 0
speed: 150
torque: 20
mode: 2
'
params: *gripper_open
- name: s9_right_arm_grasp_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
'
params: *grasp_box_s7_s9
- name: s10_move_waist_turn_right_90
params: 'move_pitch_degree: 0.0
move_yaw_degree: -90.0
'
params: *waist_turn_right_90
- name: s11_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: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
'
params: *pre_cam_take_photo
- name: s12_gripper_open
params: 'loc: 0
speed: 150
torque: 20
mode: 2
'
params: *gripper_open
- name: s13_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: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
'
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: s14_gripper_open
params: *gripper_open
- name: s15_gripper_open
params: *gripper_open
- name: s16_gripper_open
params: *gripper_open

44
src/brain/config/bt_vision_grasp.xml
View File

@@ -7,24 +7,36 @@
<!-- Vision with recovery -->
<Fallback name="vision_with_recovery">
<SubTree ID="vision_subtree"/>
<SubTree ID="vision_recovery_subtree"/>
<Sequence>
<SubTree ID="vision_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- Grasp with recovery -->
<Fallback name="grasp_with_recovery">
<SubTree ID="grasp_subtree"/>
<SubTree ID="grasp_recovery_subtree"/>
<Sequence>
<SubTree ID="grasp_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
<!-- Putdown with recovery -->
<Fallback name="putdown_with_recovery">
<SubTree ID="putdown_subtree"/>
<SubTree ID="putdown_recovery_subtree"/>
<Sequence>
<SubTree ID="putdown_recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</Sequence>
<!-- Overall recovery if the entire sequence fails -->
<SubTree ID="recovery_subtree"/>
<Sequence>
<SubTree ID="recovery_subtree"/>
<AlwaysFailure/>
</Sequence>
</Fallback>
</RetryUntilSuccessful>
</Sequence>
@@ -91,10 +103,11 @@
<BehaviorTree ID="recovery_subtree">
<Sequence name="recovery_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<RetryUntilSuccessful name="retry_recovery" num_attempts="1">
<Sequence>
<GripperCmd_H name="s12_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_overall" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
@@ -102,10 +115,11 @@
<BehaviorTree ID="vision_recovery_subtree">
<Sequence name="vision_recovery_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
<Sequence>
<!-- <GripperCmd_H name="s12_gripper_open" /> -->
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
<!-- <GripperCmd_H name="s14_gripper_open" /> -->
<Arm_H name="s14_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_vision" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
@@ -113,10 +127,11 @@
<BehaviorTree ID="grasp_recovery_subtree">
<Sequence name="grasp_recovery_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
<Sequence>
<GripperCmd_H name="s12_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
<GripperCmd_H name="s15_gripper_open" />
<Arm_H name="s15_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_grasp" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>
@@ -124,10 +139,11 @@
<BehaviorTree ID="putdown_recovery_subtree">
<Sequence name="putdown_recovery_root">
<RetryUntilSuccessful name="retry_putdown" num_attempts="1">
<RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
<Sequence>
<GripperCmd_H name="s12_gripper_open" />
<Arm_H name="s13_right_arm_pre_cam_take_photo" />
<GripperCmd_H name="s16_gripper_open" />
<Arm_H name="s16_right_arm_pre_cam_take_photo" />
<ClearDoneInstances_H name="clear_done_instances_putdown" />
</Sequence>
</RetryUntilSuccessful>
</Sequence>

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

@@ -31,6 +31,8 @@ void CerebellumHooks::ConfigureArmSpaceControlHooks(CerebellumNode * node)
(void)node;
}
#define USE_ARM_ANGLE_DIRECT_MOVE
void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
{
// Arm
@@ -113,11 +115,12 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
return goal;
}
// goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
// goal.data_length = POSE_DIMENSION;
// goal.data_array = {pose.position.x, pose.position.y, pose.position.z,
// pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
#ifdef USE_ARM_POSE_DIRECT_MOVE
goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
goal.data_length = POSE_DIMENSION;
goal.data_array = {pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
#elif defined(USE_ARM_ANGLE_DIRECT_MOVE)
if (angle.size() == USED_ARM_DOF) {
goal.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE;
goal.data_length = USED_ARM_DOF;
@@ -125,6 +128,7 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid angle size: %ld/%d", angle.size(), USED_ARM_DOF);
}
#endif
}
else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: data_type[%d] not support", goal.data_type);
@@ -649,10 +653,12 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
return false;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "right_hand take_photo angles: ");
for (const auto & angles: joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
}
if (joint_angles.size() >= 6) {
RCLCPP_INFO(node->get_logger(), "right_hand take_photo joint angles: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5]);
} else {
RCLCPP_ERROR(node->get_logger(), "right_hand take_photo joint angles size invalid: %ld", joint_angles.size());
}
#endif
node->right_hand_take_photo_poses_.push_back(pose);
node->right_hand_take_photo_angles_.push_back(joint_angles);
@@ -764,10 +770,18 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
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();
node->pre_grasp_poses_.push_back(pre_grasp_msg);
node->grasp_poses_.push_back(grasp_msg);
node->right_hand_take_object_pose_.push_back(grasp_msg);
node->right_hand_take_object_pose_.push_back(take_object_msg);
node->pre_grasp_angles_.push_back(pre_grasp_joint_angles);
node->grasp_angles_.push_back(grasp_joint_angles);

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

@@ -135,13 +135,11 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
void CerebellumNode::DeclareAndLoadParameters()
{
// Core behavior parameters
this->declare_parameter<std::string>("robot_config_path", "");
this->declare_parameter<std::string>("robot_skill_file_path", "");
this->declare_parameter<std::string>("robot_config_path", robot_config_path_);
this->declare_parameter<std::string>("robot_skill_file_path", robot_skill_file_path_);
this->declare_parameter<bool>("abort_on_failure", abort_on_failure_);
this->declare_parameter<double>("default_action_timeout_sec", default_action_timeout_sec_);
this->declare_parameter<double>(
"vision_grasp_object_timeout_sec",
vision_grasp_object_timeout_sec_);
this->declare_parameter<double>("vision_grasp_object_timeout_sec", vision_grasp_object_timeout_sec_);
this->declare_parameter<std::string>("skill_timeouts", ""); // format: SkillA=30,SkillB=10
this->declare_parameter<bool>("enable_stats_publisher", enable_stats_publisher_);
this->declare_parameter<int>("stats_publish_interval_ms", stats_publish_interval_ms_);
@@ -230,9 +228,13 @@ void CerebellumNode::DeclareAndLoadParameters()
*/
void CerebellumNode::LoadSkillsFile()
{
if (robot_skill_file_path_.empty()) {return;}
if (robot_skill_file_path_.empty()) {
RCLCPP_ERROR(
this->get_logger(), "[cerebellum_node] No skill file path provided, skipping skill load");
return;
}
if (!skill_manager_->load_from_file(robot_skill_file_path_)) {
RCLCPP_WARN(
RCLCPP_ERROR(
this->get_logger(), "[cerebellum_node] Failed to load skills from %s", robot_skill_file_path_.c_str());
return;
}

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

@@ -519,6 +519,21 @@ void CerebrumNode::RegisterSkillBtActions()
RCLCPP_WARN(this->get_logger(), "BT type exists %s: %s", bt_type.c_str(), e.what());
}
}
// Register special utility nodes
BTActionHandlers clear_handlers;
clear_handlers.on_start = [this](rclcpp::Node *, BT::TreeNode &) {
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
per_node_exec_.clear();
RCLCPP_INFO(this->get_logger(), "Cleared done_instances_epoch_ and per_node_exec_ for retry");
return BT::NodeStatus::SUCCESS;
};
try {
RegisterBtAction("ClearDoneInstances_H", clear_handlers);
} catch (const BT::BehaviorTreeException & e) {
RCLCPP_WARN(this->get_logger(), "BT type exists ClearDoneInstances_H: %s", e.what());
}
}
/**
@@ -1294,11 +1309,8 @@ bool CerebrumNode::LoadRobotConfiguration()
if (path != std::nullopt) {
for (const auto & p : *path) {
if (!p.config.empty() && !p.param.empty()) {
// If paths are relative, resolve them relative to the share directory
std::string config_path = p.config[0] == '/' ? p.config : share_directory_ + p.config;
std::string param_path = p.param[0] == '/' ? p.param : share_directory_ + p.param;
bt_config_params_paths_.push_back({config_path, param_path});
RCLCPP_WARN(this->get_logger(), "bt config param %s %s", config_path.c_str(), param_path.c_str());
bt_config_params_paths_.push_back({share_directory_ + p.config, share_directory_ + p.param});
RCLCPP_WARN(this->get_logger(), "bt config param %s %s", p.config.c_str(), p.param.c_str());
}
}
}