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

feat(brain): enhance BT sequence rebuild and parameter handling

Browse Source 
- Update BT registry to ensure unique node names with timestamps and indices.
- Implement per-instance parameter overrides for sequence executions.
- Add helpers for splitting quoted strings and handling generic rebuild requests.
- update HandleGenericRebuild to support both Remote and Sequence types with parameter matching.
This commit is contained in:
NuoDaJia02
2026-01-22 17:31:06 +08:00
parent 3fc1de3a1a
commit c4f07c73fb
3 changed files with 207 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -277,8 +277,11 @@ private:
std::ostringstream oss;
oss <<
"\n <root BTCPP_format=\"4\" >\n\n <BehaviorTree ID=\"MainTree\">\n <Sequence name=\"root_sequence\">\n";
for (const auto & act : seq) {
oss << " <" << act.type << " name=\"" << act.name << "\"";
auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
for (size_t i = 0; i < seq.size(); ++i) {
const auto & act = seq[i];
// Append timestamp and sequence index to ensure global uniqueness of the instance name in the tree
oss << " <" << act.type << " name=\"" << act.name << "_" << timestamp << "_" << i << "\"";
for (const auto & kv : act.ports) {
oss << " " << kv.first << "=\"" << kv.second << "\"";
}

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

@@ -118,6 +118,20 @@ public:
*/
static std::vector<std::string> ParseListString(const std::string & raw);
/**
* @brief Split a string by comma, ignoring commas inside quotes (single or double).
* @param raw Input string.
* @return Vector of tokens.
*/
static std::vector<std::string> SplitQuotedString(const std::string & raw);
/**
* @brief Split a string by comma, preserving order and duplicates.
* @param raw Input string.
* @return Vector of tokens.
*/
static std::vector<std::string> SplitString(const std::string & raw);
/**
* @brief Classify textual result detail (prefix or code= token) into ExecResultCode.
* @param detail Input detail string.
@@ -208,6 +222,8 @@ private:
EpochSkillFilter epoch_filter_;
// Current sequence skills (kept in original order for logging / statistics).
std::vector<std::string> current_sequence_skills_;
// Map of sequence index to override params for the current sequence.
std::unordered_map<size_t, std::string> active_sequence_param_overrides_;
// Mutex that protects both per_node_exec_ and epoch_skills_ updates.
std::mutex exec_mutex_;
@@ -395,8 +411,8 @@ private:
const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp);
/** Handle Remote type rebuild request */
void HandleRemoteRebuild(
/** Handle Generic (Remote/Sequence) type rebuild request */
void HandleGenericRebuild(
const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp);

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

@@ -273,7 +273,12 @@ void CerebrumNode::CerebrumTask()
continue;
}
BuildBehaviorTreeFromFile(path_param.config);
current_bt_config_params_path_ = path_param;
{
std::lock_guard<std::mutex> lk(exec_mutex_);
current_bt_config_params_path_ = path_param;
// Clear overridden params when switching file-based BT
active_sequence_param_overrides_.clear();
}
//update working info
auto scheduled = path_param.config;
@@ -619,7 +624,7 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
return false;
}
if (current_bt_config_params_path_.param.empty()) {
if (current_bt_config_params_path_.param.empty() && active_sequence_param_overrides_.empty()) {
RCLCPP_ERROR(this->get_logger(), "BT params file path is empty");
// return false; //move home no params
}
@@ -628,17 +633,56 @@ bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
skill_name.c_str(), instance_name.c_str());
std::string instance_params;
if (current_bt_config_params_path_.config == skill_name) {
instance_params = current_bt_config_params_path_.param;
brain::robot_config::BtConfigParam current_path_copy;
bool found_override = false;
{
std::lock_guard<std::mutex> lk(exec_mutex_);
current_path_copy = current_bt_config_params_path_;
// Pattern is BaseName_TIMESTAMP_INDEX.
// Extract INDEX (last token after underscore).
size_t last_us = instance_name.find_last_of('_');
if (last_us != std::string::npos && last_us + 1 < instance_name.size()) {
std::string idx_str = instance_name.substr(last_us + 1);
if (!idx_str.empty() && std::all_of(idx_str.begin(), idx_str.end(), ::isdigit)) {
try {
size_t idx = std::stoul(idx_str);
if (active_sequence_param_overrides_.count(idx)) {
instance_params = active_sequence_param_overrides_[idx];
found_override = true;
RCLCPP_INFO(this->get_logger(), "Found override param at index %zu, instance_params: %s", idx, instance_params.c_str());
}
} catch (...) {
// Failed to parse index, ignore
RCLCPP_ERROR(this->get_logger(), "Failed to parse index %s", idx_str.c_str());
}
}
}
}
if (current_path_copy.param.empty() && !found_override) {
RCLCPP_ERROR(this->get_logger(), "BT params file path is empty");
// return false; //move home no params
}
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;
return UpdateBtActionParamsForSkillInstance(skill_name, instance_name, instance_params);
} 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());
} else {
//READ PARAMS FROM ROBOT CONFIG FILE
try {
robot_config_params_ = std::make_unique<brain::robot_config::RobotConfig>(current_bt_config_params_path_.param);
robot_config_params_ = std::make_unique<brain::robot_config::RobotConfig>(current_path_copy.param);
auto params = robot_config_params_->GetValue(instance_name, "params");
if (params == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "BT params file %s does not contain params for %s",
current_bt_config_params_path_.param.c_str(), instance_name.c_str());
current_path_copy.param.c_str(), instance_name.c_str());
return false;
} else {
instance_params = *params;
@@ -881,6 +925,8 @@ BTSequenceSpec CerebrumNode::MakeSequenceFromSkills(const std::vector<std::strin
{
BTSequenceSpec seq; seq.reserve(names.size());
for (const auto & n : names) {
// Just use standard name. Uniqueness is handled by bt_registry via timestamp/index suffix.
// Parameter mapping is handled by DeclareBtActionParamsForSkillInstance via index suffix.
seq.emplace_back(BTActionSpec{n + std::string("_H"), n + std::string("_H"), {}});
}
return seq;
@@ -974,8 +1020,93 @@ std::vector<std::string> CerebrumNode::ParseListString(const std::string & raw)
}
/**
* @brief Cancel the active unified ExecuteBtAction goal (if any).
* @thread_safety Pass-through to registry cancellation (assumed thread-safe).
* @brief Split a string by comma, ignoring commas inside quotes (single or double).
* @param raw Input string.
* @return Vector of tokens.
*/
std::vector<std::string> CerebrumNode::SplitQuotedString(const std::string & raw)
{
std::vector<std::string> result;
std::string token;
bool in_quote_single = false;
bool in_quote_double = false;
auto trim_and_push = [&](std::string t) {
// 1. Trim surrounding whitespace
t.erase(0, t.find_first_not_of(" \t\n\r"));
t.erase(t.find_last_not_of(" \t\n\r") + 1);
// 2. If wrapped in quotes, strip them (assuming matching start/end)
if (t.size() >= 2) {
if (t.front() == '\'' && t.back() == '\'') {
t = t.substr(1, t.size() - 2);
} else if (t.front() == '"' && t.back() == '"') {
t = t.substr(1, t.size() - 2);
}
}
result.push_back(t); // Keep even if empty strings? For alignment with params, yes.
};
for (size_t i = 0; i < raw.size(); ++i) {
char c = raw[i];
if (c == '\'' && !in_quote_double) {
in_quote_single = !in_quote_single;
token.push_back(c);
} else if (c == '"' && !in_quote_single) {
in_quote_double = !in_quote_double;
token.push_back(c);
} else if (c == ',' && !in_quote_single && !in_quote_double) {
trim_and_push(token);
token.clear();
} else {
token.push_back(c);
}
}
// Last token
trim_and_push(token);
return result;
}
/**
* @brief Split a string by comma, preserving order and duplicates.
* @param raw Input string.
* @return Vector of tokens.
* @thread_safety Pure static utility.
*/
std::vector<std::string> CerebrumNode::SplitString(const std::string & raw)
{
if (raw.empty()) {
return std::vector<std::string>();
}
std::vector<std::string> result;
std::string token;
for (char c : raw) {
if (c == ',') {
// Trim
token.erase(0, token.find_first_not_of(" \t\n\r"));
token.erase(token.find_last_not_of(" \t\n\r") + 1);
if (!token.empty()) {
result.push_back(token);
}
token.clear();
} else {
token.push_back(c);
}
}
// Trim and push last
token.erase(0, token.find_first_not_of(" \t\n\r"));
token.erase(token.find_last_not_of(" \t\n\r") + 1);
if (!token.empty()) {
result.push_back(token);
}
return result;
}
/**
* @brief Parse a BT result detail string into a result code.
* @param detail Input string.
* @return Result code.
* @thread_safety Pure static utility.
*/
void CerebrumNode::CancelActiveExecuteBtGoal()
{
@@ -1186,8 +1317,8 @@ void CerebrumNode::CreateServices()
if (req->type == "Trigger") {
HandleTriggerRebuild(req, resp);
} else if (req->type == "Remote") {
HandleRemoteRebuild(req, resp);
} else if (req->type == "Remote" || req->type == "Sequence") {
HandleGenericRebuild(req, resp);
} else if (req->type == "Local") {
// TODO: Implement local rebuild
resp->success = false;
@@ -1220,6 +1351,10 @@ void CerebrumNode::HandleTriggerRebuild(
if (req->config == "CancelBTTask") {
try {
tree_.haltTree();
{
std::lock_guard<std::mutex> lk(exec_mutex_);
active_sequence_param_overrides_.clear(); // Clear overrides on cancel
}
RCLCPP_INFO(this->get_logger(), "halting previous BT ok, CancelBTTask");
} catch (...) {
// Swallow halt exceptions.
@@ -1244,7 +1379,12 @@ void CerebrumNode::HandleTriggerRebuild(
}
BuildBehaviorTreeFromFile(path_param.config);
current_bt_config_params_path_ = path_param;
{
std::lock_guard<std::mutex> lk(exec_mutex_);
current_bt_config_params_path_ = path_param;
// Clear overridden params when switching file-based BT
active_sequence_param_overrides_.clear();
}
// Update working info
// std::string scheduled = ExtractFilenameWithoutExtension(path_param.config);
@@ -1273,30 +1413,52 @@ void CerebrumNode::HandleTriggerRebuild(
* @param req
* @param resp
*/
void CerebrumNode::HandleRemoteRebuild(
void CerebrumNode::HandleGenericRebuild(
const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp)
{
auto tmp = ParseListString(req->config);
if (!tmp.empty()) {
fixed_skill_sequence_ = tmp;
current_bt_config_params_path_.config = req->config;
current_bt_config_params_path_.param = req->param;
auto skills = SplitQuotedString(req->config);
auto params = SplitQuotedString(req->param);
if (!skills.empty()) {
// Check if params match skills count
if (!params.empty() && params.size() != skills.size()) {
RCLCPP_WARN(this->get_logger(), "HandleGenericRebuild: Skills count (%zu) != Params count (%zu).", skills.size(), params.size());
}
// Clear previous override params and update config safely
{
std::lock_guard<std::mutex> lk(exec_mutex_);
active_sequence_param_overrides_.clear();
// Store params by index. Logic relies on BTRegistry generating names with corresponding indices.
for (size_t i = 0; i < skills.size(); ++i) {
if (i < params.size()) {
active_sequence_param_overrides_[i] = params[i];
}
}
current_bt_config_params_path_.config = req->config;
}
fixed_skill_sequence_ = skills;
// Current logic uses config field as "command" or "filename"
// current_bt_config_params_path_.config updated above under lock
RunVlmModel();
CancelActiveExecuteBtGoal();
UpdateBehaviorTree();
if (robot_work_info_.task.size() > 2) {
robot_work_info_.task[2] = req->config;
robot_work_info_.task[2] = (req->type == "Sequence") ? "SequenceRebuild" : req->config;
}
resp->success = true;
resp->message = "Remote rebuild triggered";
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service Remote rebuild triggered");
resp->message = req->type + " rebuild triggered";
RCLCPP_WARN(this->get_logger(), "cerebrum/rebuild_now Service %s rebuild triggered", req->type.c_str());
} else {
resp->success = false;
resp->message = "Remote rebuild failed";
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Failed to parse remote rebuild config");
resp->message = req->type + " rebuild failed";
RCLCPP_ERROR(this->get_logger(), "cerebrum/rebuild_now Service Failed to parse %s rebuild config", req->type.c_str());
}
}