add gen_payload_converters.py
This commit is contained in:
@@ -1,28 +1,113 @@
|
||||
// GENERATED FILE - do not edit by hand
|
||||
// Generated by src/scripts/gen_payload_converters.py
|
||||
#pragma once
|
||||
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <string>
|
||||
#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 <vector>
|
||||
#include <cstdint>
|
||||
#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<typename T>
|
||||
T from_yaml(const YAML::Node & n);
|
||||
|
||||
// VisionObjectRecognition::Request: camera_position
|
||||
template<>
|
||||
inline interfaces::srv::VisionObjectRecognition::Request from_yaml<interfaces::srv::VisionObjectRecognition::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::VisionObjectRecognition::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["camera_position"]) r.camera_position = n["camera_position"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// MapLoad::Request: map_url
|
||||
template<>
|
||||
inline interfaces::srv::MapLoad::Request from_yaml<interfaces::srv::MapLoad::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::MapLoad::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// MapSave::Request: map_url
|
||||
template<>
|
||||
inline interfaces::srv::MapSave::Request from_yaml<interfaces::srv::MapSave::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::MapSave::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
|
||||
}
|
||||
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<interfaces::action::Arm::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::Arm::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["body_id"]) g.body_id = n["body_id"].as<int8_t>();
|
||||
if (n["data_type"]) g.data_type = n["data_type"].as<int8_t>();
|
||||
if (n["data_length"]) g.data_length = n["data_length"].as<int16_t>();
|
||||
if (n["command_id"]) g.command_id = n["command_id"].as<int64_t>();
|
||||
if (n["frame_time_stamp"]) g.frame_time_stamp = n["frame_time_stamp"].as<int64_t>();
|
||||
if (n["data_array"]) {
|
||||
auto vec = n["data_array"].as<std::vector<double>>();
|
||||
g.data_array.assign(vec.begin(), vec.end());
|
||||
}
|
||||
if (!g.data_array.empty()) {
|
||||
using DL = decltype(g.data_length);
|
||||
g.data_length = static_cast<DL>(g.data_array.size());
|
||||
}
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// CameraTakePhoto::Goal: mode, effort
|
||||
template<>
|
||||
inline interfaces::action::CameraTakePhoto::Goal from_yaml<interfaces::action::CameraTakePhoto::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::CameraTakePhoto::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["mode"]) g.mode = n["mode"].as<int32_t>();
|
||||
if (n["effort"]) g.effort = n["effort"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// HandControl::Goal: mode, effort
|
||||
template<>
|
||||
inline interfaces::action::HandControl::Goal from_yaml<interfaces::action::HandControl::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::HandControl::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["mode"]) g.mode = n["mode"].as<int32_t>();
|
||||
if (n["effort"]) g.effort = n["effort"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// MoveWaist::Goal: move_pitch_degree, move_yaw_degree
|
||||
template<>
|
||||
inline interfaces::action::MoveWaist::Goal from_yaml<interfaces::action::MoveWaist::Goal>(const YAML::Node & n)
|
||||
@@ -58,33 +143,24 @@ inline interfaces::action::MoveWheel::Goal from_yaml<interfaces::action::MoveWhe
|
||||
return g;
|
||||
}
|
||||
|
||||
// HandControl::Goal: mode
|
||||
// MoveHome::Goal:
|
||||
template<>
|
||||
inline interfaces::action::HandControl::Goal from_yaml<interfaces::action::HandControl::Goal>(const YAML::Node & n)
|
||||
inline interfaces::action::MoveHome::Goal from_yaml<interfaces::action::MoveHome::Goal>(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<int32_t>();
|
||||
}
|
||||
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<interfaces::action::Arm::Goal>(const YAML::Node & n)
|
||||
inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(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<int32_t>();
|
||||
if (n["data_type"]) g.data_type = n["data_type"].as<int32_t>();
|
||||
if (n["command_id"]) g.command_id = n["command_id"].as<int64_t>();
|
||||
if (n["frame_time_stamp"]) g.frame_time_stamp = n["frame_time_stamp"].as<int64_t>();
|
||||
if (n["data_array"]) {
|
||||
auto vec = n["data_array"].as<std::vector<double>>();
|
||||
g.data_array.assign(vec.begin(), vec.end());
|
||||
g.data_length = static_cast<int32_t>(g.data_array.size());
|
||||
}
|
||||
if (n["data_length"]) g.data_length = n["data_length"].as<int32_t>();
|
||||
if (n["target_x"]) g.target_x = n["target_x"].as<float>();
|
||||
if (n["target_y"]) g.target_y = n["target_y"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
@@ -100,83 +176,55 @@ inline interfaces::action::SlamMode::Goal from_yaml<interfaces::action::SlamMode
|
||||
return g;
|
||||
}
|
||||
|
||||
// CameraTakePhoto::Goal: mode, effort
|
||||
template<>
|
||||
inline interfaces::action::CameraTakePhoto::Goal from_yaml<interfaces::action::CameraTakePhoto::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::CameraTakePhoto::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["mode"]) g.mode = n["mode"].as<int32_t>();
|
||||
if (n["effort"]) g.effort = n["effort"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// ArmSpaceControl::Goal: target (geometry_msgs::msg::PoseStamped)
|
||||
// ArmSpaceControl::Goal: target
|
||||
template<>
|
||||
inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::ArmSpaceControl::Goal>(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<std::string>();
|
||||
if (n["stamp_sec"]) pose.header.stamp.sec = n["stamp_sec"].as<int32_t>();
|
||||
if (n["stamp_nanosec"]) pose.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
|
||||
if (auto p = n["position"]) {
|
||||
if (p["x"]) pose.pose.position.x = p["x"].as<double>();
|
||||
if (p["y"]) pose.pose.position.y = p["y"].as<double>();
|
||||
if (p["z"]) pose.pose.position.z = p["z"].as<double>();
|
||||
}
|
||||
if (auto q = n["orientation"]) {
|
||||
if (q["x"]) pose.pose.orientation.x = q["x"].as<double>();
|
||||
if (q["y"]) pose.pose.orientation.y = q["y"].as<double>();
|
||||
if (q["z"]) pose.pose.orientation.z = q["z"].as<double>();
|
||||
if (q["w"]) pose.pose.orientation.w = q["w"].as<double>();
|
||||
// geometry_msgs::msg::PoseStamped
|
||||
if (n) {
|
||||
if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
|
||||
if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
|
||||
if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
|
||||
if (auto p = n["position"]) {
|
||||
if (p["x"]) g.target.pose.position.x = p["x"].as<double>();
|
||||
if (p["y"]) g.target.pose.position.y = p["y"].as<double>();
|
||||
if (p["z"]) g.target.pose.position.z = p["z"].as<double>();
|
||||
}
|
||||
if (auto q = n["orientation"]) {
|
||||
if (q["x"]) g.target.pose.orientation.x = q["x"].as<double>();
|
||||
if (q["y"]) g.target.pose.orientation.y = q["y"].as<double>();
|
||||
if (q["z"]) g.target.pose.orientation.z = q["z"].as<double>();
|
||||
if (q["w"]) g.target.pose.orientation.w = q["w"].as<double>();
|
||||
}
|
||||
}
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// LegControl::Goal: target (geometry_msgs::msg::TwistStamped)
|
||||
// LegControl::Goal: target
|
||||
template<>
|
||||
inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(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<std::string>();
|
||||
if (n["stamp_sec"]) t.header.stamp.sec = n["stamp_sec"].as<int32_t>();
|
||||
if (n["stamp_nanosec"]) t.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
|
||||
if (auto lin = n["linear"]) {
|
||||
if (lin["x"]) t.twist.linear.x = lin["x"].as<double>();
|
||||
if (lin["y"]) t.twist.linear.y = lin["y"].as<double>();
|
||||
if (lin["z"]) t.twist.linear.z = lin["z"].as<double>();
|
||||
// geometry_msgs::msg::TwistStamped
|
||||
if (n) {
|
||||
if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
|
||||
if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
|
||||
if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
|
||||
if (auto lin = n["linear"]) {
|
||||
if (lin["x"]) g.target.twist.linear.x = lin["x"].as<double>();
|
||||
if (lin["y"]) g.target.twist.linear.y = lin["y"].as<double>();
|
||||
if (lin["z"]) g.target.twist.linear.z = lin["z"].as<double>();
|
||||
}
|
||||
if (auto ang = n["angular"]) {
|
||||
if (ang["x"]) g.target.twist.angular.x = ang["x"].as<double>();
|
||||
if (ang["y"]) g.target.twist.angular.y = ang["y"].as<double>();
|
||||
if (ang["z"]) g.target.twist.angular.z = ang["z"].as<double>();
|
||||
}
|
||||
}
|
||||
if (auto ang = n["angular"]) {
|
||||
if (ang["x"]) t.twist.angular.x = ang["x"].as<double>();
|
||||
if (ang["y"]) t.twist.angular.y = ang["y"].as<double>();
|
||||
if (ang["z"]) t.twist.angular.z = ang["z"].as<double>();
|
||||
}
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// MoveHome::Goal: empty; accept any map and ignore
|
||||
template<>
|
||||
inline interfaces::action::MoveHome::Goal from_yaml<interfaces::action::MoveHome::Goal>(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<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::MoveToPosition::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["target_x"]) g.target_x = n["target_x"].as<float>();
|
||||
if (n["target_y"]) g.target_y = n["target_y"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
@@ -192,37 +240,4 @@ inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action:
|
||||
return g;
|
||||
}
|
||||
|
||||
// MapLoad::Request: map_url
|
||||
template<>
|
||||
inline interfaces::srv::MapLoad::Request from_yaml<interfaces::srv::MapLoad::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::MapLoad::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// MapSave::Request: map_url
|
||||
template<>
|
||||
inline interfaces::srv::MapSave::Request from_yaml<interfaces::srv::MapSave::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::MapSave::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["map_url"]) r.map_url = n["map_url"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// VisionObjectRecognition::Request: camera_position
|
||||
template<>
|
||||
inline interfaces::srv::VisionObjectRecognition::Request from_yaml<interfaces::srv::VisionObjectRecognition::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::VisionObjectRecognition::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["camera_position"]) r.camera_position = n["camera_position"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
} // namespace brain
|
||||
|
||||
@@ -107,6 +107,13 @@ void CerebellumNode::ConfigureArmHooks()
|
||||
interfaces::action::Arm::Goal goal{};
|
||||
// Plan A: per-call YAML overrides
|
||||
(void)TryParseCallPayload<interfaces::action::Arm::Goal>(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 {
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
|
||||
459
src/scripts/gen_payload_converters.py
Normal file
459
src/scripts/gen_payload_converters.py
Normal file
@@ -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 <yaml-cpp/yaml.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cstdint>
|
||||
'''
|
||||
|
||||
NAMESPACE_BEGIN = '''
|
||||
namespace brain {
|
||||
|
||||
template<typename T>
|
||||
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<interfaces::action::{base}::Goal>(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<std::vector<{cpp_type}>>();')
|
||||
cpp.append(f' g.{name}.assign(vec.begin(), vec.end());')
|
||||
cpp.append(' }')
|
||||
elif not is_array and cpp_type:
|
||||
# map to as<type>()
|
||||
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<std::string>();'.format(name))
|
||||
cpp.append(' if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
|
||||
cpp.append(' if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
|
||||
cpp.append(' if (auto p = n["position"]) {')
|
||||
cpp.append(' if (p["x"]) g.{0}.pose.position.x = p["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (p["y"]) g.{0}.pose.position.y = p["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (p["z"]) g.{0}.pose.position.z = p["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' if (auto q = n["orientation"]) {')
|
||||
cpp.append(' if (q["x"]) g.{0}.pose.orientation.x = q["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["y"]) g.{0}.pose.orientation.y = q["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["z"]) g.{0}.pose.orientation.z = q["z"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["w"]) g.{0}.pose.orientation.w = q["w"].as<double>();'.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<std::string>();'.format(name))
|
||||
cpp.append(' if (n["stamp_sec"]) g.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
|
||||
cpp.append(' if (n["stamp_nanosec"]) g.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
|
||||
cpp.append(' if (auto lin = n["linear"]) {')
|
||||
cpp.append(' if (lin["x"]) g.{0}.twist.linear.x = lin["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (lin["y"]) g.{0}.twist.linear.y = lin["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (lin["z"]) g.{0}.twist.linear.z = lin["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' if (auto ang = n["angular"]) {')
|
||||
cpp.append(' if (ang["x"]) g.{0}.twist.angular.x = ang["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (ang["y"]) g.{0}.twist.angular.y = ang["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (ang["z"]) g.{0}.twist.angular.z = ang["z"].as<double>();'.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<DL>(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<interfaces::srv::{base}::Request>(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<std::vector<{cpp_type}>>();')
|
||||
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<std::string>();'.format(name))
|
||||
cpp.append(' if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
|
||||
cpp.append(' if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
|
||||
cpp.append(' if (auto p = n["position"]) {')
|
||||
cpp.append(' if (p["x"]) r.{0}.pose.position.x = p["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (p["y"]) r.{0}.pose.position.y = p["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (p["z"]) r.{0}.pose.position.z = p["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' if (auto q = n["orientation"]) {')
|
||||
cpp.append(' if (q["x"]) r.{0}.pose.orientation.x = q["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["y"]) r.{0}.pose.orientation.y = q["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["z"]) r.{0}.pose.orientation.z = q["z"].as<double>();'.format(name))
|
||||
cpp.append(' if (q["w"]) r.{0}.pose.orientation.w = q["w"].as<double>();'.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<std::string>();'.format(name))
|
||||
cpp.append(' if (n["stamp_sec"]) r.{0}.header.stamp.sec = n["stamp_sec"].as<int32_t>();'.format(name))
|
||||
cpp.append(' if (n["stamp_nanosec"]) r.{0}.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();'.format(name))
|
||||
cpp.append(' if (auto lin = n["linear"]) {')
|
||||
cpp.append(' if (lin["x"]) r.{0}.twist.linear.x = lin["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (lin["y"]) r.{0}.twist.linear.y = lin["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (lin["z"]) r.{0}.twist.linear.z = lin["z"].as<double>();'.format(name))
|
||||
cpp.append(' }')
|
||||
cpp.append(' if (auto ang = n["angular"]) {')
|
||||
cpp.append(' if (ang["x"]) r.{0}.twist.angular.x = ang["x"].as<double>();'.format(name))
|
||||
cpp.append(' if (ang["y"]) r.{0}.twist.angular.y = ang["y"].as<double>();'.format(name))
|
||||
cpp.append(' if (ang["z"]) r.{0}.twist.angular.z = ang["z"].as<double>();'.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()
|
||||
Reference in New Issue
Block a user