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

优化robot work info处理

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-29 18:45:19 +08:00
parent 9047e71206
commit ac4a1fc8f2
2 changed files with 268 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -187,7 +187,10 @@ private:
/** Declare per-skill instance parameter entries used to customize outgoing calls. */
bool UpdateBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name, const std::string & instance_params);
/** Declare per-instance parameter entries used to customize a specific BT node occurrence. */
bool DeclareBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name);
bool DeclareBtActionParamsForSkillInstance(
const std::string & skill_name,
const std::string & instance_name,
std::string * out_instance_params = nullptr);
/** Build a SkillCall message for given skill by consulting SkillSpec and per-skill params. */
interfaces::msg::SkillCall BuildSkillCallForSkill(const std::string & skill_name) const;
/** Build a SkillCall with optional per-instance override (cerebrum.bt.<Skill>.<Instance>.*). */
@@ -227,6 +230,15 @@ private:
// Mutex that protects both per_node_exec_ and epoch_skills_ updates.
std::mutex exec_mutex_;
// Tracking currently running BT leaf instances (for serial/parallel reporting).
std::unordered_set<std::string> running_leaf_keys_;
// Index map for active RobotWorkInfo.action entries (key -> vector index).
std::unordered_map<std::string, std::size_t> action_index_by_key_;
// Parallel vector of action keys aligned with robot_work_info_.action.
std::vector<std::string> action_keys_;
// Map from BT node instance name to execution mode based on BT structure.
std::unordered_map<std::string, std::string> action_exec_by_node_name_;
rclcpp::Publisher<interfaces::msg::RobotWorkInfo>::SharedPtr robot_work_info_pub_;
interfaces::msg::RobotWorkInfo robot_work_info_;
@@ -419,6 +431,22 @@ private:
/** Extract filename without extension from a path */
std::string ExtractFilenameWithoutExtension(const std::string & path);
// Update robot_work_info_ leaf execution mode based on running_leaf_keys_.
void UpdateLeafParallelismStatusLocked();
// Manage RobotWorkInfo.action entries (guarded by exec_mutex_).
void AddActionInfoLocked(
const std::string & key,
const std::string & skill,
const std::string & action_name,
const std::string & params);
void UpdateActionInfoStateLocked(const std::string & key, const std::string & state);
void RemoveActionInfoLocked(const std::string & key);
void ClearActionInfoLocked();
// Build map of BT node instance name -> execution mode (serial/parallel).
void BuildActionExecutionMapLocked();
// Temporary override used so the generic ExecuteBtAction goal builder can
// send a single-skill goal when a per-skill BT node starts.
std::optional<std::string> single_skill_goal_override_;

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

@@ -10,6 +10,7 @@
#include <algorithm>
#include <chrono>
#include <fstream>
#include <functional>
#include <iostream>
#include <random>
#include <sstream>
@@ -19,6 +20,9 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behaviortree_cpp/action_node.h>
#include <behaviortree_cpp/control_node.h>
#include <behaviortree_cpp/decorator_node.h>
#include <behaviortree_cpp/controls/parallel_node.h>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
@@ -79,6 +83,156 @@ static constexpr std::chrono::milliseconds kInterActionDelayMs{200};
} // namespace
void CerebrumNode::UpdateLeafParallelismStatusLocked()
{
for (std::size_t i = 0; i < robot_work_info_.action.size(); ++i) {
const auto & key = action_keys_[i];
auto it = action_exec_by_node_name_.find(key);
robot_work_info_.action[i].execution = (it == action_exec_by_node_name_.end()) ? "serial" : it->second;
}
}
void CerebrumNode::BuildActionExecutionMapLocked()
{
action_exec_by_node_name_.clear();
auto * root = tree_.rootNode();
if (!root) {
return;
}
auto is_skill_action_leaf = [](BT::TreeNode * node) -> bool {
if (!node) {
return false;
}
const auto & reg = node->registrationName();
if (reg.size() >= 2 && reg.rfind("_H") == reg.size() - 2) {
return reg != "ClearDoneInstances_H";
}
return false;
};
std::function<bool(BT::TreeNode *, bool, bool *)> visit =
[&](BT::TreeNode * node, bool parallel_context, bool * first_in_parallel) -> bool {
if (!node) {
return false;
}
if (auto * parallel = dynamic_cast<BT::ParallelNode *>(node)) {
bool first_leaf = true;
bool any_leaf = false;
const auto & children = parallel->children();
for (auto * child : children) {
if (visit(child, true, &first_leaf)) {
any_leaf = true;
}
}
return any_leaf;
}
if (auto * ctrl = dynamic_cast<BT::ControlNode *>(node)) {
bool any_leaf = false;
const auto & children = ctrl->children();
for (auto * child : children) {
if (visit(child, parallel_context, first_in_parallel)) {
any_leaf = true;
}
}
return any_leaf;
}
if (auto * deco = dynamic_cast<BT::DecoratorNode *>(node)) {
return visit(deco->child(), parallel_context, first_in_parallel);
}
if (!is_skill_action_leaf(node)) {
return false;
}
// Skill action leaf: record execution mode by instance name
std::string mode = "serial";
if (parallel_context) {
if (first_in_parallel && *first_in_parallel) {
mode = "serial";
*first_in_parallel = false;
} else {
mode = "parallel";
}
}
const std::string key = node->registrationName() + "|" + node->name();
action_exec_by_node_name_[key] = mode;
return true;
};
visit(root, false, nullptr);
}
void CerebrumNode::AddActionInfoLocked(
const std::string & key,
const std::string & skill,
const std::string & action_name,
const std::string & params)
{
if (action_index_by_key_.count(key)) {
const auto idx = action_index_by_key_[key];
if (idx < robot_work_info_.action.size()) {
robot_work_info_.action[idx].state = "RUNNING";
robot_work_info_.action[idx].stamp = this->now();
robot_work_info_.action[idx].skill = skill;
robot_work_info_.action[idx].action_name = action_name;
robot_work_info_.action[idx].params = params;
auto it = action_exec_by_node_name_.find(key);
robot_work_info_.action[idx].execution = (it == action_exec_by_node_name_.end()) ? "serial" : it->second;
}
return;
}
interfaces::msg::ActionInfo info;
info.stamp = this->now();
info.skill = skill;
info.action_name = action_name;
info.params = params;
auto it = action_exec_by_node_name_.find(key);
info.execution = (it == action_exec_by_node_name_.end()) ? "serial" : it->second;
info.state = "RUNNING";
robot_work_info_.action.push_back(info);
action_keys_.push_back(key);
action_index_by_key_[key] = robot_work_info_.action.size() - 1;
}
void CerebrumNode::UpdateActionInfoStateLocked(const std::string & key, const std::string & state)
{
auto it = action_index_by_key_.find(key);
if (it == action_index_by_key_.end()) {
return;
}
const auto idx = it->second;
if (idx < robot_work_info_.action.size() && action_keys_[idx] == key) {
robot_work_info_.action[idx].state = state;
}
}
void CerebrumNode::RemoveActionInfoLocked(const std::string & key)
{
auto it = action_index_by_key_.find(key);
if (it == action_index_by_key_.end()) {
return;
}
const auto idx = it->second;
const auto last_idx = robot_work_info_.action.empty() ? 0 : (robot_work_info_.action.size() - 1);
if (idx != last_idx) {
robot_work_info_.action[idx] = robot_work_info_.action[last_idx];
action_keys_[idx] = action_keys_[last_idx];
action_index_by_key_[action_keys_[idx]] = idx;
}
robot_work_info_.action.pop_back();
action_keys_.pop_back();
action_index_by_key_.erase(it);
}
void CerebrumNode::ClearActionInfoLocked()
{
robot_work_info_.action.clear();
action_keys_.clear();
action_index_by_key_.clear();
}
/**
* @brief Construct the CerebrumNode.
*
@@ -358,6 +512,18 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
}
tree_ = std::move(new_tree);
{
std::ifstream ifs(xml_file);
std::string xml_content;
if (ifs) {
std::ostringstream oss;
oss << ifs.rdbuf();
xml_content = oss.str();
}
std::lock_guard<std::mutex> lk(exec_mutex_);
robot_work_info_.bt_xml = xml_content;
BuildActionExecutionMapLocked();
}
uint64_t new_epoch = epoch_filter_.epoch() + 1;
std::vector<std::string> seq_skills;
{
@@ -369,6 +535,9 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
{
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
running_leaf_keys_.clear();
ClearActionInfoLocked();
UpdateLeafParallelismStatusLocked();
}
bt_execution_start_time_ = this->now();
bt_pending_run_ = true;
@@ -407,11 +576,21 @@ void CerebrumNode::UpdateBehaviorTree()
std::lock_guard<std::mutex> lk(exec_mutex_);
seq_skills = current_sequence_skills_;
}
{
const auto seq = MakeSequenceFromSkills(seq_skills);
const auto xml_content = BuildXmlFromSequence(seq);
std::lock_guard<std::mutex> lk(exec_mutex_);
robot_work_info_.bt_xml = xml_content;
BuildActionExecutionMapLocked();
}
epoch_filter_.reset_epoch(new_epoch, seq_skills);
per_node_exec_.clear();
{
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
running_leaf_keys_.clear();
ClearActionInfoLocked();
UpdateLeafParallelismStatusLocked();
}
bt_execution_start_time_ = this->now();
bt_pending_run_ = true;
@@ -462,7 +641,8 @@ void CerebrumNode::RegisterSkillBtActions()
inst.result_code = ExecResultCode::UNKNOWN;
inst.start_time = this->now();
if (!DeclareBtActionParamsForSkillInstance(skill.name, instance_name) ||
std::string instance_params;
if (!DeclareBtActionParamsForSkillInstance(skill.name, instance_name, &instance_params) ||
!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
RCLCPP_ERROR(this->get_logger(), "ExecuteBtAction server unavailable");
inst.in_flight = false;
@@ -478,6 +658,13 @@ void CerebrumNode::RegisterSkillBtActions()
goal.calls.clear();
goal.calls.push_back(BuildSkillCallForSkill(skill.name, instance_name));
{
std::lock_guard<std::mutex> lk(exec_mutex_);
running_leaf_keys_.insert(key);
AddActionInfoLocked(key, skill.name, instance_name, instance_params);
UpdateLeafParallelismStatusLocked();
}
auto logger = this->get_logger();
std::thread([this, key, goal, logger]() {
using EBA = interfaces::action::ExecuteBtAction;
@@ -513,6 +700,12 @@ void CerebrumNode::RegisterSkillBtActions()
if (!inst.finished.load(std::memory_order_acquire)) {
return BT::NodeStatus::RUNNING;
}
{
std::lock_guard<std::mutex> lk(exec_mutex_);
running_leaf_keys_.erase(key);
UpdateActionInfoStateLocked(key, inst.success ? "SUCCESS" : "FAILURE");
UpdateLeafParallelismStatusLocked();
}
g_bt_resume_time_steady = std::chrono::steady_clock::now() + kInterActionDelayMs;
if (inst.success) {
std::lock_guard<std::mutex> lk(exec_mutex_);
@@ -521,7 +714,15 @@ void CerebrumNode::RegisterSkillBtActions()
}
return BT::NodeStatus::FAILURE;
};
handlers.on_halted = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
handlers.on_halted = [this, bt_type](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_);
running_leaf_keys_.erase(key);
UpdateActionInfoStateLocked(key, "HALTED");
UpdateLeafParallelismStatusLocked();
}
RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_halted", bt_type.c_str());
};
try {RegisterBtAction(bt_type, handlers);} catch (const BT::BehaviorTreeException & e) {
@@ -616,8 +817,9 @@ bool CerebrumNode::UpdateBtActionParamsForSkillInstance(
* @param instance_name
*/
bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
const std::string & skill_name,
const std::string & instance_name)
const std::string & skill_name,
const std::string & instance_name,
std::string * out_instance_params)
{
if (skill_name.empty() || instance_name.empty()) {
RCLCPP_ERROR(this->get_logger(), "Declare BtAction Params ForSkillInstance called with empty skill_name or instance_name");
@@ -666,13 +868,7 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
}
if (found_override) {
//update working info
robot_work_info_.skill = skill_name;
robot_work_info_.action_name = instance_name;
robot_work_info_.instance_params = instance_params;
robot_work_info_.stamp = this->now();
return UpdateBtActionParamsForSkillInstance(skill_name, instance_name, instance_params);
// Use override
} else if (current_path_copy.config == skill_name) {
instance_params = current_path_copy.param;
RCLCPP_INFO(this->get_logger(), "Remote params: %s", instance_params.c_str());
@@ -696,14 +892,9 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
}
}
//update working info
robot_work_info_.skill = skill_name;
robot_work_info_.action_name = instance_name;
robot_work_info_.instance_params = instance_params;
//builtin_interfaces/Time stamp # 时间戳
// # * stamp.secs: seconds (stamp_secs) since epoch
// # * stamp.nsecs: nanoseconds since stamp_secs
robot_work_info_.stamp = this->now();
if (out_instance_params) {
*out_instance_params = instance_params;
}
return UpdateBtActionParamsForSkillInstance(skill_name, instance_name, instance_params);
}
@@ -1162,6 +1353,9 @@ void CerebrumNode::StopBehaviorTree(bool cancel_goals)
{
std::lock_guard<std::mutex> lk(exec_mutex_);
done_instances_epoch_.clear();
running_leaf_keys_.clear();
ClearActionInfoLocked();
UpdateLeafParallelismStatusLocked();
}
// 6) Update work info
@@ -1578,7 +1772,11 @@ void CerebrumNode::RobotWorkInfoPublish()
robot_work_info_.task.push_back("None"); //next task
robot_work_info_.task.push_back("None"); //current task
robot_work_info_.bt_node_status = "IDLE";
robot_work_info_.stamp = this->now();
robot_work_info_.bt_xml = "";
{
std::lock_guard<std::mutex> lk(exec_mutex_);
ClearActionInfoLocked();
}
robot_work_info_pub_ = this->create_publisher<
interfaces::msg::RobotWorkInfo>("/robot_work_info", 10);
@@ -1589,10 +1787,28 @@ void CerebrumNode::RobotWorkInfoPublish()
}
pub_robot_work_info_timer_ = this->create_wall_timer(100ms, [this]() {
static int64_t robot_work_info_pub_msg_id = 0;
interfaces::msg::RobotWorkInfo msg { robot_work_info_ };
msg.msg_id = robot_work_info_pub_msg_id++;
interfaces::msg::RobotWorkInfo msg;
{
std::lock_guard<std::mutex> lk(exec_mutex_);
msg = robot_work_info_;
if (!robot_work_info_.bt_xml.empty()) {
robot_work_info_.bt_xml.clear();
}
// Prune terminal states from the persistent robot_work_info_ after copying to msg
std::vector<std::string> to_remove;
for (size_t i = 0; i < robot_work_info_.action.size(); ++i) {
const auto & state = robot_work_info_.action[i].state;
if (state == "SUCCESS" || state == "FAILURE" || state == "HALTED") {
to_remove.push_back(action_keys_[i]);
}
}
for (const auto & k : to_remove) {
RemoveActionInfoLocked(k);
}
}
msg.msg_id = robot_work_info_.msg_id++;
msg.battery_capacity = GetBatteryPercentage();
msg.working_time = GetWorkingTime();
msg.nav_state = "Normal";