From 34a629bbfb537fd55dfde209fe27c582266c1b35 Mon Sep 17 00:00:00 2001 From: david Date: Mon, 20 Oct 2025 11:08:44 +0800 Subject: [PATCH] add gen_payload_converters.py --- .../include/brain/payload_converters.hpp | 251 +++++----- src/brain/src/cerebellum_node.cpp | 9 +- src/brain/src/cerebrum_node.cpp | 13 + .../arm_control/src/arm_control_node.cpp | 5 +- src/scripts/gen_payload_converters.py | 459 ++++++++++++++++++ 5 files changed, 616 insertions(+), 121 deletions(-) create mode 100644 src/scripts/gen_payload_converters.py diff --git a/src/brain/include/brain/payload_converters.hpp b/src/brain/include/brain/payload_converters.hpp index 67fbaaf..b378b02 100644 --- a/src/brain/include/brain/payload_converters.hpp +++ b/src/brain/include/brain/payload_converters.hpp @@ -1,28 +1,113 @@ +// GENERATED FILE - do not edit by hand +// Generated by src/scripts/gen_payload_converters.py #pragma once #include #include -#include "interfaces/action/move_waist.hpp" -#include "interfaces/action/move_leg.hpp" -#include "interfaces/action/move_wheel.hpp" -#include "interfaces/action/hand_control.hpp" +#include +#include #include "interfaces/action/arm.hpp" -#include "interfaces/action/slam_mode.hpp" -#include "interfaces/action/camera_take_photo.hpp" #include "interfaces/action/arm_space_control.hpp" +#include "interfaces/action/camera_take_photo.hpp" +#include "interfaces/action/hand_control.hpp" #include "interfaces/action/leg_control.hpp" #include "interfaces/action/move_home.hpp" +#include "interfaces/action/move_leg.hpp" #include "interfaces/action/move_to_position.hpp" +#include "interfaces/action/move_waist.hpp" +#include "interfaces/action/move_wheel.hpp" +#include "interfaces/action/slam_mode.hpp" #include "interfaces/action/vision_grasp_object.hpp" -#include "interfaces/srv/vision_object_recognition.hpp" #include "interfaces/srv/map_load.hpp" #include "interfaces/srv/map_save.hpp" +#include "interfaces/srv/vision_object_recognition.hpp" + namespace brain { template T from_yaml(const YAML::Node & n); +// VisionObjectRecognition::Request: camera_position +template<> +inline interfaces::srv::VisionObjectRecognition::Request from_yaml(const YAML::Node & n) +{ + interfaces::srv::VisionObjectRecognition::Request r; + if (n && n.IsMap()) { + if (n["camera_position"]) r.camera_position = n["camera_position"].as(); + } + return r; +} + +// MapLoad::Request: map_url +template<> +inline interfaces::srv::MapLoad::Request from_yaml(const YAML::Node & n) +{ + interfaces::srv::MapLoad::Request r; + if (n && n.IsMap()) { + if (n["map_url"]) r.map_url = n["map_url"].as(); + } + return r; +} + +// MapSave::Request: map_url +template<> +inline interfaces::srv::MapSave::Request from_yaml(const YAML::Node & n) +{ + interfaces::srv::MapSave::Request r; + if (n && n.IsMap()) { + if (n["map_url"]) r.map_url = n["map_url"].as(); + } + return r; +} + +// Arm::Goal: body_id, data_type, data_length, command_id, frame_time_stamp, data_array +template<> +inline interfaces::action::Arm::Goal from_yaml(const YAML::Node & n) +{ + interfaces::action::Arm::Goal g; + if (n && n.IsMap()) { + if (n["body_id"]) g.body_id = n["body_id"].as(); + if (n["data_type"]) g.data_type = n["data_type"].as(); + if (n["data_length"]) g.data_length = n["data_length"].as(); + if (n["command_id"]) g.command_id = n["command_id"].as(); + if (n["frame_time_stamp"]) g.frame_time_stamp = n["frame_time_stamp"].as(); + if (n["data_array"]) { + auto vec = n["data_array"].as>(); + g.data_array.assign(vec.begin(), vec.end()); + } + if (!g.data_array.empty()) { + using DL = decltype(g.data_length); + g.data_length = static_cast
(g.data_array.size()); + } + } + return g; +} + +// CameraTakePhoto::Goal: mode, effort +template<> +inline interfaces::action::CameraTakePhoto::Goal from_yaml(const YAML::Node & n) +{ + interfaces::action::CameraTakePhoto::Goal g; + if (n && n.IsMap()) { + if (n["mode"]) g.mode = n["mode"].as(); + if (n["effort"]) g.effort = n["effort"].as(); + } + return g; +} + +// HandControl::Goal: mode, effort +template<> +inline interfaces::action::HandControl::Goal from_yaml(const YAML::Node & n) +{ + interfaces::action::HandControl::Goal g; + if (n && n.IsMap()) { + if (n["mode"]) g.mode = n["mode"].as(); + if (n["effort"]) g.effort = n["effort"].as(); + } + return g; +} + // MoveWaist::Goal: move_pitch_degree, move_yaw_degree template<> inline interfaces::action::MoveWaist::Goal from_yaml(const YAML::Node & n) @@ -58,33 +143,24 @@ inline interfaces::action::MoveWheel::Goal from_yaml -inline interfaces::action::HandControl::Goal from_yaml(const YAML::Node & n) +inline interfaces::action::MoveHome::Goal from_yaml(const YAML::Node & n) { - interfaces::action::HandControl::Goal g; + interfaces::action::MoveHome::Goal g; if (n && n.IsMap()) { - if (n["mode"]) g.mode = n["mode"].as(); } return g; } -// Arm::Goal: body_id, data_type, command_id, data_array, frame_time_stamp, data_length +// MoveToPosition::Goal: target_x, target_y template<> -inline interfaces::action::Arm::Goal from_yaml(const YAML::Node & n) +inline interfaces::action::MoveToPosition::Goal from_yaml(const YAML::Node & n) { - interfaces::action::Arm::Goal g; + interfaces::action::MoveToPosition::Goal g; if (n && n.IsMap()) { - if (n["body_id"]) g.body_id = n["body_id"].as(); - if (n["data_type"]) g.data_type = n["data_type"].as(); - if (n["command_id"]) g.command_id = n["command_id"].as(); - if (n["frame_time_stamp"]) g.frame_time_stamp = n["frame_time_stamp"].as(); - if (n["data_array"]) { - auto vec = n["data_array"].as>(); - g.data_array.assign(vec.begin(), vec.end()); - g.data_length = static_cast(g.data_array.size()); - } - if (n["data_length"]) g.data_length = n["data_length"].as(); + if (n["target_x"]) g.target_x = n["target_x"].as(); + if (n["target_y"]) g.target_y = n["target_y"].as(); } return g; } @@ -100,83 +176,55 @@ inline interfaces::action::SlamMode::Goal from_yaml -inline interfaces::action::CameraTakePhoto::Goal from_yaml(const YAML::Node & n) -{ - interfaces::action::CameraTakePhoto::Goal g; - if (n && n.IsMap()) { - if (n["mode"]) g.mode = n["mode"].as(); - if (n["effort"]) g.effort = n["effort"].as(); - } - return g; -} - -// ArmSpaceControl::Goal: target (geometry_msgs::msg::PoseStamped) +// ArmSpaceControl::Goal: target template<> inline interfaces::action::ArmSpaceControl::Goal from_yaml(const YAML::Node & n) { interfaces::action::ArmSpaceControl::Goal g; - // Only support position/orientation fields in a simple map if provided if (n && n.IsMap()) { - auto & pose = g.target; - if (n["frame_id"]) pose.header.frame_id = n["frame_id"].as(); - if (n["stamp_sec"]) pose.header.stamp.sec = n["stamp_sec"].as(); - if (n["stamp_nanosec"]) pose.header.stamp.nanosec = n["stamp_nanosec"].as(); - if (auto p = n["position"]) { - if (p["x"]) pose.pose.position.x = p["x"].as(); - if (p["y"]) pose.pose.position.y = p["y"].as(); - if (p["z"]) pose.pose.position.z = p["z"].as(); - } - if (auto q = n["orientation"]) { - if (q["x"]) pose.pose.orientation.x = q["x"].as(); - if (q["y"]) pose.pose.orientation.y = q["y"].as(); - if (q["z"]) pose.pose.orientation.z = q["z"].as(); - if (q["w"]) pose.pose.orientation.w = q["w"].as(); + // geometry_msgs::msg::PoseStamped + if (n) { + if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as(); + if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as(); + if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as(); + if (auto p = n["position"]) { + if (p["x"]) g.target.pose.position.x = p["x"].as(); + if (p["y"]) g.target.pose.position.y = p["y"].as(); + if (p["z"]) g.target.pose.position.z = p["z"].as(); + } + if (auto q = n["orientation"]) { + if (q["x"]) g.target.pose.orientation.x = q["x"].as(); + if (q["y"]) g.target.pose.orientation.y = q["y"].as(); + if (q["z"]) g.target.pose.orientation.z = q["z"].as(); + if (q["w"]) g.target.pose.orientation.w = q["w"].as(); + } } } return g; } -// LegControl::Goal: target (geometry_msgs::msg::TwistStamped) +// LegControl::Goal: target template<> inline interfaces::action::LegControl::Goal from_yaml(const YAML::Node & n) { interfaces::action::LegControl::Goal g; if (n && n.IsMap()) { - auto & t = g.target; - if (n["frame_id"]) t.header.frame_id = n["frame_id"].as(); - if (n["stamp_sec"]) t.header.stamp.sec = n["stamp_sec"].as(); - if (n["stamp_nanosec"]) t.header.stamp.nanosec = n["stamp_nanosec"].as(); - if (auto lin = n["linear"]) { - if (lin["x"]) t.twist.linear.x = lin["x"].as(); - if (lin["y"]) t.twist.linear.y = lin["y"].as(); - if (lin["z"]) t.twist.linear.z = lin["z"].as(); + // geometry_msgs::msg::TwistStamped + if (n) { + if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as(); + if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as(); + if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as(); + if (auto lin = n["linear"]) { + if (lin["x"]) g.target.twist.linear.x = lin["x"].as(); + if (lin["y"]) g.target.twist.linear.y = lin["y"].as(); + if (lin["z"]) g.target.twist.linear.z = lin["z"].as(); + } + if (auto ang = n["angular"]) { + if (ang["x"]) g.target.twist.angular.x = ang["x"].as(); + if (ang["y"]) g.target.twist.angular.y = ang["y"].as(); + if (ang["z"]) g.target.twist.angular.z = ang["z"].as(); + } } - if (auto ang = n["angular"]) { - if (ang["x"]) t.twist.angular.x = ang["x"].as(); - if (ang["y"]) t.twist.angular.y = ang["y"].as(); - if (ang["z"]) t.twist.angular.z = ang["z"].as(); - } - } - return g; -} - -// MoveHome::Goal: empty; accept any map and ignore -template<> -inline interfaces::action::MoveHome::Goal from_yaml(const YAML::Node & n) -{ - (void)n; return interfaces::action::MoveHome::Goal{}; -} - -// MoveToPosition::Goal: target_x, target_y -template<> -inline interfaces::action::MoveToPosition::Goal from_yaml(const YAML::Node & n) -{ - interfaces::action::MoveToPosition::Goal g; - if (n && n.IsMap()) { - if (n["target_x"]) g.target_x = n["target_x"].as(); - if (n["target_y"]) g.target_y = n["target_y"].as(); } return g; } @@ -192,37 +240,4 @@ inline interfaces::action::VisionGraspObject::Goal from_yaml -inline interfaces::srv::MapLoad::Request from_yaml(const YAML::Node & n) -{ - interfaces::srv::MapLoad::Request r; - if (n && n.IsMap()) { - if (n["map_url"]) r.map_url = n["map_url"].as(); - } - return r; -} - -// MapSave::Request: map_url -template<> -inline interfaces::srv::MapSave::Request from_yaml(const YAML::Node & n) -{ - interfaces::srv::MapSave::Request r; - if (n && n.IsMap()) { - if (n["map_url"]) r.map_url = n["map_url"].as(); - } - return r; -} - -// VisionObjectRecognition::Request: camera_position -template<> -inline interfaces::srv::VisionObjectRecognition::Request from_yaml(const YAML::Node & n) -{ - interfaces::srv::VisionObjectRecognition::Request r; - if (n && n.IsMap()) { - if (n["camera_position"]) r.camera_position = n["camera_position"].as(); - } - return r; -} - } // namespace brain diff --git a/src/brain/src/cerebellum_node.cpp b/src/brain/src/cerebellum_node.cpp index 5f40189..6af2d7d 100644 --- a/src/brain/src/cerebellum_node.cpp +++ b/src/brain/src/cerebellum_node.cpp @@ -107,6 +107,13 @@ void CerebellumNode::ConfigureArmHooks() interfaces::action::Arm::Goal goal{}; // Plan A: per-call YAML overrides (void)TryParseCallPayload(skill_name, goal); + + RCLCPP_INFO( + this->get_logger(), "Received Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.1f, %.1f, %.1f, %.1f, %.1f, %.1f, %.1f]", + goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp, + goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]); + + goal.body_id = (tls_arm_body_id == 0 || tls_arm_body_id == 1) ? tls_arm_body_id : 0; // LEFT_ARM=0, RIGHT_ARM=1 goal.data_type = 3; //ARM_COMMAND_TYPE_POSE_DIRECT_MOVE goal.command_id = ++command_id_; @@ -181,7 +188,7 @@ void CerebellumNode::ConfigureArmHooks() const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == 0); const std::string message = res.result ? std::string("action end") : std::string("无返回信息"); if (success) { - RCLCPP_INFO(this->get_logger(), "[%s] 完成: %s", skill_name.c_str(), message.c_str()); + RCLCPP_INFO(this->get_logger(), "[%s] command_id %ld 完成: %s", skill_name.c_str(), res.result->command_id, message.c_str()); } else if (res.code == rclcpp_action::ResultCode::CANCELED) { RCLCPP_WARN(this->get_logger(), "[%s] 被取消", skill_name.c_str()); } else { diff --git a/src/brain/src/cerebrum_node.cpp b/src/brain/src/cerebrum_node.cpp index 443ad3d..cf525e5 100644 --- a/src/brain/src/cerebrum_node.cpp +++ b/src/brain/src/cerebrum_node.cpp @@ -498,6 +498,11 @@ void CerebrumNode::RegisterSkillBtActions() } } +/** + * @brief Declare BT action parameters for a skill + * + * @param skill_name + */ void CerebrumNode::DeclareBtActionParamsForSkill(const std::string & skill_name) { // Declare a generic namespace for each skill to allow configuring routing/topic and payload @@ -527,6 +532,8 @@ void CerebrumNode::DeclareBtActionParamsForSkill(const std::string & skill_name) } 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)); @@ -538,6 +545,12 @@ void CerebrumNode::DeclareBtActionParamsForSkill(const std::string & skill_name) } } +/** + * @brief Build a SkillCall message for a specific skill. + * + * @param skill_name The name of the skill. + * @return brain_interfaces::msg::SkillCall The constructed SkillCall message. + */ brain_interfaces::msg::SkillCall CerebrumNode::BuildSkillCallForSkill(const std::string & skill_name) const { brain_interfaces::msg::SkillCall call; diff --git a/src/modules/motion/arm_control/src/arm_control_node.cpp b/src/modules/motion/arm_control/src/arm_control_node.cpp index ef8f986..2c267f1 100644 --- a/src/modules/motion/arm_control/src/arm_control_node.cpp +++ b/src/modules/motion/arm_control/src/arm_control_node.cpp @@ -103,7 +103,7 @@ private: // Simulate progress from 0 to 100 with small delay; react to cancel requests for (int i = 0; i <= 100; i += 10) { if (goal_handle->is_canceling()) { - result->result = false; + result->result = 1; // result->message = "Arm canceled"; goal_handle->canceled(result); RCLCPP_INFO(this->get_logger(), "Arm goal canceled"); @@ -118,7 +118,8 @@ private: } // Complete successfully - result->result = true; + result->result = 0; + result->command_id = goal->command_id; // result->message = std::string("Arm moved to target in frame '") + // goal->target.header.frame_id + "'"; goal_handle->succeed(result); diff --git a/src/scripts/gen_payload_converters.py b/src/scripts/gen_payload_converters.py new file mode 100644 index 0000000..1914b0b --- /dev/null +++ b/src/scripts/gen_payload_converters.py @@ -0,0 +1,459 @@ +#!/usr/bin/env python3 +""" +Generate a C++ header with YAML -> Goal/Request converters +based on src/brain/config/robot_skills.yaml and a sibling +'hivecore_robot_interfaces' package containing .action/.srv files. + +Output: src/scripts/payload_converters.hpp + +Usage: + python3 gen_payload_converters.py [--skills path/to/robot_skills.yaml] [--interfaces /path/to/hivecore_robot_interfaces] [--out path] + +If arguments omitted the script assumes: + skills: ../src/brain/config/robot_skills.yaml (relative to repo root) + interfaces: ../hivecore_robot_interfaces (sibling folder) + out: src/scripts/payload_converters.hpp + +This generator is conservative: it only auto-generates handling for primitive +and array primitives. For complex ROS message types it will emit a TODO comment. +""" + +import argparse +import os +import re +import sys +import textwrap +import yaml + +PRIMITIVE_MAP = { + 'int8': 'int8_t', + 'int16': 'int16_t', + 'float32': 'float', + 'float64': 'double', + 'float': 'double', + 'double': 'double', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'int': 'int32_t', + 'string': 'std::string', + 'bool': 'bool', + 'time': 'rclcpp::Time', +} + +HEADER_PREAMBLE = '''// GENERATED FILE - do not edit by hand +// Generated by src/scripts/gen_payload_converters.py +#pragma once + +#include +#include +#include +#include +''' + +NAMESPACE_BEGIN = ''' +namespace brain { + +template +T from_yaml(const YAML::Node & n); + +''' + +HEADER_POSTAMBLE = '\n} // namespace brain\n' + + +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 parse_action_file(path: str): + """Parse an .action file, return dict with goal_fields: list of (type,name) + """ + with open(path, 'r', encoding='utf-8') as f: + content = f.read() + parts = content.split('\n---\n') + goal_part = parts[0] if parts else '' + fields = [] + for line in goal_part.splitlines(): + line = line.strip() + if not line or line.startswith('#'): + continue + # lines like: float32 move_pitch_degree + 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 parse_srv_file(path: str): + # request before --- + with open(path, 'r', encoding='utf-8') as f: + content = f.read() + parts = content.split('\n---\n') + req_part = parts[0] if parts else '' + fields = [] + for line in req_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 find_file_case_insensitive(dirname: str, filename: str): + """Return the path to a file in dirname matching filename case-insensitively, or None.""" + 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 make_cpp_type(ros_type: str): + # arrays: float32[] + is_array = ros_type.endswith('[]') + base = ros_type[:-2] if is_array else ros_type + if '/' in base: + # complex message type + cpp = None + else: + cpp = PRIMITIVE_MAP.get(base) + return is_array, cpp, base + + +def snake_to_camel(s: str) -> str: + # move_waist -> MoveWaist + parts = re.split(r'[_\-]', s) + return ''.join(p.capitalize() for p in parts if p) + + +def resolve_interface_subdir(root: str, sub: str): + """Return the first existing directory for common layouts: root/sub, root/src/sub, root/interfaces/sub.""" + 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 + # default to primary location even if missing + return candidates[0] + + +def gen_converter_action(base: str, fields, include_path: str): + # base: MoveWaist + cpp = [] + cpp.append(f'// {base}::Goal: ' + ', '.join([n for _,n in fields])) + cpp.append('template<>') + cpp.append(f'inline interfaces::action::{base}::Goal from_yaml(const YAML::Node & n)') + cpp.append('{') + cpp.append(f' interfaces::action::{base}::Goal g;') + cpp.append(' if (n && n.IsMap()) {') + has_data_array = any(n == 'data_array' for _, n in fields) + has_data_length = any(n == 'data_length' for _, n in fields) + for ty,name in fields: + is_array, cpp_type, base_type = make_cpp_type(ty) + if is_array and cpp_type: + cpp.append(f' if (n["{name}"]) {{') + cpp.append(f' auto vec = n["{name}"].as>();') + cpp.append(f' g.{name}.assign(vec.begin(), vec.end());') + cpp.append(' }') + elif not is_array and cpp_type: + # map to as() + cpp.append(f' if (n["{name}"]) g.{name} = n["{name}"].as<{cpp_type}>();') + else: + # complex types: handle some known messages + # normalize geometry type names like geometry_msgs/msg/PoseStamped -> geometry_msgs/PoseStamped + bt = base_type.replace('::', '/') + bt = bt.replace('/msg/', '/') + bt = bt.replace('/Msg/', '/') + if bt in ("geometry_msgs/PoseStamped", "geometry_msgs/PoseStamped") and not is_array: + cpp.append(' // geometry_msgs::msg::PoseStamped') + cpp.append(' if (n) {') + cpp.append(' if (n["frame_id"]) g.{0}.header.frame_id = n["frame_id"].as();'.format(name)) + cpp.append(' if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as();'.format(name)) + cpp.append(' if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as();'.format(name)) + cpp.append(' if (auto p = n["position"]) {') + cpp.append(' if (p["x"]) g.{0}.pose.position.x = p["x"].as();'.format(name)) + cpp.append(' if (p["y"]) g.{0}.pose.position.y = p["y"].as();'.format(name)) + cpp.append(' if (p["z"]) g.{0}.pose.position.z = p["z"].as();'.format(name)) + cpp.append(' }') + cpp.append(' if (auto q = n["orientation"]) {') + cpp.append(' if (q["x"]) g.{0}.pose.orientation.x = q["x"].as();'.format(name)) + cpp.append(' if (q["y"]) g.{0}.pose.orientation.y = q["y"].as();'.format(name)) + cpp.append(' if (q["z"]) g.{0}.pose.orientation.z = q["z"].as();'.format(name)) + cpp.append(' if (q["w"]) g.{0}.pose.orientation.w = q["w"].as();'.format(name)) + cpp.append(' }') + cpp.append(' }') + elif bt in ("geometry_msgs/TwistStamped", "geometry_msgs/TwistStamped") and not is_array: + cpp.append(' // geometry_msgs::msg::TwistStamped') + cpp.append(' if (n) {') + cpp.append(' if (n["frame_id"]) g.{0}.header.frame_id = n["frame_id"].as();'.format(name)) + cpp.append(' if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as();'.format(name)) + cpp.append(' if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as();'.format(name)) + cpp.append(' if (auto lin = n["linear"]) {') + cpp.append(' if (lin["x"]) g.{0}.twist.linear.x = lin["x"].as();'.format(name)) + cpp.append(' if (lin["y"]) g.{0}.twist.linear.y = lin["y"].as();'.format(name)) + cpp.append(' if (lin["z"]) g.{0}.twist.linear.z = lin["z"].as();'.format(name)) + cpp.append(' }') + cpp.append(' if (auto ang = n["angular"]) {') + cpp.append(' if (ang["x"]) g.{0}.twist.angular.x = ang["x"].as();'.format(name)) + cpp.append(' if (ang["y"]) g.{0}.twist.angular.y = ang["y"].as();'.format(name)) + cpp.append(' if (ang["z"]) g.{0}.twist.angular.z = ang["z"].as();'.format(name)) + cpp.append(' }') + cpp.append(' }') + else: + cpp.append(f' // TODO: parse field `{name}` of ROS type `{ty}` into g.{name} (complex type)') + # post-processing example: if both data_array and data_length exist, set length from array size + if has_data_array and has_data_length: + cpp.append(' if (!g.data_array.empty()) {') + cpp.append(' using DL = decltype(g.data_length);') + cpp.append(' g.data_length = static_cast
(g.data_array.size());') + cpp.append(' }') + cpp.append(' }') + cpp.append(' return g;') + cpp.append('}\n') + return '\n'.join(cpp) + + +def gen_converter_srv(base: str, fields, include_path: str): + cpp = [] + cpp.append(f'// {base}::Request: ' + ', '.join([n for _,n in fields])) + cpp.append('template<>') + cpp.append(f'inline interfaces::srv::{base}::Request from_yaml(const YAML::Node & n)') + cpp.append('{') + cpp.append(f' interfaces::srv::{base}::Request r;') + cpp.append(' if (n && n.IsMap()) {') + for ty,name in fields: + is_array, cpp_type, base_type = make_cpp_type(ty) + if is_array and cpp_type: + cpp.append(f' if (n["{name}"]) {{') + cpp.append(f' auto vec = n["{name}"].as>();') + cpp.append(f' r.{name}.assign(vec.begin(), vec.end());') + cpp.append(' }') + elif not is_array and cpp_type: + cpp.append(f' if (n["{name}"]) r.{name} = n["{name}"].as<{cpp_type}>();') + else: + bt = base_type.replace('::', '/') + bt = bt.replace('/msg/', '/') + bt = bt.replace('/Msg/', '/') + if bt in ("geometry_msgs/PoseStamped", "geometry_msgs/PoseStamped") and not is_array: + cpp.append(' // geometry_msgs::msg::PoseStamped') + cpp.append(' if (n) {') + cpp.append(' if (n["frame_id"]) r.{0}.header.frame_id = n["frame_id"].as();'.format(name)) + cpp.append(' if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as();'.format(name)) + cpp.append(' if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as();'.format(name)) + cpp.append(' if (auto p = n["position"]) {') + cpp.append(' if (p["x"]) r.{0}.pose.position.x = p["x"].as();'.format(name)) + cpp.append(' if (p["y"]) r.{0}.pose.position.y = p["y"].as();'.format(name)) + cpp.append(' if (p["z"]) r.{0}.pose.position.z = p["z"].as();'.format(name)) + cpp.append(' }') + cpp.append(' if (auto q = n["orientation"]) {') + cpp.append(' if (q["x"]) r.{0}.pose.orientation.x = q["x"].as();'.format(name)) + cpp.append(' if (q["y"]) r.{0}.pose.orientation.y = q["y"].as();'.format(name)) + cpp.append(' if (q["z"]) r.{0}.pose.orientation.z = q["z"].as();'.format(name)) + cpp.append(' if (q["w"]) r.{0}.pose.orientation.w = q["w"].as();'.format(name)) + cpp.append(' }') + cpp.append(' }') + elif bt in ("geometry_msgs/TwistStamped", "geometry_msgs/TwistStamped") and not is_array: + cpp.append(' // geometry_msgs::msg::TwistStamped') + cpp.append(' if (n) {') + cpp.append(' if (n["frame_id"]) r.{0}.header.frame_id = n["frame_id"].as();'.format(name)) + cpp.append(' if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as();'.format(name)) + cpp.append(' if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as();'.format(name)) + cpp.append(' if (auto lin = n["linear"]) {') + cpp.append(' if (lin["x"]) r.{0}.twist.linear.x = lin["x"].as();'.format(name)) + cpp.append(' if (lin["y"]) r.{0}.twist.linear.y = lin["y"].as();'.format(name)) + cpp.append(' if (lin["z"]) r.{0}.twist.linear.z = lin["z"].as();'.format(name)) + cpp.append(' }') + cpp.append(' if (auto ang = n["angular"]) {') + cpp.append(' if (ang["x"]) r.{0}.twist.angular.x = ang["x"].as();'.format(name)) + cpp.append(' if (ang["y"]) r.{0}.twist.angular.y = ang["y"].as();'.format(name)) + cpp.append(' if (ang["z"]) r.{0}.twist.angular.z = ang["z"].as();'.format(name)) + cpp.append(' }') + cpp.append(' }') + else: + cpp.append(f' // TODO: parse field `{name}` of ROS type `{ty}` into r.{name} (complex type)') + cpp.append(' }') + cpp.append(' return r;') + cpp.append('}\n') + return '\n'.join(cpp) + + +def main(): + p = argparse.ArgumentParser() + p.add_argument('--skills', default='src/brain/config/robot_skills.yaml') + p.add_argument('--interfaces', default='../hivecore_robot_interfaces') + p.add_argument('--out', default='src/brain/include/brain/payload_converters.hpp') + args = p.parse_args() + + repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..')) + skills_path = args.skills if os.path.isabs(args.skills) else os.path.join(repo_root, args.skills) + 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(skills_path): + print('ERROR: skills file not found:', skills_path, file=sys.stderr) + sys.exit(2) + + print('Using skills:', skills_path) + print('Using interfaces root:', interfaces_root) + print('Writing output to:', out_path) + + with open(skills_path, 'r', encoding='utf-8') as f: + skills = yaml.safe_load(f) + + includes = set() + converters = [] + added_actions = set() + added_srvs = set() + + for item in skills: + if not isinstance(item, dict): + continue + name = item.get('name') + interfaces = item.get('interfaces', []) + for iface in interfaces: + token = None + if isinstance(iface, str): + token = iface + elif isinstance(iface, dict): + token = iface.get('name') + if not token: + continue + # token like BaseName.action + if '.' not in token: + continue + base, kind = token.split('.', 1) + kind = kind.lower() + header_snake = to_snake(base) + if kind.startswith('action'): + includes.add(f'"interfaces/action/{header_snake}.hpp"') + # locate .action file in common subdirs + action_dir = resolve_interface_subdir(interfaces_root, 'action') + tried_paths = [] + action_file = os.path.join(action_dir, base + '.action') + tried_paths.append(action_file) + if not os.path.exists(action_file): + camel = snake_to_camel(base) + if camel != base: + action_file_camel = os.path.join(action_dir, camel + '.action') + tried_paths.append(action_file_camel) + if os.path.exists(action_file_camel): + action_file = action_file_camel + if not os.path.exists(action_file): + found = find_file_case_insensitive(action_dir, base + '.action') + if found: + action_file = found + if not os.path.exists(action_file): + # final fallback to snake name in case-insensitive form + snake_path = os.path.join(action_dir, header_snake + '.action') + tried_paths.append(snake_path) + if os.path.exists(snake_path): + action_file = snake_path + if not os.path.exists(action_file): + print('WARN: action file not found for', base, 'checked', tried_paths[-1]) + fields = [] + else: + fields = parse_action_file(action_file) + converters.append(gen_converter_action(base, fields, header_snake)) + added_actions.add(base) + elif kind.startswith('srv') or kind.startswith('service'): + includes.add(f'"interfaces/srv/{header_snake}.hpp"') + srv_dir = resolve_interface_subdir(interfaces_root, 'srv') + tried_paths = [] + srv_file = os.path.join(srv_dir, base + '.srv') + tried_paths.append(srv_file) + if not os.path.exists(srv_file): + # try snake_case -> CamelCase fallback + camel = snake_to_camel(base) + if camel != base: + srv_file_camel = os.path.join(srv_dir, camel + '.srv') + tried_paths.append(srv_file_camel) + if os.path.exists(srv_file_camel): + srv_file = srv_file_camel + if srv_file == os.path.join(srv_dir, base + '.srv'): + found = find_file_case_insensitive(srv_dir, base + '.srv') + if found: + srv_file = found + else: + srv_file = os.path.join(srv_dir, header_snake + '.srv') + tried_paths.append(srv_file) + if not os.path.exists(srv_file): + print('WARN: srv file not found for', base, 'checked', tried_paths[-1]) + fields = [] + else: + fields = parse_srv_file(srv_file) + converters.append(gen_converter_srv(base, fields, header_snake)) + added_srvs.add(base) + # end of iface loop + pass + + # Second pass: also generate for all action/srv files in interfaces even if not referenced in skills + action_dir = resolve_interface_subdir(interfaces_root, 'action') + if os.path.isdir(action_dir): + for fname in os.listdir(action_dir): + if not fname.endswith('.action'): + continue + base_name = os.path.splitext(fname)[0] + # preserve filename's CamelCase as type, only use to_snake for header path + base_type = base_name + if base_type in added_actions: + continue + header_snake = to_snake(base_type) + includes.add(f'"interfaces/action/{header_snake}.hpp"') + fields = parse_action_file(os.path.join(action_dir, fname)) + converters.append(gen_converter_action(base_type, fields, header_snake)) + added_actions.add(base_type) + + srv_dir = resolve_interface_subdir(interfaces_root, 'srv') + if os.path.isdir(srv_dir): + for fname in os.listdir(srv_dir): + if not fname.endswith('.srv'): + continue + base_name = os.path.splitext(fname)[0] + base_type = base_name + if base_type in added_srvs: + continue + header_snake = to_snake(base_type) + includes.add(f'"interfaces/srv/{header_snake}.hpp"') + fields = parse_srv_file(os.path.join(srv_dir, fname)) + converters.append(gen_converter_srv(base_type, fields, header_snake)) + added_srvs.add(base_type) + + # assemble header + os.makedirs(os.path.dirname(out_path), exist_ok=True) + with open(out_path, 'w', encoding='utf-8') as outf: + outf.write(HEADER_PREAMBLE) + if includes: + for inc in sorted(includes): + outf.write(f'#include {inc}\n') + outf.write('\n') + outf.write(NAMESPACE_BEGIN) + outf.write('\n'.join(converters)) + outf.write(HEADER_POSTAMBLE) + + print('Done.') + +if __name__ == '__main__': + main()