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:
david
2025-10-29 17:04:06 +08:00
parent 5b83a865cc
commit 8c6bb8cd6e
2 changed files with 32 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -171,9 +171,9 @@ private:
void LoadParameters();
/** Declare per-skill instance parameter entries used to customize outgoing calls. */
void UpdateBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name, const std::string & instance_params);
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. */
void DeclareBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name);
bool DeclareBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name);
/** 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>.*). */

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

@@ -337,11 +337,6 @@ void CerebrumNode::CerebrumTask()
RCLCPP_WARN(this->get_logger(), "CerebrumTask Switching to BT config file path: %s", path_param.config.c_str());
return;
}
// Use correctly cased API methods
// RunVlmModel();
// CancelActiveExecuteBtGoal();
// UpdateBehaviorTree();
}
/**
@@ -478,7 +473,7 @@ void CerebrumNode::RegisterSkillBtActions()
const auto & skill = kv.second;
const std::string bt_type = skill.name + std::string("_H");
BTActionHandlers handlers;
handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode & node) {
handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode & node) {
auto & info = per_node_exec_[bt_type];
const std::string instance_name = node.name();
// If this specific BT node instance already finished successfully earlier in this tree run,
@@ -499,11 +494,9 @@ void CerebrumNode::RegisterSkillBtActions()
info.last_progress = 0.0;
info.cancel_state = PerNodeExecInfo::CancelState::NONE;
info.current_instance = instance_name;
DeclareBtActionParamsForSkillInstance(skill.name, instance_name);
// Override goal builder to send only this skill.
single_skill_goal_override_ = skill.name;
single_skill_instance_override_ = instance_name;
if (!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
if (!DeclareBtActionParamsForSkillInstance(skill.name, instance_name) ||
!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
RCLCPP_ERROR(this->get_logger(), "ExecuteBtAction server unavailable");
info.in_flight = false;
info.result = false;
@@ -512,6 +505,10 @@ void CerebrumNode::RegisterSkillBtActions()
info.current_instance.reset();
return BT::NodeStatus::FAILURE;
}
// Override goal builder to send only this skill.
single_skill_goal_override_ = skill.name;
single_skill_instance_override_ = instance_name;
action_registry_->send(brain::kExecuteBtActionName, this->get_logger());
RCLCPP_INFO(this->get_logger(), "[%s/%s] BTAction on_start", bt_type.c_str(), instance_name.c_str());
// Clear override after send to avoid affecting other triggers.
@@ -582,14 +579,14 @@ void CerebrumNode::RegisterSkillBtActions()
* @param instance_name
* @param instance_params
*/
void CerebrumNode::UpdateBtActionParamsForSkillInstance(
bool CerebrumNode::UpdateBtActionParamsForSkillInstance(
const std::string & skill_name,
const std::string & instance_name,
const std::string & instance_params)
{
if (skill_name.empty() || instance_name.empty() || instance_params.empty()) {
RCLCPP_WARN(this->get_logger(), "Update BtAction Params ForSkillInstance called with empty skill_name, instance_name, or instance_params");
return;
RCLCPP_ERROR(this->get_logger(), "Update BtAction Params ForSkillInstance called with empty skill_name, instance_name, or instance_params");
return false;
}
// Per-instance namespace: cerebrum.bt.<Skill>.<Instance>.*
const std::string ns = std::string("cerebrum.bt.") + skill_name + "." + instance_name + ".";
@@ -607,7 +604,9 @@ void CerebrumNode::UpdateBtActionParamsForSkillInstance(
RCLCPP_INFO(this->get_logger(), "Seeded sample payload for %s instance %s", skill_name.c_str(), instance_name.c_str());
} catch (const std::exception & e) {
RCLCPP_ERROR(this->get_logger(), "[%s] seeding sample payload failed: %s", skill_name.c_str(), e.what());
return false;
}
return true;
}
/**
@@ -616,50 +615,53 @@ void CerebrumNode::UpdateBtActionParamsForSkillInstance(
* @param skill_name
* @param instance_name
*/
void CerebrumNode::DeclareBtActionParamsForSkillInstance(
bool CerebrumNode::DeclareBtActionParamsForSkillInstance(
const std::string & skill_name,
const std::string & instance_name)
{
if (skill_name.empty() || instance_name.empty()) {
RCLCPP_WARN(this->get_logger(), "Declare BtAction Params ForSkillInstance called with empty skill_name or instance_name");
return;
RCLCPP_ERROR(this->get_logger(), "Declare BtAction Params ForSkillInstance called with empty skill_name or instance_name");
return false;
}
if (current_bt_config_params_path_.param.empty()) {
RCLCPP_ERROR(this->get_logger(), "BT params file path is empty; cannot load sample params");
return false;
}
RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill_name=%s, instance_name=%s",
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;
RCLCPP_INFO(this->get_logger(), "Remote params: %s", instance_params.c_str());
} else {
//READ PARAMS FROM ROBOT CONFIG FILE
if (current_bt_config_params_path_.param.empty()) {
RCLCPP_WARN(this->get_logger(), "BT params file path is empty; cannot load sample params");
return;
}
try {
robot_config_params_ = std::make_unique<brain::robot_config::RobotConfig>(current_bt_config_params_path_.param);
auto params = robot_config_params_->GetValue(instance_name, "params");
if (params == std::nullopt) {
RCLCPP_WARN(this->get_logger(), "BT params file %s does not contain params for %s",
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());
return false;
} else {
instance_params = *params;
//update working info
robot_work_info_.skill = skill_name;
robot_work_info_.action_name = instance_name;
robot_work_info_.instance_params = instance_params;
RCLCPP_INFO(this->get_logger(), "Loaded BT params for %s, instance_params: %s",
instance_name.c_str(), instance_params.c_str());
}
} catch (const std::exception & e) {
RCLCPP_ERROR(this->get_logger(), "[%s] read params failed: %s", skill_name.c_str(), e.what());
return false;
}
}
UpdateBtActionParamsForSkillInstance(skill_name, instance_name, instance_params);
//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);
}
/**