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

optimize code

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-19 17:55:49 +08:00
parent 02f7c103c7
commit 3fc1de3a1a
6 changed files with 76 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -84,7 +84,7 @@ cerebellum_node:
top_cam_left_arm_orientation: [0.7071, 0.0, 0.0, 0.7071] #左臂相机视觉识别时的四元数
side_cam_right_arm_y_offset: 0.0 # 补偿右臂y轴方向的抓取偏差（前后）
side_cam_right_arm_z_offset: 0.02 # 补偿右臂z轴方向的抓取偏差（左右）
side_cam_left_arm_y_offset: -0.02 # 补偿左臂y轴方向的抓取偏差（前后）
side_cam_left_arm_y_offset: 0.02 # 补偿左臂y轴方向的抓取偏差（前后）
side_cam_left_arm_z_offset: 0.01 # 补偿左臂z轴方向的抓取偏差（左右）
take_object_arm_x: -0.05 # 抓取物体时x坐标
left_camera_frame: "LeftLink6" # 左臂相机坐标系

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

@@ -338,10 +338,6 @@
params: *pre_grasp_right
- name: s15_left_arm_pre_grasp
params: *pre_grasp_left
- name: s15_right_arm_initial
params: *arm_inittial_right
- name: s15_left_arm_initial
params: *arm_inittial_left
- name: s16_right_arm_initial
params: *arm_inittial_right
- name: s16_left_arm_initial
@@ -395,4 +391,16 @@
- name: s15_left_arm_cam_take_photo
params: *left_arm_cam_take_photo
- name: s15_right_arm_cam_take_photo
params: *right_arm_cam_take_photo
params: *right_arm_cam_take_photo
- name: s15_right_arm_initial_vision
params: *arm_inittial_right
- name: s15_left_arm_initial_vision
params: *arm_inittial_left
- name: s15_right_arm_initial_grasp
params: *arm_inittial_right
- name: s15_left_arm_initial_grasp
params: *arm_inittial_left
- name: s15_right_arm_initial_cam
params: *arm_inittial_right
- name: s15_left_arm_initial_cam
params: *arm_inittial_left

32
src/brain/config/bt_vision_grasp_dual_arm.xml
View File

@@ -67,11 +67,15 @@
<!-- Right Arm Specific Subtrees -->
<BehaviorTree ID="right_arm_hand_vision_subtree">
<Sequence name="right_hand_vision_root">
<Fallback name="right_hand_vision_fallback">
<RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
<Sequence>
<Arm_H name="s15_right_arm_initial_vision" />
<AlwaysFailure />
</Sequence>
</Fallback>
</BehaviorTree>
<BehaviorTree ID="right_arm_grasp_subtree">
@@ -104,11 +108,15 @@
<!-- Left Arm Specific Subtrees -->
<BehaviorTree ID="left_arm_hand_vision_subtree">
<Sequence name="left_hand_vision_root">
<Fallback name="left_hand_vision_fallback">
<RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
<VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
</RetryUntilSuccessful>
</Sequence>
<Sequence>
<Arm_H name="s15_left_arm_initial_vision" />
<AlwaysFailure />
</Sequence>
</Fallback>
</BehaviorTree>
<BehaviorTree ID="left_arm_grasp_subtree">
@@ -167,8 +175,8 @@
<GripperCmd1_H name="s15_right_hand_gripper_open" />
<Arm_H name="s15_right_arm_pre_grasp" />
<Arm_H name="s15_right_arm_cam_take_photo" />
<Arm_H name="s15_right_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_right_grasp" />
<Arm_H name="s15_right_arm_initial_grasp" />
<ClearDoneInstances_H name="clear_done_instances_right_grasp" match="right" />
</Sequence>
</BehaviorTree>
@@ -177,8 +185,8 @@
<GripperCmd0_H name="s15_left_hand_gripper_open" />
<Arm_H name="s15_left_arm_pre_grasp" />
<Arm_H name="s15_left_arm_cam_take_photo" />
<Arm_H name="s15_left_arm_initial" />
<ClearDoneInstances_H name="clear_done_instances_left_grasp" />
<Arm_H name="s15_left_arm_initial_grasp" />
<ClearDoneInstances_H name="clear_done_instances_left_grasp" match="left" />
</Sequence>
</BehaviorTree>
@@ -197,7 +205,7 @@
<Parallel name="left_arm_initial">
<Arm_H name="s17_left_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_left_arm_vision" />
<ClearDoneInstances_H name="clear_done_instances_left_arm_vision" match="left" />
</Sequence>
</BehaviorTree>
@@ -206,7 +214,7 @@
<Parallel name="right_arm_initial">
<Arm_H name="s17_right_arm_initial" />
</Parallel>
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" />
<ClearDoneInstances_H name="clear_done_instances_right_arm_vision" match="right" />
</Sequence>
</BehaviorTree>
@@ -222,7 +230,7 @@
<Arm_H name="s2_right_arm_cam_take_photo" />
<Script code="right_cam_status:=true" />
</Sequence>
<Arm_H name="s15_right_arm_initial" />
<Arm_H name="s15_right_arm_initial_cam" />
<AlwaysSuccess/>
</Fallback>
<!-- 左臂 -->
@@ -231,7 +239,7 @@
<Arm_H name="s2_left_arm_cam_take_photo" />
<Script code="left_cam_status:=true" />
</Sequence>
<Arm_H name="s15_left_arm_initial" />
<Arm_H name="s15_left_arm_initial_cam" />
<AlwaysSuccess/>
</Fallback>
</Parallel>

7
src/brain/include/brain/bt_registry.hpp
View File

@@ -86,9 +86,12 @@ public:
node) {}
/**
* @brief Declare static ports. Currently none (empty list) simplifying dynamic XML generation.
* @brief Declare static ports. Added "match" for selective clearing logic in ClearDoneInstances_H.
*/
static BT::PortsList providedPorts() {return {};}
static BT::PortsList providedPorts()
{
return {BT::InputPort<std::string>("match")};
}
/**
* @brief First tick transition handler.

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

@@ -203,6 +203,7 @@ private:
rclcpp::Service<interfaces::srv::BtRebuild>::SharedPtr rebuild_service_;
std::atomic<bool> bt_updating_{false};
std::atomic<bool> bt_pending_run_{false};
rclcpp::Time bt_execution_start_time_;
// Encapsulated epoch+skill filter
EpochSkillFilter epoch_filter_;
// Current sequence skills (kept in original order for logging / statistics).

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

@@ -326,7 +326,9 @@ void CerebrumNode::ExecuteBehaviorTree()
robot_work_info_.bt_node_status = BT::toStr(status, false);
robot_work_time_.total_seconds += (rclcpp::Clock().now() - robot_work_time_.start).seconds();
RCLCPP_INFO(this->get_logger(), "BT finished status=%s, scheduled=%s", BT::toStr(status, true).c_str(), scheduled.c_str());
auto duration = (this->now() - bt_execution_start_time_).seconds();
RCLCPP_INFO(this->get_logger(), "BT finished status=%s, scheduled=%s, duration=%.2f s",
BT::toStr(status, true).c_str(), scheduled.c_str(), duration);
}
}
@@ -363,6 +365,7 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
}
bt_execution_start_time_ = this->now();
bt_pending_run_ = true;
RCLCPP_INFO(this->get_logger(), "BT built from file epoch=%llu", static_cast<unsigned long long>(new_epoch));
}
@@ -405,6 +408,7 @@ void CerebrumNode::UpdateBehaviorTree()
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
}
bt_execution_start_time_ = this->now();
bt_pending_run_ = true;
RCLCPP_INFO(this->get_logger(), "BT updated epoch=%llu sequence=%s",
static_cast<unsigned long long>(new_epoch), active_sequence_.c_str());
@@ -522,11 +526,38 @@ void CerebrumNode::RegisterSkillBtActions()
// Register special utility nodes
BTActionHandlers clear_handlers;
clear_handlers.on_start = [this](rclcpp::Node *, BT::TreeNode &) {
clear_handlers.on_start = [this](rclcpp::Node *, BT::TreeNode & node) {
std::string match_pattern;
bool has_match = false;
auto res = node.getInput("match", match_pattern);
if (res && !match_pattern.empty()) {
has_match = true;
}
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");
if (!has_match) {
done_instances_epoch_.clear();
per_node_exec_.clear();
RCLCPP_INFO(this->get_logger(), "Cleared done_instances_epoch_ and per_node_exec_ for retry");
} else {
std::size_t removed_count = 0;
for (auto it = done_instances_epoch_.begin(); it != done_instances_epoch_.end(); ) {
if (it->find(match_pattern) != std::string::npos) {
it = done_instances_epoch_.erase(it);
removed_count++;
} else {
++it;
}
}
for (auto it = per_node_exec_.begin(); it != per_node_exec_.end(); ) {
if (it->first.find(match_pattern) != std::string::npos) {
it = per_node_exec_.erase(it);
} else {
++it;
}
}
RCLCPP_INFO(this->get_logger(), "Cleared %zu instances matching '%s'", removed_count, match_pattern.c_str());
}
return BT::NodeStatus::SUCCESS;
};
try {