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

add BtActionParamsForSkillInstance

Browse Source
This commit is contained in:
david
2025-10-20 17:11:09 +08:00
parent 34a629bbfb
commit 0269009987
6 changed files with 645 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
src/brain/config/bt_carry_boxes.params.yaml Normal file
View File

@@ -0,0 +1,136 @@
- name: root
params: '{}
'
- name: retry_all_action
params: '{}
'
- name: s1_waist_bend_down
params: 'move_pitch_degree: 0.0
move_yaw_degree: 0.0
'
- name: s2_arm_stretch_out
params: 'body_id: 0
data_type: 0
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s3_hand_pickup
params: 'mode: 0
effort: 0.0
'
- name: s4_arm_lift
params: 'body_id: 0
data_type: 0
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s5_waist_bend_up
params: 'move_pitch_degree: 0.0
move_yaw_degree: 0.0
'
- name: s6_waist_turn_around
params: 'move_pitch_degree: 0.0
move_yaw_degree: 0.0
'
- name: s7_leg_move_back
params: 'move_up_distance: 0.0
'
- name: s8_wheel_move_back
params: 'move_distance: 0.0
move_angle: 0.0
'
- name: s9_waist_bend_down
params: 'move_pitch_degree: 0.0
move_yaw_degree: 0.0
'
- name: s10_arm_stretch_out
params: 'body_id: 0
data_type: 0
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s11_hand_release
params: 'mode: 0
effort: 0.0
'
- name: s12_arm_move_to_snapshot_pose
params: 'body_id: 0
data_type: 0
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: s13_camera_take_photo
params: 'mode: 0
effort: 0.0
'
- name: s14_waist_bend_up
params: 'move_pitch_degree: 0.0
move_yaw_degree: 0.0
'
- name: s15_arm_retract
params: 'body_id: 0
data_type: 0
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'

32
src/brain/config/bt_vision_grasp.params.yaml Normal file
View File

@@ -0,0 +1,32 @@
- name: root
params: '{}
'
- name: retry_vision_hand
params: '{}
'
- name: VisionObjectRecognition_H
params: '{}
'
- name: Arm_H
params: 'body_id: 0
data_type: 0
data_length: 0
command_id: 0
frame_time_stamp: 0
data_array: []
'
- name: HandControl_H
params: 'mode: 0
effort: 0.0
'

11
src/brain/config/robot_config.yaml Normal file
View File

@@ -0,0 +1,11 @@
- name: brain
version: 1.0.0
skill_file: robot_skills.yaml
- name: cerebrum_node
version: 1.0.0
bt_config_file: bt_carry_boxes.xml
bt_params_file: bt_carry_boxes.params.yaml
- name: cerebellum_node
version: 1.0.0

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

@@ -18,8 +18,8 @@
#include "brain/constants.hpp"
#include "brain/epoch_filter.hpp"
#include <std_srvs/srv/trigger.hpp>
// For ExecuteBtAction SkillCall building
#include "brain_interfaces/msg/skill_call.hpp"
#include <rclcpp/node_interfaces/node_parameters_interface.hpp>
namespace brain
@@ -125,14 +125,6 @@ public:
static ExecResultCode ParseResultDetail(const std::string & detail);
private:
/**
* Initialize core registry objects (action, BT, skill manager).
*
* Ownership: Creates unique_ptr instances and wires dependencies.
* Must be called first in the constructor before any method that relies on
* these registries. Safe to call only once (no internal guards).
*/
void InitializeRegistries();
/**
* Declare ROS 2 parameters used by this node.
@@ -152,7 +144,7 @@ private:
*
* Returns: Absolute or relative path string (may be empty on failure).
*/
std::string ResolveSkillsFile();
std::string ResolveSkillsFile(std::string skills_file_path);
/**
* Load skill definitions from a YAML file via SkillManager.
@@ -176,24 +168,6 @@ private:
*/
void LoadParameters();
/**
* Register per-skill BT node handlers and the unified ExecuteBtAction client.
*
* This combines two tightly related registration phases to ensure action
* client availability for the handlers. Safe to call exactly once during
* construction.
*/
void RegisterBtAndActionClients();
/**
* Create periodic timers:
* - bt_timer_: high-frequency BehaviorTree ticking.
* - task_timer_: low-frequency sequence/model update trigger.
*
* Must be invoked after registries & parameters are ready. Not re-entrant.
*/
void CreateTimers();
/**
* Create external service interfaces (currently a Trigger to force BT rebuild).
*
@@ -203,11 +177,16 @@ private:
*/
void CreateServices();
// ---- Per-BT-action dynamic parameter support ----
/** Declare per-skill parameter entries used to customize outgoing calls. */
void DeclareBtActionParamsForSkill(const std::string & skill_name);
/** 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);
/** 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);
/** Build a SkillCall message for given skill by consulting SkillSpec and per-skill params. */
brain_interfaces::msg::SkillCall BuildSkillCallForSkill(const std::string & skill_name) const;
/** Build a SkillCall with optional per-instance override (cerebrum.bt.<Skill>.<Instance>.*). */
brain_interfaces::msg::SkillCall BuildSkillCallForSkill(const std::string & skill_name, const std::optional<std::string> & instance_name) const;
/** Install on_set_parameters callback to validate and accept dynamic updates. */
void SetupDynamicParameterHandling();
BT::Tree tree_;
BT::BehaviorTreeFactory factory_;
@@ -215,7 +194,10 @@ private:
std::unique_ptr<BTRegistry> bt_registry_;
std::unique_ptr<SkillManager> skill_manager_;
std::string active_sequence_;
std::string share_directory_;
std::string robot_skill_file_path_;
std::string bt_config_file_path_;
std::string bt_params_file_path_;
std::filesystem::file_time_type bt_xml_last_write_time_{}; // last observed mod time (for hot reload)
rclcpp::TimerBase::SharedPtr task_timer_;
rclcpp::TimerBase::SharedPtr bt_timer_;
@@ -342,6 +324,11 @@ private:
// 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_;
// When a per-skill BT node starts, capture its BT node instance name for per-instance params.
std::optional<std::string> single_skill_instance_override_;
// Handle for dynamic parameter updates (cerebrum.bt.<skill>.*)
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_handle_;
};

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

@@ -20,6 +20,7 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behaviortree_cpp/action_node.h>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
// Project headers
#include "brain/constants.hpp"
@@ -82,18 +83,32 @@ struct UpdatingFlagGuard
*/
CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
: rclcpp::Node("cerebrum_node", options),
bt_config_file_path_(ament_index_cpp::get_package_share_directory("brain") + std::string("/config/bt_carry_boxes.xml"))
share_directory_(ament_index_cpp::get_package_share_directory("brain")),
robot_skill_file_path_(share_directory_ + std::string("/config/robot_skills.yaml")),
bt_config_file_path_(share_directory_ + std::string("/config/bt_carry_boxes.xml")),
bt_params_file_path_(share_directory_ + std::string("/config/bt_carry_boxes.params.yaml"))
{
InitializeRegistries();
// Initialize
action_registry_ = std::make_unique<ActionClientRegistry>(this);
bt_registry_ = std::make_unique<BTRegistry>(factory_, this);
skill_manager_ = std::make_unique<SkillManager>(this, action_registry_.get(), bt_registry_.get());
if (!skill_manager_->load_from_file(robot_skill_file_path_)) {
RCLCPP_WARN(this->get_logger(), "Failed to load skills from %s", robot_skill_file_path_.c_str());
}
DeclareParameters();
auto skills_file = ResolveSkillsFile();
LoadSkills(skills_file);
LoadParameters();
RegisterBtAndActionClients();
CreateTimers();
SetupDynamicParameterHandling();
RegisterSkillBtActions();
RegisterActionClient();
bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
task_timer_ = this->create_wall_timer(5000ms, [this]() {CerebrumTask();});
CreateServices();
RCLCPP_INFO(this->get_logger(),
"CerebrumNode initialized (timeout=%.2f random_pick=%zu)",
RCLCPP_INFO(this->get_logger(), "CerebrumNode initialized (timeout=%.2f random_pick=%zu)",
node_timeout_sec_, random_skill_pick_count_);
}
@@ -119,9 +134,7 @@ void CerebrumNode::RegisterBtAction(
void CerebrumNode::SendExecuteBt(const std::string & bt_action_name)
{
if (!action_registry_) {
RCLCPP_WARN(
this->get_logger(), "Action registry not initialized; cannot send %s",
bt_action_name.c_str());
RCLCPP_WARN(this->get_logger(), "Action registry not initialized; cannot send %s", bt_action_name.c_str());
return;
}
action_registry_->send(bt_action_name, this->get_logger());
@@ -162,10 +175,10 @@ void CerebrumNode::RegisterActionClient()
// Populate per-skill calls[] using dynamic parameters
g.calls.clear();
if (single_skill_goal_override_) {
g.calls.push_back(BuildSkillCallForSkill(*single_skill_goal_override_));
g.calls.push_back(BuildSkillCallForSkill(*single_skill_goal_override_, single_skill_instance_override_));
} else {
for (const auto & s : current_sequence_skills_) {
g.calls.push_back(BuildSkillCallForSkill(s));
g.calls.push_back(BuildSkillCallForSkill(s, std::nullopt));
}
}
// Debug: log outgoing calls being sent to Cerebellum
@@ -175,8 +188,7 @@ void CerebrumNode::RegisterActionClient()
RCLCPP_INFO(this->get_logger(), " out_call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu",
i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size());
}
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction]Sending goal epoch=%u skills=%s",
g.epoch, g.action_name.c_str());
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction]Sending goal epoch=%u skills=%s", g.epoch, g.action_name.c_str());
return g;
},
// Feedback
@@ -237,16 +249,11 @@ void CerebrumNode::RegisterActionClient()
res.result &&
res.result->success);
if (ok) {
RCLCPP_INFO(
this->get_logger(),
"[ExecuteBtAction][result] OK: %s",
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] OK: %s",
res.result ? res.result->message.c_str() : "<null>");
} else {
RCLCPP_ERROR(
this->get_logger(),
"[ExecuteBtAction][result] FAIL code=%d msg=%s",
static_cast<int>(res.code),
res.result ? res.result->message.c_str() : "<null>");
RCLCPP_ERROR(this->get_logger(), "[ExecuteBtAction][result] FAIL code=%d msg=%s",
static_cast<int>(res.code), res.result ? res.result->message.c_str() : "<null>");
}
for (auto & kv : per_node_exec_) {
auto & info = kv.second;
@@ -255,11 +262,8 @@ void CerebrumNode::RegisterActionClient()
info.awaiting_result = false;
info.result = ok; // authoritative final
info.result_code = ok ? ExecResultCode::SUCCESS : ExecResultCode::FAILURE;
RCLCPP_INFO(
this->get_logger(),
"[ExecuteBtAction][result] Finalizing node %s result=%s",
kv.first.c_str(),
info.result ? "SUCCESS" : "FAILURE");
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction][result] Finalizing node %s result=%s",
kv.first.c_str(), info.result ? "SUCCESS" : "FAILURE");
}
}
});
@@ -339,15 +343,12 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
{
BT::Tree new_tree;
if (!bt_registry_->build_tree_from_xml_file(xml_file, new_tree)) {
RCLCPP_ERROR(
this->get_logger(), "Failed building BT from file %s",
xml_file.c_str());
RCLCPP_ERROR(this->get_logger(), "Failed building BT from file %s", xml_file.c_str());
return;
}
try {
tree_.haltTree();
} catch (...) {
// Swallow halt exceptions.
RCLCPP_ERROR(this->get_logger(), "Exception halting previous BT");
}
tree_ = std::move(new_tree);
@@ -360,10 +361,7 @@ void CerebrumNode::BuildBehaviorTreeFromFile(const std::string & xml_file)
epoch_filter_.reset_epoch(new_epoch, seq_skills);
per_node_exec_.clear();
bt_pending_run_ = true;
RCLCPP_INFO(
this->get_logger(),
"BT built from file epoch=%llu",
static_cast<unsigned long long>(new_epoch));
RCLCPP_INFO(this->get_logger(), "BT built from file epoch=%llu", static_cast<unsigned long long>(new_epoch));
}
/**
@@ -382,9 +380,7 @@ void CerebrumNode::UpdateBehaviorTree()
UpdatingFlagGuard guard(bt_updating_);
BT::Tree new_tree;
if (!bt_registry_->build_tree_from_sequence(active_sequence_, new_tree)) {
RCLCPP_ERROR(
this->get_logger(), "Failed building BT for sequence %s",
active_sequence_.c_str());
RCLCPP_ERROR(this->get_logger(), "Failed building BT for sequence %s", active_sequence_.c_str());
return;
}
try {
@@ -403,11 +399,8 @@ void CerebrumNode::UpdateBehaviorTree()
epoch_filter_.reset_epoch(new_epoch, seq_skills);
per_node_exec_.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());
RCLCPP_INFO(this->get_logger(), "BT updated epoch=%llu sequence=%s",
static_cast<unsigned long long>(new_epoch), active_sequence_.c_str());
}
/**
@@ -431,7 +424,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 &) {
handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode & node) {
auto & info = per_node_exec_[bt_type];
if (info.in_flight || info.result) {
return info.result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::RUNNING;
@@ -443,10 +436,12 @@ void CerebrumNode::RegisterSkillBtActions()
info.skill_name = skill.name;
info.last_progress = 0.0;
info.cancel_state = PerNodeExecInfo::CancelState::NONE;
// Ensure per-skill params are declared before send
DeclareBtActionParamsForSkill(skill.name);
const std::string instance_name = node.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)) {
RCLCPP_ERROR(this->get_logger(), "ExecuteBtAction server unavailable");
info.in_flight = false;
@@ -459,6 +454,7 @@ void CerebrumNode::RegisterSkillBtActions()
RCLCPP_INFO(this->get_logger(), "[%s] BTAction on_start", bt_type.c_str());
// Clear override after send to avoid affecting other triggers.
single_skill_goal_override_.reset();
single_skill_instance_override_.reset();
return BT::NodeStatus::RUNNING;
};
handlers.on_running = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
@@ -499,17 +495,23 @@ void CerebrumNode::RegisterSkillBtActions()
}
/**
* @brief Declare BT action parameters for a skill
* @brief Update BT action parameters for a skill instance
*
* @param skill_name
* @param skill_name
* @param instance_name
* @param instance_params
*/
void CerebrumNode::DeclareBtActionParamsForSkill(const std::string & skill_name)
void CerebrumNode::UpdateBtActionParamsForSkillInstance(
const std::string & skill_name,
const std::string & instance_name,
const std::string & instance_params)
{
// Declare a generic namespace for each skill to allow configuring routing/topic and payload
// Example params:
// cerebrum.bt.<Skill>.topic (string)
// cerebrum.bt.<Skill>.payload_yaml (string)
const std::string ns = std::string("cerebrum.bt.") + skill_name + ".";
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;
}
// Per-instance namespace: cerebrum.bt.<Skill>.<Instance>.*
const std::string ns = std::string("cerebrum.bt.") + skill_name + "." + instance_name + ".";
const std::string p_topic = ns + "topic";
const std::string p_payload = ns + "payload_yaml";
if (!this->has_parameter(p_topic)) {
@@ -518,33 +520,108 @@ void CerebrumNode::DeclareBtActionParamsForSkill(const std::string & skill_name)
if (!this->has_parameter(p_payload)) {
this->declare_parameter<std::string>(p_payload, std::string(""));
}
// Seed example payload for selected skills to ease testing
RCLCPP_INFO(this->get_logger(), "Declaring BT action parameters for skill instance %s", instance_name.c_str());
try {
auto current = this->get_parameter(p_payload).as_string();
if (current.empty()) {
std::string sample;
if (skill_name == "MoveWaist") {
sample = "move_pitch_degree: 8.0\nmove_yaw_degree: -12.0\n";
} else if (skill_name == "MoveLeg") {
sample = "move_up_distance: 0.06\n";
} else if (skill_name == "MoveWheel") {
sample = "move_distance: 0.50\nmove_angle: 15.0\n";
} else if (skill_name == "MoveHome") {
// MoveHome goal is empty; seed explicit empty map for clarity
sample = "{}\n";
} else if (skill_name == "Arm") {
sample = "body_id: 1\ndata_type: 3\ndata_length: 7\ncommand_id: 1\nframe_time_stamp: 123456789\ndata_array: [0.222853, 0.514124, 0.261742, -0.65115316, 0.05180144, -0.19539139, 0.73153153]\n";
}
if (!sample.empty()) {
this->set_parameter(rclcpp::Parameter(p_payload, sample));
RCLCPP_INFO(this->get_logger(), "[Cerebrum] Seeded sample payload for %s", skill_name.c_str());
}
}
this->set_parameter(rclcpp::Parameter(p_payload, instance_params));
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_WARN(this->get_logger(), "[%s] seeding sample payload failed: %s", skill_name.c_str(), e.what());
RCLCPP_ERROR(this->get_logger(), "[%s] seeding sample payload failed: %s", skill_name.c_str(), e.what());
}
}
/**
* @brief Declare BT action parameters for a skill instance
*
* @param skill_name
* @param instance_name
*/
void 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;
}
std::string instance_params;
if (skill_name == "MoveWaist" && instance_name == "s1_waist_bend_down") {
instance_params = "move_pitch_degree: 10.0\nmove_yaw_degree: -15.0\n";
} else if (skill_name == "MoveLeg" && instance_name == "s7_leg_move_back") {
instance_params = "move_up_distance: 0.08\n";
} else if (skill_name == "MoveWheel" && instance_name == "s8_wheel_move_back") {
instance_params = "move_distance: 0.60\nmove_angle: 20.0\n";
} else if (skill_name == "MoveHome" && instance_name == "s0_move_home") {
// MoveHome goal is empty; seed explicit empty map for clarity
instance_params = "{}\n";
} else if (skill_name == "Arm" && instance_name == "s2_arm_stretch_out") {
instance_params = "body_id: 1\ndata_type: 3\ndata_length: 7\ncommand_id: 1\nframe_time_stamp: 123456789\ndata_array: [0.3, 0.4, 0.5, -0.6, 0.07, -0.18, 0.72]\n";
}
UpdateBtActionParamsForSkillInstance(skill_name, instance_name, instance_params);
}
/**
* @brief Install on_set_parameters callback to accept dynamic per-skill updates.
*
* Only parameters within the prefix "cerebrum.bt." are intercepted. Minimal validation:
* - topic: any string accepted
* - payload_yaml: any string accepted (executor will validate downstream)
# Set MoveWheel topic
ros2 param set /cerebrum_node cerebrum.bt.MoveWheel.topic /robot/move_wheel
# Set Arm payload
ros2 param set /cerebrum_node cerebrum.bt.Arm.payload_yaml "body_id: 1
data_type: 3
data_length: 7
command_id: 1
frame_time_stamp: 123456789
data_array: [0.1, 0.2, 0.3, -0.4, 0.05, -0.19, 0.73]"
*/
void CerebrumNode::SetupDynamicParameterHandling()
{
using rclcpp::Parameter;
using rcl_interfaces::msg::SetParametersResult;
auto cb = [this](const std::vector<Parameter> & params) -> SetParametersResult {
SetParametersResult result;
result.successful = true;
result.reason = "ok";
const std::string prefix = "cerebrum.bt.";
for (const auto & p : params) {
const std::string & name = p.get_name();
if (name.rfind(prefix, 0) != 0) {
continue; // other params handled by default mechanisms
}
// Expected pattern: cerebrum.bt.<Skill>.topic | payload_yaml
// We perform a light parse to ensure a field segment exists
auto last_dot = name.rfind('.');
if (last_dot == std::string::npos || last_dot + 1 >= name.size()) {
result.successful = false;
result.reason = "invalid per-skill param name";
break;
}
const std::string field = name.substr(last_dot + 1);
if (field != "topic" && field != "payload_yaml") {
// Reject unknown fields under this namespace to avoid typos
result.successful = false;
result.reason = "unsupported field for cerebrum.bt.*";
break;
}
// Accept string-only values
if (p.get_type() != rclcpp::ParameterType::PARAMETER_STRING) {
result.successful = false;
result.reason = "per-skill params must be strings";
break;
}
// No further validation here; downstream will interpret.
RCLCPP_INFO(this->get_logger(), "[params] updated %s", name.c_str());
}
return result;
};
param_cb_handle_ = this->add_on_set_parameters_callback(cb);
}
/**
* @brief Build a SkillCall message for a specific skill.
*
@@ -585,6 +662,42 @@ brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std:
return call;
}
/**
* @brief Declare BT action parameters for a skill instance
*
* @param skill_name
* @param instance_name
* @return brain_interfaces::msg::SkillCall
*/
brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(
const std::string & skill_name,
const std::optional<std::string> & instance_name) const
{
auto base = BuildSkillCallForSkill(skill_name);
// If instance_name provided, try per-instance override first, falling back to per-skill
if (!instance_name) {
return base;
}
const std::string ns = std::string("cerebrum.bt.") + skill_name + "." + *instance_name + ".";
const std::string p_topic = ns + "topic";
const std::string p_payload = ns + "payload_yaml";
try {
// topic override
if (this->has_parameter(p_topic)) {
auto v = this->get_parameter(p_topic).as_string();
if (!v.empty()) base.topic = v;
}
// payload override
if (this->has_parameter(p_payload)) {
auto v = this->get_parameter(p_payload).as_string();
if (!v.empty()) base.payload_yaml = v;
}
} catch (const std::exception & e) {
RCLCPP_WARN(this->get_logger(), "[%s/%s] read instance params failed: %s", skill_name.c_str(), instance_name->c_str(), e.what());
}
return base;
}
/**
* @brief Randomly select up to count distinct action-capable skills (respecting whitelist).
* @param count Maximum number of skills.
@@ -788,21 +901,6 @@ CerebrumNode::ExecResultCode CerebrumNode::ParseResultDetail(const std::string &
return ExecResultCode::UNKNOWN;
}
/**
* @brief Initialize registry/manager singletons used by the node.
*
* Allocates ActionClientRegistry, BTRegistry and SkillManager with proper
* dependency ordering so later initialization steps (skill loading, BT
* registration) can safely use them. Must be called exactly once during
* construction before any other helper depending on these objects.
*/
void CerebrumNode::InitializeRegistries()
{
action_registry_ = std::make_unique<ActionClientRegistry>(this);
bt_registry_ = std::make_unique<BTRegistry>(factory_, this);
skill_manager_ = std::make_unique<SkillManager>(this, action_registry_.get(), bt_registry_.get());
}
/**
* @brief Declare all ROS 2 parameters consumed by this node.
*
@@ -812,60 +910,11 @@ void CerebrumNode::InitializeRegistries()
*/
void CerebrumNode::DeclareParameters()
{
this->declare_parameter<std::string>("skills_file", "");
this->declare_parameter<double>("node_timeout_sec", node_timeout_sec_);
this->declare_parameter<bool>(
"allow_bt_rebuild_during_execution",
allow_bt_rebuild_during_execution_);
this->declare_parameter<bool>("allow_bt_rebuild_during_execution", allow_bt_rebuild_during_execution_);
this->declare_parameter<std::string>("allowed_action_skills", "");
this->declare_parameter<std::string>("fixed_skill_sequence", "");
this->declare_parameter<int>(
"random_skill_pick_count",
static_cast<int>(random_skill_pick_count_));
}
/**
* @brief Resolve the effective skills definition YAML file path.
*
* Reads the declared 'skills_file' parameter. If empty, attempts to locate a
* packaged default at:
* <ament_package_share_directory(brain)>/config/robot_skills.yaml
* Fallback discovery failures are logged as warnings; the function never
* throws. An empty return value indicates no usable path could be determined.
*
* @return Resolved skills file path (may be empty if neither parameter nor default exists).
* @thread_safety Read-only parameter access; intended for use during construction only.
*/
std::string CerebrumNode::ResolveSkillsFile()
{
std::string skills_file;
this->get_parameter("skills_file", skills_file);
if (skills_file.empty()) {
try {
auto share = ament_index_cpp::get_package_share_directory("brain");
skills_file = share + std::string("/config/robot_skills.yaml");
RCLCPP_INFO(this->get_logger(), "Using default skills file: %s", skills_file.c_str());
} catch (const std::exception & e) {
RCLCPP_WARN(this->get_logger(), "Could not locate default skills file: %s", e.what());
}
}
return skills_file;
}
/**
* @brief Load skill specifications from a YAML file.
*
* Emits a warning if the file cannot be parsed or loaded, but does not throw.
* An empty path results in a no-op so callers can unconditionally invoke it.
*
* @param skills_file Absolute or relative path to YAML file (may be empty).
*/
void CerebrumNode::LoadSkills(const std::string & skills_file)
{
if (skills_file.empty()) {return;}
if (!skill_manager_->load_from_file(skills_file)) {
RCLCPP_WARN(this->get_logger(), "Failed to load skills from %s", skills_file.c_str());
}
this->declare_parameter<int>("random_skill_pick_count", static_cast<int>(random_skill_pick_count_));
}
/**
@@ -905,31 +954,6 @@ void CerebrumNode::LoadParameters()
}
}
/**
* @brief Register per-skill BT action handlers plus the unified action client.
*
* Bundled to ensure the ExecuteBtAction client exists before any BT node can
* attempt to send goals. Idempotent in practice because internal registration
* functions guard against duplication.
*/
void CerebrumNode::RegisterBtAndActionClients()
{
RegisterSkillBtActions();
RegisterActionClient();
}
/**
* @brief Create timers used for BT ticking and periodic sequence/model updates.
*
* bt_timer_: High-frequency (10ms) ticker executing the BT root when a run is pending.
* task_timer_: Low-frequency (5s) trigger for model-driven sequence selection and rebuild.
*/
void CerebrumNode::CreateTimers()
{
bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();});
task_timer_ = this->create_wall_timer(5000ms, [this]() {CerebrumTask();});
}
/**
* @brief Create externally callable services.
*

256
src/scripts/gen_bt_params.py Normal file
View File

@@ -0,0 +1,256 @@
#!/usr/bin/env python3
"""
Generate YAML params mapping for BehaviorTree action nodes.
This script parses a BehaviorTree XML (e.g., src/brain/config/bt_carry_boxes.xml),
extracts each leaf action node's instance name (the 'name' attribute) and node type
tag (e.g., MoveWaist_H), resolves the corresponding .action definition from a
neighboring hivecore_robot_interfaces package, and writes a YAML configuration file.
Output YAML format (list):
- name: <bt_node_name>
params: | # YAML snippet as a string
field_a: 0
field_b: []
By default, interface types are resolved from ../hivecore_robot_interfaces relative to
this repository root, matching other generator scripts.
Usage:
python3 src/scripts/gen_bt_params.py \
--xml src/brain/config/bt_carry_boxes.xml \
--interfaces ../hivecore_robot_interfaces \
--out src/brain/config/bt_carry_boxes.params.yaml
Notes:
- Only the Goal part of .action files is considered (fields before the first ---).
- For unknown/complex ROS types, placeholder structures are emitted (e.g., for
geometry_msgs/PoseStamped). Otherwise the field is set to null as a placeholder.
"""
from __future__ import annotations
import argparse
import os
import re
import sys
import textwrap
import xml.etree.ElementTree as ET
from typing import Dict, List, Optional, Sequence, Tuple
import yaml
PRIMITIVES_DEFAULTS: Dict[str, object] = {
# integers
'int8': 0, 'int16': 0, 'int32': 0, 'int64': 0,
'uint8': 0, 'uint16': 0, 'uint32': 0, 'uint64': 0,
'int': 0,
# floats
'float32': 0.0, 'float64': 0.0, 'float': 0.0, 'double': 0.0,
# misc
'bool': False,
'string': '',
'time': 0,
}
def to_snake(s: str) -> str:
out = ''
for i, ch in enumerate(s):
if ch.isupper():
if i > 0:
out += '_'
out += ch.lower()
else:
out += ch
return out
def snake_to_camel(s: str) -> str:
parts = re.split(r'[_\-]', s)
return ''.join(p.capitalize() for p in parts if p)
def find_file_case_insensitive(dirname: str, filename: str) -> Optional[str]:
if not os.path.isdir(dirname):
return None
target = filename.lower()
for e in os.listdir(dirname):
if e.lower() == target:
return os.path.join(dirname, e)
return None
def resolve_interface_subdir(root: str, sub: str) -> str:
candidates = [
os.path.join(root, sub),
os.path.join(root, 'src', sub),
os.path.join(root, 'interfaces', sub),
]
for c in candidates:
if os.path.isdir(c):
return c
return candidates[0]
def parse_action_file(path: str) -> List[Tuple[str, str]]:
"""Parse a .action file, return list of (ros_type, field_name) only for Goal part."""
try:
with open(path, 'r', encoding='utf-8') as f:
content = f.read()
except FileNotFoundError:
return []
parts = content.split('\n---\n')
goal_part = parts[0] if parts else ''
fields: List[Tuple[str, str]] = []
for line in goal_part.splitlines():
line = line.strip()
if not line or line.startswith('#'):
continue
m = re.match(r"^([A-Za-z0-9_\[\]/]+)\s+([A-Za-z0-9_]+).*$", line)
if m:
ty = m.group(1)
name = m.group(2)
fields.append((ty, name))
return fields
def locate_action_file(interfaces_root: str, base: str) -> Optional[str]:
action_dir = resolve_interface_subdir(interfaces_root, 'action')
# 1) Try as-is (CamelCase)
candidate = os.path.join(action_dir, f'{base}.action')
if os.path.exists(candidate):
return candidate
# 2) Try CamelCase of snake
camel = snake_to_camel(base)
if camel != base:
candidate2 = os.path.join(action_dir, f'{camel}.action')
if os.path.exists(candidate2):
return candidate2
# 3) Try snake_case
snake = to_snake(base)
candidate3 = os.path.join(action_dir, f'{snake}.action')
if os.path.exists(candidate3):
return candidate3
# 4) Case-insensitive lookup
found = find_file_case_insensitive(action_dir, f'{base}.action')
if found:
return found
# Not found
return None
def make_placeholder_for_type(ros_type: str) -> object:
"""Return a default placeholder Python value for a ROS field type.
- Primitive -> default numeric/bool/string
- Primitive[] -> empty list
- geometry_msgs/PoseStamped -> nested dict with common keys
- geometry_msgs/TwistStamped -> nested dict with common keys
- Otherwise -> None
"""
is_array = ros_type.endswith('[]')
base = ros_type[:-2] if is_array else ros_type
# normalize namespace
base_norm = base.replace('::', '/')
base_norm = base_norm.replace('/msg/', '/').replace('/Msg/', '/')
if is_array:
return []
if '/' not in base_norm:
# primitive
return PRIMITIVES_DEFAULTS.get(base_norm, None)
if base_norm in ("geometry_msgs/PoseStamped",):
return {
'frame_id': '',
'stamp_sec': 0,
'stamp_nanosec': 0,
'position': {'x': 0.0, 'y': 0.0, 'z': 0.0},
'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0},
}
if base_norm in ("geometry_msgs/TwistStamped",):
return {
'frame_id': '',
'stamp_sec': 0,
'stamp_nanosec': 0,
'linear': {'x': 0.0, 'y': 0.0, 'z': 0.0},
'angular': {'x': 0.0, 'y': 0.0, 'z': 0.0},
}
# unknown complex -> None
return None
def build_yaml_params_from_fields(fields: Sequence[Tuple[str, str]]) -> str:
"""Create a YAML snippet string from .action Goal fields."""
mapping: Dict[str, object] = {}
for ros_type, name in fields:
mapping[name] = make_placeholder_for_type(ros_type)
# Dump as YAML block string
# Ensure multi-line by setting default_flow_style=False
snippet = yaml.safe_dump(mapping, allow_unicode=True, sort_keys=False)
# Guarantee trailing newline for nicer block scalars
if not snippet.endswith('\n'):
snippet += '\n'
return snippet
def collect_bt_nodes(xml_path: str) -> List[Tuple[str, str]]:
"""Return list of (instance_name, node_tag) in source order for elements with a 'name' attr.
Heuristic: We primarily care about leaf actions registered as <SkillName>_H. To be inclusive,
we collect any XML element with a 'name' attribute that is not a container like Sequence,
Fallback, RetryUntilSuccessful, etc. Containers typically don't have a 'name' in our trees,
and their tags are known; but we simply include all with 'name' and let type resolution
decide (missing interface files will yield empty params).
"""
tree = ET.parse(xml_path)
root = tree.getroot()
out: List[Tuple[str, str]] = []
for elem in root.iter():
name = elem.attrib.get('name')
if not name:
continue
tag = elem.tag
out.append((name, tag))
return out
def main():
ap = argparse.ArgumentParser(description='Generate YAML params for BT nodes.')
ap.add_argument('--xml', default='src/brain/config/bt_carry_boxes.xml', help='Path to BehaviorTree XML')
ap.add_argument('--interfaces', default='../hivecore_robot_interfaces', help='Path to hivecore_robot_interfaces root')
ap.add_argument('--out', default='src/brain/config/bt_carry_boxes.params.default.yaml', help='Output YAML path')
args = ap.parse_args()
# Resolve repo root as two levels up from this script (src/scripts/.. -> repo)
repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
xml_path = args.xml if os.path.isabs(args.xml) else os.path.join(repo_root, args.xml)
interfaces_root = args.interfaces if os.path.isabs(args.interfaces) else os.path.join(repo_root, args.interfaces)
out_path = args.out if os.path.isabs(args.out) else os.path.join(repo_root, args.out)
if not os.path.exists(xml_path):
print(f'ERROR: XML not found: {xml_path}', file=sys.stderr)
sys.exit(2)
nodes = collect_bt_nodes(xml_path)
results = []
for instance_name, node_tag in nodes:
# Map tag to interface base by stripping trailing '_H' if present
base = node_tag[:-2] if node_tag.endswith('_H') else node_tag
action_file = locate_action_file(interfaces_root, base)
fields: List[Tuple[str, str]] = parse_action_file(action_file) if action_file else []
yaml_params = build_yaml_params_from_fields(fields)
results.append({'name': instance_name, 'params': yaml_params})
# Write output as YAML list; ensure deterministic order as in XML
os.makedirs(os.path.dirname(out_path), exist_ok=True)
with open(out_path, 'w', encoding='utf-8') as f:
yaml.safe_dump(results, f, allow_unicode=True, sort_keys=False)
print(f'Wrote {len(results)} entries to {out_path}')
if __name__ == '__main__':
main()