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

only retry failed action

Browse Source
This commit is contained in:
david
2025-11-01 12:02:57 +08:00
parent 4da85d16dc
commit 74c6ee0046
4 changed files with 25 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,7 +2,7 @@
<BehaviorTree ID="MainTree">
<Sequence name="root">
<!-- Retry waist + arm + hand until HandControl_H succeeds -->
<RetryUntilSuccessful name="retry_all_action" num_attempts="5">
<RetryUntilSuccessful name="retry_all_action" num_attempts="3">
<Sequence>
<!-- <MoveHome_H name="s0_motion_move_home" /> -->
<!-- <Arm_H name="s1_arm_move_origin_position" /> -->
@@ -11,7 +11,7 @@
<Arm_H name="s4_arm_grasp_box" />
<Arm_H name="s5_arm_pickup_box" />
<MoveWheel_H name="s6_wheel_move_back_to_second_position" />
<!-- <MoveWaist_H name="s7_waist_turn_around" /> -->
<MoveWaist_H name="s7_waist_turn_around" />
<Parallel name="move_waist">
<MoveWaist_H name="s8_waist_bend_down" />
<Arm_H name="s9_arm_put_down_box" />

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

@@ -2,7 +2,7 @@
<BehaviorTree ID="MainTree">
<Sequence name="root">
<!-- Retry waist + arm + hand until HandControl_H succeeds -->
<RetryUntilSuccessful name="retry_all_action" num_attempts="5">
<RetryUntilSuccessful name="retry_all_action" num_attempts="3">
<Sequence>
<Arm_H name="s1_arm_move_origin_position" />
<MoveWheel_H name="s2_wheel_move_to_origin_pickup_position" />

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

@@ -248,6 +248,7 @@ private:
double last_progress {0.0};
};
std::unordered_map<std::string, PerInstanceExecInfo> per_instance_exec_;
std::unordered_set<std::string> done_instances_epoch_;
// Set of already registered action client names (prevents duplicate registration & duplicate feedback).
std::unordered_set<std::string> registered_actions_;
// Per-BT-node execution timeout (seconds).

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

@@ -359,6 +359,10 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
}
epoch_filter_.reset_epoch(new_epoch, seq_skills);
per_node_exec_.clear();
{
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
}
bt_pending_run_ = true;
RCLCPP_INFO(this->get_logger(), "BT built from file epoch=%llu", static_cast<unsigned long long>(new_epoch));
}
@@ -397,6 +401,10 @@ void CerebrumNode::UpdateBehaviorTree()
}
epoch_filter_.reset_epoch(new_epoch, seq_skills);
per_node_exec_.clear();
{
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
}
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());
@@ -426,6 +434,13 @@ void CerebrumNode::RegisterSkillBtActions()
handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode & node) {
const std::string instance_name = node.name();
const std::string key = bt_type + std::string("|") + instance_name;
{
std::lock_guard<std::mutex> lk(exec_mutex_);
if (done_instances_epoch_.find(key) != done_instances_epoch_.end()) {
RCLCPP_INFO(this->get_logger(), "[%s/%s] skipped: already SUCCESS in current epoch", bt_type.c_str(), instance_name.c_str());
return BT::NodeStatus::SUCCESS;
}
}
auto & inst = per_instance_exec_[key];
if (inst.in_flight.load(std::memory_order_acquire) && !inst.finished.load(std::memory_order_acquire)) {
return BT::NodeStatus::RUNNING;
@@ -488,7 +503,12 @@ void CerebrumNode::RegisterSkillBtActions()
return BT::NodeStatus::RUNNING;
}
g_bt_resume_time_steady = std::chrono::steady_clock::now() + kInterActionDelayMs;
return inst.success ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
if (inst.success) {
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.insert(key);
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
};
handlers.on_halted = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_halted", bt_type.c_str());