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

add robot configuration

Browse Source
This commit is contained in:
david
2025-10-20 19:28:44 +08:00
parent 0269009987
commit c5fd2d059c
7 changed files with 356 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/brain/config/robot_config.yaml
View File

@@ -1,11 +1,11 @@
- name: brain
version: 1.0.0
skill_file: robot_skills.yaml
skill_file: "/config/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
bt_config_file: "/config/bt_carry_boxes.xml"
bt_params_file: "/config/bt_carry_boxes.params.yaml"
- name: cerebellum_node
version: 1.0.0

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

@@ -19,6 +19,7 @@
#include "brain/sm_cerebellum.hpp" // for CerebellumData::ExecResult
// For from_yaml<T>(YAML::Node)
#include "brain/payload_converters.hpp"
#include "brain/robot_config.hpp"
namespace brain
{
@@ -46,6 +47,9 @@ private:
std::unique_ptr<ActionClientRegistry> action_clients_;
std::unique_ptr<SkillManager> skill_manager_;
std::unique_ptr<BTRegistry> bt_registry_;
std::unique_ptr<brain::robot_config::RobotConfig> robot_config_;
std::string share_directory_;
std::string robot_skill_file_path_;
// SMACC2 execution context
std::unique_ptr<smacc2::SmExecution> sm_exec_;

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

@@ -20,6 +20,7 @@
#include <std_srvs/srv/trigger.hpp>
#include "brain_interfaces/msg/skill_call.hpp"
#include <rclcpp/node_interfaces/node_parameters_interface.hpp>
#include "brain/robot_config.hpp"
namespace brain
@@ -193,6 +194,7 @@ private:
std::unique_ptr<ActionClientRegistry> action_registry_;
std::unique_ptr<BTRegistry> bt_registry_;
std::unique_ptr<SkillManager> skill_manager_;
std::unique_ptr<brain::robot_config::RobotConfig> robot_config_;
std::string active_sequence_;
std::string share_directory_;
std::string robot_skill_file_path_;
@@ -321,6 +323,12 @@ private:
*/
void RegisterSkillBtActions();
/**
* @brief Load robot configuration from file.
* @return True on success.
*/
bool LoadRobotConfiguration();
// 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_;

99
src/brain/include/brain/robot_config.hpp Normal file
View File

@@ -0,0 +1,99 @@
// GENERATED FILE - do not edit by hand
// Generated by src/scripts/gen_robot_config.py
#ifndef BRAIN_ROBOT_CONFIG_HPP_
#define BRAIN_ROBOT_CONFIG_HPP_
#include <yaml-cpp/yaml.h>
#include <cstddef>
#include <optional>
#include <string>
#include <string_view>
#include <unordered_map>
#include <utility>
#include <vector>
#include <ament_index_cpp/get_package_share_directory.hpp>
namespace brain { namespace robot_config {
// Represents a single entry parsed from robot_config.yaml.
struct Entry {
std::string name;
std::unordered_map<std::string, std::string> fields;
};
// Loader and query interface for robot configuration.
class RobotConfig {
public:
RobotConfig() {
LoadFromFile(ament_index_cpp::get_package_share_directory("brain") + std::string("/config/robot_config.yaml"));
}
// Parse YAML from a file path. Returns true on success.
bool LoadFromFile(const std::string& path) {
entries_.clear();
YAML::Node root = YAML::LoadFile(path);
if (!root || !root.IsSequence()) { return false; }
for (const auto& node : root) {
if (!node.IsMap()) { continue; }
Entry e;
auto name_it = node["name"];
if (name_it && name_it.IsScalar()) { e.name = name_it.as<std::string>(); }
for (auto it = node.begin(); it != node.end(); ++it) {
const auto key = it->first.as<std::string>();
if (key == "name") { continue; }
if (it->second) {
if (it->second.IsScalar()) {
e.fields.emplace(key, it->second.as<std::string>());
} else {
e.fields.emplace(key, YAML::Dump(it->second));
}
}
}
if (!e.name.empty()) { entries_.emplace_back(std::move(e)); }
}
return true;
}
// Return pointer to an Entry with matching name or nullptr if not found.
const Entry* Find(const std::string& name) const {
for (const auto& e : entries_) { if (e.name == name) return &e; }
return nullptr;
}
// List all entry names.
std::vector<std::string> Names() const {
std::vector<std::string> out; out.reserve(entries_.size());
for (const auto& e : entries_) out.push_back(e.name);
return out;
}
// Generic value getter by name and key.
std::optional<std::string> GetValue(const std::string& name, const std::string& key) const {
const Entry* e = Find(name); if (!e) return std::nullopt;
auto it = e->fields.find(key);
if (it == e->fields.end()) return std::nullopt;
return it->second;
}
std::optional<std::string> Version(const std::string& name) const {
return GetValue(name, "version");
}
std::optional<std::string> SkillFile(const std::string& name) const {
return GetValue(name, "skill_file");
}
std::optional<std::string> BtConfigFile(const std::string& name) const {
return GetValue(name, "bt_config_file");
}
std::optional<std::string> BtParamsFile(const std::string& name) const {
return GetValue(name, "bt_params_file");
}
private:
std::vector<Entry> entries_;
};
}} // namespace robot_config; } // namespace brain
#endif // BRAIN_ROBOT_CONFIG_HPP_

36
src/brain/src/cerebellum_node.cpp
View File

@@ -64,13 +64,24 @@ static thread_local int tls_arm_body_id = -1;
* @param options Optional ROS 2 node options.
*/
CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
: Node("cerebellum_node", options)
: Node("cerebellum_node", options),
share_directory_(ament_index_cpp::get_package_share_directory("brain"))
{
RCLCPP_INFO(this->get_logger(), "Cerebellum node started");
robot_config_ = std::make_unique<brain::robot_config::RobotConfig>();
auto skill_file = robot_config_->SkillFile("brain");
if (skill_file == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "No skill_file entry found for 'brain' in robot_config.yaml");
return;
}
robot_skill_file_path_ = share_directory_ + *skill_file;
RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_buffer_->setUsingDedicatedThread(true);
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
RCLCPP_INFO(this->get_logger(), "Cerebellum node started");
action_registry_ = std::make_unique<ActionServerRegistry>(this);
action_clients_ = std::make_unique<ActionClientRegistry>(this);
skill_manager_ = std::make_unique<SkillManager>(this, action_clients_.get(), nullptr);
@@ -774,25 +785,10 @@ void CerebellumNode::DeclareAndLoadParameters()
*/
void CerebellumNode::LoadSkillsFile()
{
std::string skills_file;
this->get_parameter("skills_file", skills_file);
if (skills_file.empty()) {
try {
const auto share = ament_index_cpp::get_package_share_directory("brain");
skills_file = share + std::string("/config/robot_skills.yaml");
RCLCPP_INFO(
this->get_logger(), "[cerebellum_node] Using default skills file: %s",
skills_file.c_str());
} catch (const std::exception & e) {
RCLCPP_WARN(
this->get_logger(), "[cerebellum_node] Could not locate default skills file: %s",
e.what());
}
}
if (skills_file.empty()) {return;}
if (!skill_manager_->load_from_file(skills_file)) {
if (robot_skill_file_path_.empty()) {return;}
if (!skill_manager_->load_from_file(robot_skill_file_path_)) {
RCLCPP_WARN(
this->get_logger(), "[cerebellum_node] Failed to load skills from %s", skills_file.c_str());
this->get_logger(), "[cerebellum_node] Failed to load skills from %s", robot_skill_file_path_.c_str());
return;
}
for (const auto & kv : skill_manager_->skills()) {

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

@@ -83,11 +83,15 @@ struct UpdatingFlagGuard
*/
CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
: rclcpp::Node("cerebrum_node", options),
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"))
share_directory_(ament_index_cpp::get_package_share_directory("brain"))
{
RCLCPP_INFO(this->get_logger(), "Cerebrum node started");
if (!LoadRobotConfiguration()) {
RCLCPP_ERROR(this->get_logger(), "Failed to load robot configuration");
return;
}
// Initialize
action_registry_ = std::make_unique<ActionClientRegistry>(this);
bt_registry_ = std::make_unique<BTRegistry>(factory_, this);
@@ -987,4 +991,40 @@ void CerebrumNode::CreateServices()
});
}
/**
* @brief Load the robot configuration file.
*
* @return true
* @return false
*/
bool CerebrumNode::LoadRobotConfiguration()
{
robot_config_ = std::make_unique<brain::robot_config::RobotConfig>();
auto skill_file = robot_config_->SkillFile("brain");
if (skill_file == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "No skill_file entry found for 'brain' in robot_config.yaml");
return false;
}
robot_skill_file_path_ = share_directory_ + *skill_file;
RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
auto bt_config_file = robot_config_->BtConfigFile("cerebrum_node");
if (bt_config_file == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "No bt_config_file entry found for 'cerebrum_node' in robot_config.yaml");
return false;
}
bt_config_file_path_ = share_directory_ + *bt_config_file;
RCLCPP_WARN(this->get_logger(), "bt config file %s", bt_config_file_path_.c_str());
auto bt_params_file = robot_config_->BtParamsFile("cerebrum_node");
if (bt_params_file == std::nullopt) {
RCLCPP_ERROR(this->get_logger(), "No bt_params_file entry found for 'cerebrum_node' in robot_config.yaml");
return false;
}
bt_params_file_path_ = share_directory_ + *bt_params_file;
RCLCPP_WARN(this->get_logger(), "bt params file %s", bt_params_file_path_.c_str());
return true;
}
} // namespace brain

182
src/scripts/gen_robot_config.py Normal file
View File

@@ -0,0 +1,182 @@
#!/usr/bin/env python3
"""
Generate a C++ header `robot_config.hpp` from YAML `robot_config.yaml`.
Requirements:
- The generated header must parse the YAML file at runtime (do not embed YAML values).
- Provide a clear C++ interface to query configuration by name and key.
- Follow Google C++ style (CamelCase types/functions, namespace lower_case, member_ with trailing underscore, include guards).
YAML example:
- 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
The header exposes:
- brain::robot_config::Entry: holds a name and a map of string keys->string values.
- brain::robot_config::RobotConfig: loader and query API.
* bool LoadFromFile(const std::string& path)
* const Entry* Find(const std::string& name) const
* std::vector<std::string> Names() const
* std::optional<std::string> GetValue(const std::string& name, const std::string& key) const
* Typed helpers for all keys present in YAML (CamelCase)
Usage:
python3 src/scripts/gen_robot_config.py \
--in src/brain/config/robot_config.yaml \
--out src/brain/include/robot_config.hpp
"""
from __future__ import annotations
import argparse
import os
from pathlib import Path
from typing import List
import yaml
def to_camel(s: str) -> str:
import re
parts = re.split(r"[^A-Za-z0-9]+", s)
parts = [p for p in parts if p]
camel = "".join(p[:1].upper() + p[1:] for p in parts)
if not camel or not camel[0].isalpha():
camel = "X" + camel
return camel
def main() -> None:
parser = argparse.ArgumentParser(description="Generate robot_config.hpp from YAML")
parser.add_argument("--in", dest="inp", default="src/brain/config/robot_config.yaml")
parser.add_argument("--out", dest="out", default="src/brain/include/brain/robot_config.hpp")
args = parser.parse_args()
repo_root = Path(__file__).resolve().parents[2]
in_path = Path(args.inp) if os.path.isabs(args.inp) else repo_root / args.inp
out_path = Path(args.out) if os.path.isabs(args.out) else repo_root / args.out
if not in_path.exists():
raise SystemExit(f"Config YAML not found: {in_path}")
data = yaml.safe_load(in_path.read_text(encoding="utf-8")) or []
if not isinstance(data, list):
raise SystemExit("robot_config.yaml must be a YAML sequence (list)")
# Collect all keys across items (ensure 'name' exists first)
key_set = {"name"}
preferred_order = ["version", "skill_file", "bt_config_file", "bt_params_file"]
for item in data:
if isinstance(item, dict):
for k in item.keys():
key_set.add(k)
# Build ordered keys: name, preferred if present, then remaining alphabetically
keys: List[str] = ["name"]
for k in preferred_order:
if k in key_set and k not in keys:
keys.append(k)
keys.extend(sorted(k for k in key_set if k not in keys))
out_path.parent.mkdir(parents=True, exist_ok=True)
with out_path.open("w", encoding="utf-8") as out:
out.write("// GENERATED FILE - do not edit by hand\n")
out.write("// Generated by src/scripts/gen_robot_config.py\n")
out.write("#ifndef BRAIN_ROBOT_CONFIG_HPP_\n")
out.write("#define BRAIN_ROBOT_CONFIG_HPP_\n\n")
out.write("#include <yaml-cpp/yaml.h>\n")
out.write("#include <cstddef>\n")
out.write("#include <optional>\n")
out.write("#include <string>\n")
out.write("#include <string_view>\n")
out.write("#include <unordered_map>\n")
out.write("#include <utility>\n")
out.write("#include <vector>\n")
out.write("#include <ament_index_cpp/get_package_share_directory.hpp>\n\n")
out.write("namespace brain { namespace robot_config {\n\n")
out.write("// Represents a single entry parsed from robot_config.yaml.\n")
out.write("struct Entry {\n")
out.write(" std::string name;\n")
out.write(" std::unordered_map<std::string, std::string> fields;\n")
out.write("};\n\n")
out.write("// Loader and query interface for robot configuration.\n")
out.write("class RobotConfig {\n")
out.write(" public:\n")
out.write(" RobotConfig() {\n")
out.write(
" LoadFromFile(ament_index_cpp::get_package_share_directory(\"brain\") + std::string(\"/config/robot_config.yaml\"));\n"
)
out.write(" }\n")
out.write(" // Parse YAML from a file path. Returns true on success.\n")
out.write(" bool LoadFromFile(const std::string& path) {\n")
out.write(" entries_.clear();\n")
out.write(" YAML::Node root = YAML::LoadFile(path);\n")
out.write(" if (!root || !root.IsSequence()) { return false; }\n")
out.write(" for (const auto& node : root) {\n")
out.write(" if (!node.IsMap()) { continue; }\n")
out.write(" Entry e;\n")
out.write(" auto name_it = node[\"name\"];\n")
out.write(" if (name_it && name_it.IsScalar()) { e.name = name_it.as<std::string>(); }\n")
out.write(" for (auto it = node.begin(); it != node.end(); ++it) {\n")
out.write(" const auto key = it->first.as<std::string>();\n")
out.write(" if (key == \"name\") { continue; }\n")
out.write(" if (it->second) {\n")
out.write(" if (it->second.IsScalar()) {\n")
out.write(" e.fields.emplace(key, it->second.as<std::string>());\n")
out.write(" } else {\n")
out.write(" e.fields.emplace(key, YAML::Dump(it->second));\n")
out.write(" }\n")
out.write(" }\n")
out.write(" }\n")
out.write(" if (!e.name.empty()) { entries_.emplace_back(std::move(e)); }\n")
out.write(" }\n")
out.write(" return true;\n")
out.write(" }\n\n")
out.write(" // Return pointer to an Entry with matching name or nullptr if not found.\n")
out.write(" const Entry* Find(const std::string& name) const {\n")
out.write(" for (const auto& e : entries_) { if (e.name == name) return &e; }\n")
out.write(" return nullptr;\n")
out.write(" }\n\n")
out.write(" // List all entry names.\n")
out.write(" std::vector<std::string> Names() const {\n")
out.write(" std::vector<std::string> out; out.reserve(entries_.size());\n")
out.write(" for (const auto& e : entries_) out.push_back(e.name);\n")
out.write(" return out;\n")
out.write(" }\n\n")
out.write(" // Generic value getter by name and key.\n")
out.write(
" std::optional<std::string> GetValue(const std::string& name, const std::string& key) const {\n"
)
out.write(" const Entry* e = Find(name); if (!e) return std::nullopt;\n")
out.write(" auto it = e->fields.find(key);\n")
out.write(" if (it == e->fields.end()) return std::nullopt;\n")
out.write(" return it->second;\n")
out.write(" }\n\n")
# Typed getters for all keys except 'name'
for k in keys:
if k == "name":
continue
func = to_camel(k)
out.write(
f" std::optional<std::string> {func}(const std::string& name) const " + "{\n"
)
out.write(f" return GetValue(name, \"{k}\");\n")
out.write(" }\n\n")
out.write(" private:\n")
out.write(" std::vector<Entry> entries_;\n")
out.write("};\n\n")
out.write("}} // namespace robot_config; } // namespace brain\n\n")
out.write("#endif // BRAIN_ROBOT_CONFIG_HPP_\n")
print(f"Wrote {out_path}")
if __name__ == "__main__":
main()