diff --git a/src/brain/config/robot_config.yaml b/src/brain/config/robot_config.yaml index f853b7c..05ba070 100644 --- a/src/brain/config/robot_config.yaml +++ b/src/brain/config/robot_config.yaml @@ -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 \ No newline at end of file diff --git a/src/brain/include/brain/cerebellum_node.hpp b/src/brain/include/brain/cerebellum_node.hpp index 09ceb8f..081a8db 100644 --- a/src/brain/include/brain/cerebellum_node.hpp +++ b/src/brain/include/brain/cerebellum_node.hpp @@ -19,6 +19,7 @@ #include "brain/sm_cerebellum.hpp" // for CerebellumData::ExecResult // For from_yaml(YAML::Node) #include "brain/payload_converters.hpp" +#include "brain/robot_config.hpp" namespace brain { @@ -46,6 +47,9 @@ private: std::unique_ptr action_clients_; std::unique_ptr skill_manager_; std::unique_ptr bt_registry_; + std::unique_ptr robot_config_; + std::string share_directory_; + std::string robot_skill_file_path_; // SMACC2 execution context std::unique_ptr sm_exec_; diff --git a/src/brain/include/brain/cerebrum_node.hpp b/src/brain/include/brain/cerebrum_node.hpp index 94f64f3..c35e9f2 100644 --- a/src/brain/include/brain/cerebrum_node.hpp +++ b/src/brain/include/brain/cerebrum_node.hpp @@ -20,6 +20,7 @@ #include #include "brain_interfaces/msg/skill_call.hpp" #include +#include "brain/robot_config.hpp" namespace brain @@ -193,6 +194,7 @@ private: std::unique_ptr action_registry_; std::unique_ptr bt_registry_; std::unique_ptr skill_manager_; + std::unique_ptr 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 single_skill_goal_override_; diff --git a/src/brain/include/brain/robot_config.hpp b/src/brain/include/brain/robot_config.hpp new file mode 100644 index 0000000..2c142ae --- /dev/null +++ b/src/brain/include/brain/robot_config.hpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +namespace brain { namespace robot_config { + +// Represents a single entry parsed from robot_config.yaml. +struct Entry { + std::string name; + std::unordered_map 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(); } + for (auto it = node.begin(); it != node.end(); ++it) { + const auto key = it->first.as(); + if (key == "name") { continue; } + if (it->second) { + if (it->second.IsScalar()) { + e.fields.emplace(key, it->second.as()); + } 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 Names() const { + std::vector 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 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 Version(const std::string& name) const { + return GetValue(name, "version"); + } + + std::optional SkillFile(const std::string& name) const { + return GetValue(name, "skill_file"); + } + + std::optional BtConfigFile(const std::string& name) const { + return GetValue(name, "bt_config_file"); + } + + std::optional BtParamsFile(const std::string& name) const { + return GetValue(name, "bt_params_file"); + } + + private: + std::vector entries_; +}; + +}} // namespace robot_config; } // namespace brain + +#endif // BRAIN_ROBOT_CONFIG_HPP_ diff --git a/src/brain/src/cerebellum_node.cpp b/src/brain/src/cerebellum_node.cpp index 6af2d7d..eae59ac 100644 --- a/src/brain/src/cerebellum_node.cpp +++ b/src/brain/src/cerebellum_node.cpp @@ -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(); + 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(this->get_clock()); tf_buffer_->setUsingDedicatedThread(true); tf_listener_ = std::make_shared(*tf_buffer_); - RCLCPP_INFO(this->get_logger(), "Cerebellum node started"); action_registry_ = std::make_unique(this); action_clients_ = std::make_unique(this); skill_manager_ = std::make_unique(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()) { diff --git a/src/brain/src/cerebrum_node.cpp b/src/brain/src/cerebrum_node.cpp index 59cd790..2a53f31 100644 --- a/src/brain/src/cerebrum_node.cpp +++ b/src/brain/src/cerebrum_node.cpp @@ -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(this); bt_registry_ = std::make_unique(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(); + 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 diff --git a/src/scripts/gen_robot_config.py b/src/scripts/gen_robot_config.py new file mode 100644 index 0000000..4c55eb7 --- /dev/null +++ b/src/scripts/gen_robot_config.py @@ -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 Names() const + * std::optional 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 \n") + out.write("#include \n") + out.write("#include \n") + out.write("#include \n") + out.write("#include \n") + out.write("#include \n") + out.write("#include \n") + out.write("#include \n") + out.write("#include \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 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(); }\n") + out.write(" for (auto it = node.begin(); it != node.end(); ++it) {\n") + out.write(" const auto key = it->first.as();\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());\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 Names() const {\n") + out.write(" std::vector 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 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 {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 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() +