Fix generators for payload, skills, and BT params
This commit is contained in:
NuoDaJia02
@@ -1,27 +1,42 @@
|
||||
- name: VisionObjectRecognition
|
||||
- name: VisionObjectRecognitionSrv
|
||||
version: 1.0.0
|
||||
description: "视觉识别指定物体"
|
||||
interfaces:
|
||||
- VisionObjectRecognition.srv
|
||||
- name: VisionObjectRecognition.srv
|
||||
default_topic: "/vision_object_recognition"
|
||||
|
||||
- name: MapLoad
|
||||
- name: VisionObjectRecognitionAct
|
||||
version: 1.0.0
|
||||
description: "加载地图"
|
||||
description: "视觉识别指定物体"
|
||||
interfaces:
|
||||
- MapLoad.srv
|
||||
- name: VisionObjectRecognition.action
|
||||
default_topic: "/vision_object_recognition"
|
||||
|
||||
- name: MapSave
|
||||
version: 1.0.0
|
||||
description: "保存地图"
|
||||
interfaces:
|
||||
- MapSave.srv
|
||||
# - name: MapLoad
|
||||
# version: 1.0.0
|
||||
# description: "加载地图"
|
||||
# interfaces:
|
||||
# - MapLoad.srv
|
||||
|
||||
- name: Arm
|
||||
# - name: MapSave
|
||||
# version: 1.0.0
|
||||
# description: "保存地图"
|
||||
# interfaces:
|
||||
# - MapSave.srv
|
||||
|
||||
# - name: Arm
|
||||
# version: 1.0.0
|
||||
# description: "手臂控制"
|
||||
# interfaces:
|
||||
# - name: Arm.action
|
||||
# default_topic: "ArmAction"
|
||||
|
||||
- name: DualArm
|
||||
version: 1.0.0
|
||||
description: "手臂控制"
|
||||
description: "双臂控制"
|
||||
interfaces:
|
||||
- name: Arm.action
|
||||
default_topic: "ArmAction"
|
||||
- name: DualArm.action
|
||||
default_topic: "DualArm"
|
||||
|
||||
- name: CameraTakePhoto
|
||||
version: 1.0.0
|
||||
@@ -29,11 +44,11 @@
|
||||
interfaces:
|
||||
- CameraTakePhoto.action
|
||||
|
||||
- name: HandControl
|
||||
version: 1.0.0
|
||||
description: "手部控制"
|
||||
interfaces:
|
||||
- HandControl.action
|
||||
# - name: HandControl
|
||||
# version: 1.0.0
|
||||
# description: "手部控制"
|
||||
# interfaces:
|
||||
# - HandControl.action
|
||||
|
||||
- name: MoveWaist
|
||||
version: 1.0.0
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
#include "interfaces/action/arm.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
#include "interfaces/action/camera_take_photo.hpp"
|
||||
#include "interfaces/action/dual_arm.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/gripper_cmd.hpp"
|
||||
#include "interfaces/action/hand_control.hpp"
|
||||
@@ -20,12 +21,21 @@
|
||||
#include "interfaces/action/move_wheel.hpp"
|
||||
#include "interfaces/action/slam_mode.hpp"
|
||||
#include "interfaces/action/vision_grasp_object.hpp"
|
||||
#include "interfaces/action/vision_object_recognition.hpp"
|
||||
#include "interfaces/msg/arm_motion_params.hpp"
|
||||
#include "interfaces/msg/skill_call.hpp"
|
||||
#include "interfaces/srv/asr_recognize.hpp"
|
||||
#include "interfaces/srv/audio_data.hpp"
|
||||
#include "interfaces/srv/bt_rebuild.hpp"
|
||||
#include "interfaces/srv/clear_arm_error.hpp"
|
||||
#include "interfaces/srv/gripper_cmd.hpp"
|
||||
#include "interfaces/srv/inverse_kinematics.hpp"
|
||||
#include "interfaces/srv/map_load.hpp"
|
||||
#include "interfaces/srv/map_save.hpp"
|
||||
#include "interfaces/srv/motor_info.hpp"
|
||||
#include "interfaces/srv/motor_param.hpp"
|
||||
#include "interfaces/srv/tts_synthesize.hpp"
|
||||
#include "interfaces/srv/vad_event.hpp"
|
||||
#include "interfaces/srv/vision_object_recognition.hpp"
|
||||
|
||||
|
||||
@@ -34,58 +44,69 @@ namespace brain {
|
||||
template<typename T>
|
||||
T from_yaml(const YAML::Node & n);
|
||||
|
||||
// VisionObjectRecognition::Request: camera_position
|
||||
// VisionObjectRecognition::Request: camera_position, classes
|
||||
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 (n["classes"]) {
|
||||
auto vec = n["classes"].as<std::vector<std::string>>();
|
||||
r.classes.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 r;
|
||||
}
|
||||
|
||||
// VisionObjectRecognition::Goal: camera_position, classes
|
||||
template<>
|
||||
inline interfaces::action::VisionObjectRecognition::Goal from_yaml<interfaces::action::VisionObjectRecognition::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::VisionObjectRecognition::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["camera_position"]) g.camera_position = n["camera_position"].as<std::string>();
|
||||
if (n["classes"]) {
|
||||
auto vec = n["classes"].as<std::vector<std::string>>();
|
||||
g.classes.assign(vec.begin(), vec.end());
|
||||
}
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// DualArm::Goal: arm_motion_params, velocity_scaling
|
||||
template<>
|
||||
inline interfaces::action::DualArm::Goal from_yaml<interfaces::action::DualArm::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::DualArm::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["arm_motion_params"] && n["arm_motion_params"].IsSequence()) {
|
||||
for (const auto & item : n["arm_motion_params"]) {
|
||||
interfaces::msg::ArmMotionParams tmp;
|
||||
if (item["arm_id"]) tmp.arm_id = item["arm_id"].as<uint8_t>();
|
||||
if (item["motion_type"]) tmp.motion_type = item["motion_type"].as<uint8_t>();
|
||||
if (auto node_1_target_pose = item["target_pose"]) {
|
||||
if (auto node_2_position = node_1_target_pose["position"]) {
|
||||
if (node_2_position["x"]) tmp.target_pose.position.x = node_2_position["x"].as<double>();
|
||||
if (node_2_position["y"]) tmp.target_pose.position.y = node_2_position["y"].as<double>();
|
||||
if (node_2_position["z"]) tmp.target_pose.position.z = node_2_position["z"].as<double>();
|
||||
}
|
||||
if (auto node_2_orientation = node_1_target_pose["orientation"]) {
|
||||
if (node_2_orientation["x"]) tmp.target_pose.orientation.x = node_2_orientation["x"].as<double>();
|
||||
if (node_2_orientation["y"]) tmp.target_pose.orientation.y = node_2_orientation["y"].as<double>();
|
||||
if (node_2_orientation["z"]) tmp.target_pose.orientation.z = node_2_orientation["z"].as<double>();
|
||||
if (node_2_orientation["w"]) tmp.target_pose.orientation.w = node_2_orientation["w"].as<double>();
|
||||
}
|
||||
}
|
||||
if (item["target_joints"]) {
|
||||
auto vec = item["target_joints"].as<std::vector<double>>();
|
||||
tmp.target_joints.assign(vec.begin(), vec.end());
|
||||
}
|
||||
if (item["blend_radius"]) tmp.blend_radius = item["blend_radius"].as<int32_t>();
|
||||
g.arm_motion_params.push_back(tmp);
|
||||
}
|
||||
}
|
||||
if (n["velocity_scaling"]) g.velocity_scaling = n["velocity_scaling"].as<int32_t>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
@@ -102,18 +123,6 @@ inline interfaces::action::CameraTakePhoto::Goal from_yaml<interfaces::action::C
|
||||
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)
|
||||
@@ -173,6 +182,131 @@ inline interfaces::action::GripperCmd::Goal from_yaml<interfaces::action::Grippe
|
||||
return g;
|
||||
}
|
||||
|
||||
// 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()) {
|
||||
if (auto node_0_target = n["target"]) {
|
||||
if (auto node_1_header = node_0_target["header"]) {
|
||||
if (auto node_2_stamp = node_1_header["stamp"]) {
|
||||
if (node_2_stamp["sec"]) g.target.header.stamp.sec = node_2_stamp["sec"].as<int32_t>();
|
||||
if (node_2_stamp["nanosec"]) g.target.header.stamp.nanosec = node_2_stamp["nanosec"].as<uint32_t>();
|
||||
}
|
||||
if (node_1_header["frame_id"]) g.target.header.frame_id = node_1_header["frame_id"].as<std::string>();
|
||||
}
|
||||
if (auto node_1_twist = node_0_target["twist"]) {
|
||||
if (auto node_2_linear = node_1_twist["linear"]) {
|
||||
if (node_2_linear["x"]) g.target.twist.linear.x = node_2_linear["x"].as<double>();
|
||||
if (node_2_linear["y"]) g.target.twist.linear.y = node_2_linear["y"].as<double>();
|
||||
if (node_2_linear["z"]) g.target.twist.linear.z = node_2_linear["z"].as<double>();
|
||||
}
|
||||
if (auto node_2_angular = node_1_twist["angular"]) {
|
||||
if (node_2_angular["x"]) g.target.twist.angular.x = node_2_angular["x"].as<double>();
|
||||
if (node_2_angular["y"]) g.target.twist.angular.y = node_2_angular["y"].as<double>();
|
||||
if (node_2_angular["z"]) g.target.twist.angular.z = node_2_angular["z"].as<double>();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
// SlamMode::Goal: enable_mapping
|
||||
template<>
|
||||
inline interfaces::action::SlamMode::Goal from_yaml<interfaces::action::SlamMode::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::SlamMode::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["enable_mapping"]) g.enable_mapping = n["enable_mapping"].as<bool>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// MoveToPosition::Goal: target_x, target_y, target_z, target_angle_pitch, target_angle_roll, target_angle_yaw
|
||||
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>();
|
||||
if (n["target_z"]) g.target_z = n["target_z"].as<float>();
|
||||
if (n["target_angle_pitch"]) g.target_angle_pitch = n["target_angle_pitch"].as<float>();
|
||||
if (n["target_angle_roll"]) g.target_angle_roll = n["target_angle_roll"].as<float>();
|
||||
if (n["target_angle_yaw"]) g.target_angle_yaw = n["target_angle_yaw"].as<float>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// ArmSpaceControl::Goal: target
|
||||
template<>
|
||||
inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::ArmSpaceControl::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::ArmSpaceControl::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (auto node_0_target = n["target"]) {
|
||||
if (auto node_1_header = node_0_target["header"]) {
|
||||
if (auto node_2_stamp = node_1_header["stamp"]) {
|
||||
if (node_2_stamp["sec"]) g.target.header.stamp.sec = node_2_stamp["sec"].as<int32_t>();
|
||||
if (node_2_stamp["nanosec"]) g.target.header.stamp.nanosec = node_2_stamp["nanosec"].as<uint32_t>();
|
||||
}
|
||||
if (node_1_header["frame_id"]) g.target.header.frame_id = node_1_header["frame_id"].as<std::string>();
|
||||
}
|
||||
if (auto node_1_pose = node_0_target["pose"]) {
|
||||
if (auto node_2_position = node_1_pose["position"]) {
|
||||
if (node_2_position["x"]) g.target.pose.position.x = node_2_position["x"].as<double>();
|
||||
if (node_2_position["y"]) g.target.pose.position.y = node_2_position["y"].as<double>();
|
||||
if (node_2_position["z"]) g.target.pose.position.z = node_2_position["z"].as<double>();
|
||||
}
|
||||
if (auto node_2_orientation = node_1_pose["orientation"]) {
|
||||
if (node_2_orientation["x"]) g.target.pose.orientation.x = node_2_orientation["x"].as<double>();
|
||||
if (node_2_orientation["y"]) g.target.pose.orientation.y = node_2_orientation["y"].as<double>();
|
||||
if (node_2_orientation["z"]) g.target.pose.orientation.z = node_2_orientation["z"].as<double>();
|
||||
if (node_2_orientation["w"]) g.target.pose.orientation.w = node_2_orientation["w"].as<double>();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// ExecuteBtAction::Goal: epoch, action_name, calls
|
||||
template<>
|
||||
inline interfaces::action::ExecuteBtAction::Goal from_yaml<interfaces::action::ExecuteBtAction::Goal>(const YAML::Node & n)
|
||||
@@ -181,31 +315,17 @@ inline interfaces::action::ExecuteBtAction::Goal from_yaml<interfaces::action::E
|
||||
if (n && n.IsMap()) {
|
||||
if (n["epoch"]) g.epoch = n["epoch"].as<uint32_t>();
|
||||
if (n["action_name"]) g.action_name = n["action_name"].as<std::string>();
|
||||
// TODO: parse field `calls` of ROS type `interfaces/SkillCall[]` into g.calls (complex type)
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// 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()) {
|
||||
// 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 (n["calls"] && n["calls"].IsSequence()) {
|
||||
for (const auto & item : n["calls"]) {
|
||||
interfaces::msg::SkillCall tmp;
|
||||
if (item["name"]) tmp.name = item["name"].as<std::string>();
|
||||
if (item["interface_type"]) tmp.interface_type = item["interface_type"].as<std::string>();
|
||||
if (item["call_kind"]) tmp.call_kind = item["call_kind"].as<std::string>();
|
||||
if (item["topic"]) tmp.topic = item["topic"].as<std::string>();
|
||||
if (item["payload_yaml"]) tmp.payload_yaml = item["payload_yaml"].as<std::string>();
|
||||
if (item["instance_name"]) tmp.instance_name = item["instance_name"].as<std::string>();
|
||||
if (item["timeout_ms"]) tmp.timeout_ms = item["timeout_ms"].as<int32_t>();
|
||||
g.calls.push_back(tmp);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -223,66 +343,14 @@ inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action:
|
||||
return g;
|
||||
}
|
||||
|
||||
// MoveToPosition::Goal: target_x, target_y
|
||||
// VADEvent::Request: command, timeout_ms
|
||||
template<>
|
||||
inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
|
||||
inline interfaces::srv::VADEvent::Request from_yaml<interfaces::srv::VADEvent::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::MoveToPosition::Goal g;
|
||||
interfaces::srv::VADEvent::Request r;
|
||||
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;
|
||||
}
|
||||
|
||||
// SlamMode::Goal: enable_mapping
|
||||
template<>
|
||||
inline interfaces::action::SlamMode::Goal from_yaml<interfaces::action::SlamMode::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::SlamMode::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["enable_mapping"]) g.enable_mapping = n["enable_mapping"].as<bool>();
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
// ArmSpaceControl::Goal: target
|
||||
template<>
|
||||
inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::ArmSpaceControl::Goal>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::action::ArmSpaceControl::Goal g;
|
||||
if (n && n.IsMap()) {
|
||||
// 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;
|
||||
}
|
||||
|
||||
// GripperCmd::Request: devid, loc, speed, torque
|
||||
template<>
|
||||
inline interfaces::srv::GripperCmd::Request from_yaml<interfaces::srv::GripperCmd::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::GripperCmd::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["devid"]) r.devid = n["devid"].as<uint8_t>();
|
||||
if (n["loc"]) r.loc = n["loc"].as<uint8_t>();
|
||||
if (n["speed"]) r.speed = n["speed"].as<uint8_t>();
|
||||
if (n["torque"]) r.torque = n["torque"].as<uint8_t>();
|
||||
if (n["command"]) r.command = n["command"].as<std::string>();
|
||||
if (n["timeout_ms"]) r.timeout_ms = n["timeout_ms"].as<int32_t>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
@@ -303,6 +371,65 @@ inline interfaces::srv::MotorInfo::Request from_yaml<interfaces::srv::MotorInfo:
|
||||
return r;
|
||||
}
|
||||
|
||||
// BtRebuild::Request: type, config, param
|
||||
template<>
|
||||
inline interfaces::srv::BtRebuild::Request from_yaml<interfaces::srv::BtRebuild::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::BtRebuild::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["type"]) r.type = n["type"].as<std::string>();
|
||||
if (n["config"]) r.config = n["config"].as<std::string>();
|
||||
if (n["param"]) r.param = n["param"].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;
|
||||
}
|
||||
|
||||
// AudioData::Request: command, duration_ms
|
||||
template<>
|
||||
inline interfaces::srv::AudioData::Request from_yaml<interfaces::srv::AudioData::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::AudioData::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["command"]) r.command = n["command"].as<std::string>();
|
||||
if (n["duration_ms"]) r.duration_ms = n["duration_ms"].as<int32_t>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// ClearArmError::Request: arm_id, joint_num
|
||||
template<>
|
||||
inline interfaces::srv::ClearArmError::Request from_yaml<interfaces::srv::ClearArmError::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::ClearArmError::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["arm_id"]) r.arm_id = n["arm_id"].as<int8_t>();
|
||||
if (n["joint_num"]) r.joint_num = n["joint_num"].as<int8_t>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// ASRRecognize::Request: command
|
||||
template<>
|
||||
inline interfaces::srv::ASRRecognize::Request from_yaml<interfaces::srv::ASRRecognize::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::ASRRecognize::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["command"]) r.command = n["command"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// MotorParam::Request: motor_id, max_speed, add_acc, dec_acc
|
||||
template<>
|
||||
inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorParam::Request>(const YAML::Node & n)
|
||||
@@ -317,15 +444,64 @@ inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorPara
|
||||
return r;
|
||||
}
|
||||
|
||||
// BtRebuild::Request: type, config, param
|
||||
// GripperCmd::Request: devid, loc, speed, torque
|
||||
template<>
|
||||
inline interfaces::srv::BtRebuild::Request from_yaml<interfaces::srv::BtRebuild::Request>(const YAML::Node & n)
|
||||
inline interfaces::srv::GripperCmd::Request from_yaml<interfaces::srv::GripperCmd::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::BtRebuild::Request r;
|
||||
interfaces::srv::GripperCmd::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["type"]) r.type = n["type"].as<std::string>();
|
||||
if (n["config"]) r.config = n["config"].as<std::string>();
|
||||
if (n["param"]) r.param = n["param"].as<std::string>();
|
||||
if (n["devid"]) r.devid = n["devid"].as<uint8_t>();
|
||||
if (n["loc"]) r.loc = n["loc"].as<uint8_t>();
|
||||
if (n["speed"]) r.speed = n["speed"].as<uint8_t>();
|
||||
if (n["torque"]) r.torque = n["torque"].as<uint8_t>();
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
// TTSSynthesize::Request: command, text, voice
|
||||
template<>
|
||||
inline interfaces::srv::TTSSynthesize::Request from_yaml<interfaces::srv::TTSSynthesize::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::TTSSynthesize::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["command"]) r.command = n["command"].as<std::string>();
|
||||
if (n["text"]) r.text = n["text"].as<std::string>();
|
||||
if (n["voice"]) r.voice = n["voice"].as<std::string>();
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// InverseKinematics::Request: arm_id, pose
|
||||
template<>
|
||||
inline interfaces::srv::InverseKinematics::Request from_yaml<interfaces::srv::InverseKinematics::Request>(const YAML::Node & n)
|
||||
{
|
||||
interfaces::srv::InverseKinematics::Request r;
|
||||
if (n && n.IsMap()) {
|
||||
if (n["arm_id"]) r.arm_id = n["arm_id"].as<int32_t>();
|
||||
if (auto node_0_pose = n["pose"]) {
|
||||
if (auto node_1_position = node_0_pose["position"]) {
|
||||
if (node_1_position["x"]) r.pose.position.x = node_1_position["x"].as<double>();
|
||||
if (node_1_position["y"]) r.pose.position.y = node_1_position["y"].as<double>();
|
||||
if (node_1_position["z"]) r.pose.position.z = node_1_position["z"].as<double>();
|
||||
}
|
||||
if (auto node_1_orientation = node_0_pose["orientation"]) {
|
||||
if (node_1_orientation["x"]) r.pose.orientation.x = node_1_orientation["x"].as<double>();
|
||||
if (node_1_orientation["y"]) r.pose.orientation.y = node_1_orientation["y"].as<double>();
|
||||
if (node_1_orientation["z"]) r.pose.orientation.z = node_1_orientation["z"].as<double>();
|
||||
if (node_1_orientation["w"]) r.pose.orientation.w = node_1_orientation["w"].as<double>();
|
||||
}
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
@@ -5,15 +5,33 @@
|
||||
#include <string>
|
||||
|
||||
#include "interfaces/action/arm.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
#include "interfaces/action/camera_take_photo.hpp"
|
||||
#include "interfaces/action/dual_arm.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/gripper_cmd.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/action/vision_object_recognition.hpp"
|
||||
#include "interfaces/srv/asr_recognize.hpp"
|
||||
#include "interfaces/srv/audio_data.hpp"
|
||||
#include "interfaces/srv/bt_rebuild.hpp"
|
||||
#include "interfaces/srv/clear_arm_error.hpp"
|
||||
#include "interfaces/srv/gripper_cmd.hpp"
|
||||
#include "interfaces/srv/inverse_kinematics.hpp"
|
||||
#include "interfaces/srv/map_load.hpp"
|
||||
#include "interfaces/srv/map_save.hpp"
|
||||
#include "interfaces/srv/motor_info.hpp"
|
||||
#include "interfaces/srv/motor_param.hpp"
|
||||
#include "interfaces/srv/tts_synthesize.hpp"
|
||||
#include "interfaces/srv/vad_event.hpp"
|
||||
#include "interfaces/srv/vision_object_recognition.hpp"
|
||||
|
||||
namespace brain {
|
||||
@@ -25,29 +43,56 @@ template<typename ServiceT>
|
||||
struct SkillServiceTrait;
|
||||
|
||||
using SkillActionTypes = std::tuple<
|
||||
interfaces::action::Arm,
|
||||
interfaces::action::VisionObjectRecognition,
|
||||
interfaces::action::DualArm,
|
||||
interfaces::action::CameraTakePhoto,
|
||||
interfaces::action::HandControl,
|
||||
interfaces::action::MoveWaist,
|
||||
interfaces::action::MoveLeg,
|
||||
interfaces::action::MoveWheel,
|
||||
interfaces::action::MoveHome,
|
||||
interfaces::action::GripperCmd
|
||||
interfaces::action::GripperCmd,
|
||||
interfaces::action::Arm,
|
||||
interfaces::action::ArmSpaceControl,
|
||||
interfaces::action::ExecuteBtAction,
|
||||
interfaces::action::HandControl,
|
||||
interfaces::action::LegControl,
|
||||
interfaces::action::MoveToPosition,
|
||||
interfaces::action::SlamMode,
|
||||
interfaces::action::VisionGraspObject
|
||||
>;
|
||||
|
||||
using SkillServiceTypes = std::tuple<
|
||||
interfaces::srv::VisionObjectRecognition,
|
||||
interfaces::srv::ASRRecognize,
|
||||
interfaces::srv::AudioData,
|
||||
interfaces::srv::BtRebuild,
|
||||
interfaces::srv::ClearArmError,
|
||||
interfaces::srv::GripperCmd,
|
||||
interfaces::srv::InverseKinematics,
|
||||
interfaces::srv::MapLoad,
|
||||
interfaces::srv::MapSave
|
||||
interfaces::srv::MapSave,
|
||||
interfaces::srv::MotorInfo,
|
||||
interfaces::srv::MotorParam,
|
||||
interfaces::srv::TTSSynthesize,
|
||||
interfaces::srv::VADEvent
|
||||
>;
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::Arm>
|
||||
struct SkillActionTrait<interfaces::action::VisionObjectRecognition>
|
||||
{
|
||||
static constexpr const char * skill_name = "Arm";
|
||||
static constexpr const char * default_topic = "ArmAction";
|
||||
static bool success(const interfaces::action::Arm::Result & r, const std::string & instance_name) {(void)instance_name; return (r.result == 0);}
|
||||
static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
|
||||
static constexpr const char * skill_name = "VisionObjectRecognition";
|
||||
static constexpr const char * default_topic = "/vision_object_recognition";
|
||||
static bool success(const interfaces::action::VisionObjectRecognition::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::VisionObjectRecognition::Result & r) {return r.info;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::DualArm>
|
||||
{
|
||||
static constexpr const char * skill_name = "DualArm";
|
||||
static constexpr const char * default_topic = "DualArm";
|
||||
static bool success(const interfaces::action::DualArm::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::DualArm::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
@@ -59,15 +104,6 @@ struct SkillActionTrait<interfaces::action::CameraTakePhoto>
|
||||
static std::string message(const interfaces::action::CameraTakePhoto::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::HandControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "HandControl";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::HandControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::MoveWaist>
|
||||
{
|
||||
@@ -128,10 +164,124 @@ struct SkillActionTrait<interfaces::action::GripperCmd>
|
||||
static std::string message(const interfaces::action::GripperCmd::Result & r) {return r.state;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::Arm>
|
||||
{
|
||||
static constexpr const char * skill_name = "Arm";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::Arm::Result & r, const std::string & instance_name) {(void)instance_name; return (r.result == 0);}
|
||||
static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::ArmSpaceControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "ArmSpaceControl";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::ArmSpaceControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::ArmSpaceControl::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::ExecuteBtAction>
|
||||
{
|
||||
static constexpr const char * skill_name = "ExecuteBtAction";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::ExecuteBtAction::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::ExecuteBtAction::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::HandControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "HandControl";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::HandControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::LegControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "LegControl";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::LegControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::LegControl::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::MoveToPosition>
|
||||
{
|
||||
static constexpr const char * skill_name = "MoveToPosition";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::MoveToPosition::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::MoveToPosition::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::SlamMode>
|
||||
{
|
||||
static constexpr const char * skill_name = "SlamMode";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::SlamMode::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::SlamMode::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::VisionGraspObject>
|
||||
{
|
||||
static constexpr const char * skill_name = "VisionGraspObject";
|
||||
static constexpr const char * default_topic = "";
|
||||
static bool success(const interfaces::action::VisionGraspObject::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
|
||||
static std::string message(const interfaces::action::VisionGraspObject::Result & r) {return r.message;}
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::VisionObjectRecognition>
|
||||
{
|
||||
static constexpr const char * skill_name = "VisionObjectRecognition";
|
||||
static constexpr const char * default_topic = "/vision_object_recognition";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::ASRRecognize>
|
||||
{
|
||||
static constexpr const char * skill_name = "ASRRecognize";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::AudioData>
|
||||
{
|
||||
static constexpr const char * skill_name = "AudioData";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::BtRebuild>
|
||||
{
|
||||
static constexpr const char * skill_name = "BtRebuild";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::ClearArmError>
|
||||
{
|
||||
static constexpr const char * skill_name = "ClearArmError";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::GripperCmd>
|
||||
{
|
||||
static constexpr const char * skill_name = "GripperCmd";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::InverseKinematics>
|
||||
{
|
||||
static constexpr const char * skill_name = "InverseKinematics";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
@@ -149,4 +299,32 @@ struct SkillServiceTrait<interfaces::srv::MapSave>
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::MotorInfo>
|
||||
{
|
||||
static constexpr const char * skill_name = "MotorInfo";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::MotorParam>
|
||||
{
|
||||
static constexpr const char * skill_name = "MotorParam";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::TTSSynthesize>
|
||||
{
|
||||
static constexpr const char * skill_name = "TTSSynthesize";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
template<>
|
||||
struct SkillServiceTrait<interfaces::srv::VADEvent>
|
||||
{
|
||||
static constexpr const char * skill_name = "VADEvent";
|
||||
static constexpr const char * default_topic = "";
|
||||
};
|
||||
|
||||
} // namespace brain
|
||||
|
||||
@@ -31,6 +31,7 @@ Notes:
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import glob
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
@@ -94,6 +95,51 @@ def resolve_interface_subdir(root: str, sub: str) -> str:
|
||||
return candidates[0]
|
||||
|
||||
|
||||
def normalize_ros_type(ros_type: str) -> str:
|
||||
bt = ros_type.replace('::', '/')
|
||||
bt = bt.replace('/msg/', '/').replace('/Msg/', '/')
|
||||
return bt
|
||||
|
||||
|
||||
def split_pkg_msg(ros_type: str):
|
||||
if '/' not in ros_type:
|
||||
return None, None
|
||||
pkg, msg = ros_type.split('/', 1)
|
||||
return pkg, msg
|
||||
|
||||
|
||||
def find_msg_file(interfaces_root: str, pkg: str, msg: str) -> Optional[str]:
|
||||
if not pkg or not msg:
|
||||
return None
|
||||
if pkg == 'interfaces':
|
||||
msg_dir = resolve_interface_subdir(interfaces_root, 'msg')
|
||||
if not os.path.isdir(msg_dir):
|
||||
return None
|
||||
msg_file = os.path.join(msg_dir, msg + '.msg')
|
||||
if os.path.exists(msg_file):
|
||||
return msg_file
|
||||
found = find_file_case_insensitive(msg_dir, msg + '.msg')
|
||||
if found:
|
||||
return found
|
||||
snake_path = os.path.join(msg_dir, to_snake(msg) + '.msg')
|
||||
if os.path.exists(snake_path):
|
||||
return snake_path
|
||||
return None
|
||||
|
||||
distro = os.environ.get('ROS_DISTRO')
|
||||
if distro:
|
||||
candidate = os.path.join('/opt/ros', distro, 'share', pkg, 'msg', msg + '.msg')
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
for candidate in glob.glob(os.path.join('/opt/ros', '*', 'share', pkg, 'msg', msg + '.msg')):
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
candidate = os.path.join('/usr/share', pkg, 'msg', msg + '.msg')
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
return None
|
||||
|
||||
|
||||
def parse_action_file(path: str) -> List[Tuple[str, str]]:
|
||||
"""Parse a .action file, return list of (ros_type, field_name) only for Goal part."""
|
||||
try:
|
||||
@@ -108,6 +154,11 @@ def parse_action_file(path: str) -> List[Tuple[str, str]]:
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
if '=' in line:
|
||||
continue
|
||||
m = re.match(r"^([A-Za-z0-9_\[\]/]+)\s+([A-Za-z0-9_]+).*$", line)
|
||||
if m:
|
||||
ty = m.group(1)
|
||||
@@ -116,6 +167,47 @@ def parse_action_file(path: str) -> List[Tuple[str, str]]:
|
||||
return fields
|
||||
|
||||
|
||||
def parse_msg_file(path: str) -> List[Tuple[str, str]]:
|
||||
"""Parse a .msg file, return list of (ros_type, field_name)."""
|
||||
try:
|
||||
with open(path, 'r', encoding='utf-8') as f:
|
||||
content = f.read()
|
||||
except FileNotFoundError:
|
||||
return []
|
||||
fields: List[Tuple[str, str]] = []
|
||||
for line in content.splitlines():
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
if '=' in line:
|
||||
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
|
||||
|
||||
|
||||
_MSG_FIELDS_CACHE: Dict[Tuple[str, str], Optional[List[Tuple[str, str]]]] = {}
|
||||
|
||||
|
||||
def get_msg_fields(interfaces_root: str, pkg: str, msg: str) -> Optional[List[Tuple[str, str]]]:
|
||||
key = (pkg, msg)
|
||||
if key in _MSG_FIELDS_CACHE:
|
||||
return _MSG_FIELDS_CACHE[key]
|
||||
path = find_msg_file(interfaces_root, pkg, msg)
|
||||
if path and os.path.exists(path):
|
||||
fields = parse_msg_file(path)
|
||||
else:
|
||||
fields = None
|
||||
_MSG_FIELDS_CACHE[key] = fields
|
||||
return fields
|
||||
|
||||
|
||||
def locate_action_file(interfaces_root: str, base: str, type: str) -> Optional[str]:
|
||||
action_dir = resolve_interface_subdir(interfaces_root, type)
|
||||
# 1) Try as-is (CamelCase)
|
||||
@@ -141,7 +233,8 @@ def locate_action_file(interfaces_root: str, base: str, type: str) -> Optional[s
|
||||
return None
|
||||
|
||||
|
||||
def make_placeholder_for_type(ros_type: str) -> object:
|
||||
def make_placeholder_for_type(ros_type: str, interfaces_root: str, current_pkg: Optional[str] = None,
|
||||
visited: Optional[set] = None) -> object:
|
||||
"""Return a default placeholder Python value for a ROS field type.
|
||||
|
||||
- Primitive -> default numeric/bool/string
|
||||
@@ -150,42 +243,62 @@ def make_placeholder_for_type(ros_type: str) -> object:
|
||||
- geometry_msgs/TwistStamped -> nested dict with common keys
|
||||
- Otherwise -> None
|
||||
"""
|
||||
if visited is None:
|
||||
visited = set()
|
||||
is_array = ros_type.endswith('[]')
|
||||
base = ros_type[:-2] if is_array else ros_type
|
||||
# normalize namespace
|
||||
base_norm = base.replace('::', '/')
|
||||
base_norm = base_norm.replace('/msg/', '/').replace('/Msg/', '/')
|
||||
base_norm = normalize_ros_type(base)
|
||||
|
||||
if is_array:
|
||||
return []
|
||||
if '/' not in base_norm:
|
||||
# primitive
|
||||
return PRIMITIVES_DEFAULTS.get(base_norm, None)
|
||||
if base_norm in ("geometry_msgs/PoseStamped",):
|
||||
return {
|
||||
'frame_id': '',
|
||||
'stamp_sec': 0,
|
||||
'stamp_nanosec': 0,
|
||||
val = PRIMITIVES_DEFAULTS.get(base_norm, None)
|
||||
return [] if is_array else val
|
||||
|
||||
# known geometry messages fallback
|
||||
if base_norm in ("geometry_msgs/Pose",):
|
||||
val = {
|
||||
'position': {'x': 0.0, 'y': 0.0, 'z': 0.0},
|
||||
'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0},
|
||||
}
|
||||
if base_norm in ("geometry_msgs/TwistStamped",):
|
||||
return {
|
||||
'frame_id': '',
|
||||
'stamp_sec': 0,
|
||||
'stamp_nanosec': 0,
|
||||
'linear': {'x': 0.0, 'y': 0.0, 'z': 0.0},
|
||||
'angular': {'x': 0.0, 'y': 0.0, 'z': 0.0},
|
||||
}
|
||||
# unknown complex -> None
|
||||
return None
|
||||
return [val] if is_array else val
|
||||
if base_norm in ("geometry_msgs/Point",):
|
||||
val = {'x': 0.0, 'y': 0.0, 'z': 0.0}
|
||||
return [val] if is_array else val
|
||||
if base_norm in ("geometry_msgs/Quaternion",):
|
||||
val = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0}
|
||||
return [val] if is_array else val
|
||||
|
||||
pkg, msg = split_pkg_msg(base_norm)
|
||||
if not pkg:
|
||||
pkg = current_pkg
|
||||
msg = base_norm
|
||||
if not pkg or not msg:
|
||||
return [] if is_array else None
|
||||
|
||||
if (pkg, msg) in visited:
|
||||
return [] if is_array else None
|
||||
visited.add((pkg, msg))
|
||||
|
||||
fields = get_msg_fields(interfaces_root, pkg, msg)
|
||||
if not fields:
|
||||
visited.discard((pkg, msg))
|
||||
return [] if is_array else None
|
||||
|
||||
mapping: Dict[str, object] = {}
|
||||
for ftype, fname in fields:
|
||||
mapping[fname] = make_placeholder_for_type(ftype, interfaces_root, pkg, visited)
|
||||
|
||||
visited.discard((pkg, msg))
|
||||
if is_array:
|
||||
return [mapping]
|
||||
return mapping
|
||||
|
||||
|
||||
def build_yaml_params_from_fields(fields: Sequence[Tuple[str, str]]) -> str:
|
||||
def build_yaml_params_from_fields(fields: Sequence[Tuple[str, str]], interfaces_root: str) -> str:
|
||||
"""Create a YAML snippet string from .action Goal fields."""
|
||||
mapping: Dict[str, object] = {}
|
||||
for ros_type, name in fields:
|
||||
mapping[name] = make_placeholder_for_type(ros_type)
|
||||
mapping[name] = make_placeholder_for_type(ros_type, interfaces_root)
|
||||
# Dump as YAML block string
|
||||
# Ensure multi-line by setting default_flow_style=False
|
||||
snippet = yaml.safe_dump(mapping, allow_unicode=True, sort_keys=False)
|
||||
@@ -272,7 +385,7 @@ def main():
|
||||
if action_file == None:
|
||||
action_file = locate_action_file(interfaces_root, base, "srv")
|
||||
fields: List[Tuple[str, str]] = parse_action_file(action_file) if action_file else []
|
||||
yaml_params = build_yaml_params_from_fields(fields)
|
||||
yaml_params = build_yaml_params_from_fields(fields, interfaces_root)
|
||||
results.append({'name': instance_name, 'params': yaml_params})
|
||||
|
||||
# Write output as YAML list; ensure deterministic order as in XML
|
||||
|
||||
@@ -19,6 +19,7 @@ and array primitives. For complex ROS message types it will emit a TODO comment.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import glob
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
@@ -66,15 +67,10 @@ 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
|
||||
# Convert CamelCase (including initialisms like ASRRecognize) to snake_case
|
||||
s1 = re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', s)
|
||||
s2 = re.sub(r'([a-z0-9])([A-Z])', r'\1_\2', s1)
|
||||
return s2.lower()
|
||||
|
||||
|
||||
def parse_action_file(path: str):
|
||||
@@ -89,6 +85,13 @@ def parse_action_file(path: str):
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
# Strip comments before checking constants
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
# Skip constants like: uint8 STATUS_PLANNING=0
|
||||
if '=' in line:
|
||||
continue
|
||||
# lines like: float32 move_pitch_degree
|
||||
m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
|
||||
if m:
|
||||
@@ -109,6 +112,37 @@ def parse_srv_file(path: str):
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
# Strip comments before checking constants
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
# Skip constants like: uint8 STATUS_PLANNING=0
|
||||
if '=' in line:
|
||||
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 parse_msg_file(path: str):
|
||||
"""Parse a .msg file, return list of (type,name) fields."""
|
||||
with open(path, 'r', encoding='utf-8') as f:
|
||||
content = f.read()
|
||||
fields = []
|
||||
for line in content.splitlines():
|
||||
line = line.strip()
|
||||
if not line or line.startswith('#'):
|
||||
continue
|
||||
# Strip comments before checking constants
|
||||
line = line.split('#', 1)[0].strip()
|
||||
if not line:
|
||||
continue
|
||||
# Skip constants like: uint8 STATUS_PLANNING=0
|
||||
if '=' in line:
|
||||
continue
|
||||
m = re.match(r"^([A-Za-z0-9_/\[\]]+)\s+([A-Za-z0-9_]+).*$", line)
|
||||
if m:
|
||||
ty = m.group(1)
|
||||
@@ -160,7 +194,148 @@ def resolve_interface_subdir(root: str, sub: str):
|
||||
return candidates[0]
|
||||
|
||||
|
||||
def gen_converter_action(base: str, fields, include_path: str):
|
||||
def normalize_ros_type(ros_type: str) -> str:
|
||||
bt = ros_type.replace('::', '/')
|
||||
bt = bt.replace('/msg/', '/')
|
||||
bt = bt.replace('/Msg/', '/')
|
||||
return bt
|
||||
|
||||
|
||||
def split_pkg_msg(ros_type: str):
|
||||
if '/' not in ros_type:
|
||||
return None, None
|
||||
pkg, msg = ros_type.split('/', 1)
|
||||
return pkg, msg
|
||||
|
||||
|
||||
def find_msg_file(interfaces_root: str, pkg: str, msg: str):
|
||||
if not pkg or not msg:
|
||||
return None
|
||||
if pkg == 'interfaces':
|
||||
msg_dir = resolve_interface_subdir(interfaces_root, 'msg')
|
||||
if not os.path.isdir(msg_dir):
|
||||
return None
|
||||
msg_file = os.path.join(msg_dir, msg + '.msg')
|
||||
if os.path.exists(msg_file):
|
||||
return msg_file
|
||||
found = find_file_case_insensitive(msg_dir, msg + '.msg')
|
||||
if found:
|
||||
return found
|
||||
snake_path = os.path.join(msg_dir, to_snake(msg) + '.msg')
|
||||
if os.path.exists(snake_path):
|
||||
return snake_path
|
||||
return None
|
||||
|
||||
# Try common ROS share locations for other packages (e.g. geometry_msgs)
|
||||
distro = os.environ.get('ROS_DISTRO')
|
||||
if distro:
|
||||
candidate = os.path.join('/opt/ros', distro, 'share', pkg, 'msg', msg + '.msg')
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
for candidate in glob.glob(os.path.join('/opt/ros', '*', 'share', pkg, 'msg', msg + '.msg')):
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
candidate = os.path.join('/usr/share', pkg, 'msg', msg + '.msg')
|
||||
if os.path.exists(candidate):
|
||||
return candidate
|
||||
return None
|
||||
|
||||
|
||||
_MSG_FIELDS_CACHE = {}
|
||||
|
||||
|
||||
def get_msg_fields(interfaces_root: str, pkg: str, msg: str):
|
||||
key = (pkg, msg)
|
||||
if key in _MSG_FIELDS_CACHE:
|
||||
return _MSG_FIELDS_CACHE[key]
|
||||
path = find_msg_file(interfaces_root, pkg, msg)
|
||||
if path and os.path.exists(path):
|
||||
fields = parse_msg_file(path)
|
||||
else:
|
||||
fields = None
|
||||
_MSG_FIELDS_CACHE[key] = fields
|
||||
return fields
|
||||
|
||||
|
||||
def msg_cpp_type(pkg: str, msg: str):
|
||||
return f'{pkg}::msg::{msg}'
|
||||
|
||||
|
||||
def emit_msg_fields(cpp, node_expr: str, target_expr: str, fields, indent: str,
|
||||
interfaces_root: str, visited: set, current_pkg: str, depth: int = 0):
|
||||
for ty, fname in fields:
|
||||
is_array, cpp_type, base_type = make_cpp_type(ty)
|
||||
if is_array and cpp_type:
|
||||
cpp.append(f'{indent}if ({node_expr}["{fname}"]) {{')
|
||||
cpp.append(f'{indent} auto vec = {node_expr}["{fname}"].as<std::vector<{cpp_type}>>();')
|
||||
cpp.append(f'{indent} {target_expr}.{fname}.assign(vec.begin(), vec.end());')
|
||||
cpp.append(f'{indent}}}')
|
||||
elif not is_array and cpp_type:
|
||||
cpp.append(f'{indent}if ({node_expr}["{fname}"]) {target_expr}.{fname} = {node_expr}["{fname}"].as<{cpp_type}>();')
|
||||
else:
|
||||
bt = normalize_ros_type(base_type)
|
||||
pkg, msg = split_pkg_msg(bt)
|
||||
if not pkg:
|
||||
pkg = current_pkg
|
||||
msg = bt
|
||||
if pkg and msg and (pkg, msg) not in visited:
|
||||
nested_fields = get_msg_fields(interfaces_root, pkg, msg)
|
||||
if nested_fields:
|
||||
visited.add((pkg, msg))
|
||||
if is_array:
|
||||
cpp.append(f'{indent}if ({node_expr}["{fname}"] && {node_expr}["{fname}"].IsSequence()) {{')
|
||||
cpp.append(f'{indent} for (const auto & item : {node_expr}["{fname}"]) {{')
|
||||
cpp.append(f'{indent} {msg_cpp_type(pkg, msg)} tmp;')
|
||||
emit_msg_fields(cpp, 'item', 'tmp', nested_fields, indent + ' ', interfaces_root, visited, pkg, depth + 1)
|
||||
cpp.append(f'{indent} {target_expr}.{fname}.push_back(tmp);')
|
||||
cpp.append(f'{indent} }}')
|
||||
cpp.append(f'{indent}}}')
|
||||
else:
|
||||
sub_var = f'node_{depth}_{fname}'
|
||||
cpp.append(f'{indent}if (auto {sub_var} = {node_expr}["{fname}"]) {{')
|
||||
emit_msg_fields(cpp, sub_var, f'{target_expr}.{fname}', nested_fields, indent + ' ', interfaces_root, visited, pkg, depth + 1)
|
||||
cpp.append(f'{indent}}}')
|
||||
visited.discard((pkg, msg))
|
||||
continue
|
||||
cpp.append(f'{indent}// TODO: parse field `{fname}` of ROS type `{ty}` into {target_expr}.{fname} (complex type)')
|
||||
|
||||
|
||||
def emit_complex_field(cpp, name: str, base_type: str, is_array: bool, target_root: str,
|
||||
interfaces_root: str):
|
||||
bt = normalize_ros_type(base_type)
|
||||
pkg, msg = split_pkg_msg(bt)
|
||||
if not pkg or not msg:
|
||||
return False
|
||||
fields = get_msg_fields(interfaces_root, pkg, msg)
|
||||
if not fields:
|
||||
return False
|
||||
visited = {(pkg, msg)}
|
||||
if is_array:
|
||||
cpp.append(f' if (n["{name}"] && n["{name}"].IsSequence()) {{')
|
||||
cpp.append(f' for (const auto & item : n["{name}"]) {{')
|
||||
cpp.append(f' {msg_cpp_type(pkg, msg)} tmp;')
|
||||
emit_msg_fields(cpp, 'item', 'tmp', fields, ' ', interfaces_root, visited, pkg, 1)
|
||||
cpp.append(f' {target_root}.{name}.push_back(tmp);')
|
||||
cpp.append(' }')
|
||||
cpp.append(' }')
|
||||
else:
|
||||
sub_var = f'node_0_{name}'
|
||||
cpp.append(f' if (auto {sub_var} = n["{name}"]) {{')
|
||||
emit_msg_fields(cpp, sub_var, f'{target_root}.{name}', fields, ' ', interfaces_root, visited, pkg, 1)
|
||||
cpp.append(' }')
|
||||
return True
|
||||
|
||||
|
||||
def add_msg_includes_for_fields(fields, includes):
|
||||
for ty, _ in fields:
|
||||
base_ty = ty[:-2] if ty.endswith('[]') else ty
|
||||
base_ty = normalize_ros_type(base_ty)
|
||||
pkg, msg = split_pkg_msg(base_ty)
|
||||
if pkg == 'interfaces' and msg:
|
||||
includes.add(f'"interfaces/msg/{to_snake(msg)}.hpp"')
|
||||
|
||||
|
||||
def gen_converter_action(base: str, fields, include_path: str, interfaces_root: str):
|
||||
# base: MoveWaist
|
||||
cpp = []
|
||||
cpp.append(f'// {base}::Goal: ' + ', '.join([n for _,n in fields]))
|
||||
@@ -182,47 +357,7 @@ def gen_converter_action(base: str, fields, include_path: str):
|
||||
# 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:
|
||||
if not emit_complex_field(cpp, name, base_type, is_array, 'g', interfaces_root):
|
||||
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:
|
||||
@@ -236,7 +371,7 @@ def gen_converter_action(base: str, fields, include_path: str):
|
||||
return '\n'.join(cpp)
|
||||
|
||||
|
||||
def gen_converter_srv(base: str, fields, include_path: str):
|
||||
def gen_converter_srv(base: str, fields, include_path: str, interfaces_root: str):
|
||||
cpp = []
|
||||
cpp.append(f'// {base}::Request: ' + ', '.join([n for _,n in fields]))
|
||||
cpp.append('template<>')
|
||||
@@ -254,45 +389,7 @@ def gen_converter_srv(base: str, fields, include_path: str):
|
||||
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:
|
||||
if not emit_complex_field(cpp, name, base_type, is_array, 'r', interfaces_root):
|
||||
cpp.append(f' // TODO: parse field `{name}` of ROS type `{ty}` into r.{name} (complex type)')
|
||||
cpp.append(' }')
|
||||
cpp.append(' return r;')
|
||||
@@ -378,7 +475,8 @@ def main():
|
||||
fields = []
|
||||
else:
|
||||
fields = parse_action_file(action_file)
|
||||
converters.append(gen_converter_action(base, fields, header_snake))
|
||||
add_msg_includes_for_fields(fields, includes)
|
||||
converters.append(gen_converter_action(base, fields, header_snake, interfaces_root))
|
||||
added_actions.add(base)
|
||||
elif kind.startswith('srv') or kind.startswith('service'):
|
||||
if base in added_srvs:
|
||||
@@ -408,7 +506,8 @@ def main():
|
||||
fields = []
|
||||
else:
|
||||
fields = parse_srv_file(srv_file)
|
||||
converters.append(gen_converter_srv(base, fields, header_snake))
|
||||
add_msg_includes_for_fields(fields, includes)
|
||||
converters.append(gen_converter_srv(base, fields, header_snake, interfaces_root))
|
||||
added_srvs.add(base)
|
||||
# end of iface loop
|
||||
pass
|
||||
@@ -427,7 +526,8 @@ def main():
|
||||
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))
|
||||
add_msg_includes_for_fields(fields, includes)
|
||||
converters.append(gen_converter_action(base_type, fields, header_snake, interfaces_root))
|
||||
added_actions.add(base_type)
|
||||
|
||||
srv_dir = resolve_interface_subdir(interfaces_root, 'srv')
|
||||
@@ -442,7 +542,8 @@ def main():
|
||||
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))
|
||||
add_msg_includes_for_fields(fields, includes)
|
||||
converters.append(gen_converter_srv(base_type, fields, header_snake, interfaces_root))
|
||||
added_srvs.add(base_type)
|
||||
|
||||
# assemble header
|
||||
|
||||
@@ -14,6 +14,7 @@ The script is intentionally conservative: it emits empty default_topic values; y
|
||||
"""
|
||||
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
import yaml
|
||||
from pathlib import Path
|
||||
@@ -118,6 +119,14 @@ for entry in skills:
|
||||
if base not in service_default_map or skill_name == base:
|
||||
service_default_map[base] = dtopic
|
||||
|
||||
# Ensure all available interfaces are included so traits exist for any action/srv used in code
|
||||
for a in actions:
|
||||
if a not in skill_action_types:
|
||||
skill_action_types.append(a)
|
||||
for s in services:
|
||||
if s not in skill_service_types:
|
||||
skill_service_types.append(s)
|
||||
|
||||
# Helper to map base name to include path / C++ type name.
|
||||
# Assumption: interfaces live in package `interfaces` and types are under `interfaces::action::Name` or `interfaces::srv::Name`.
|
||||
|
||||
@@ -128,15 +137,10 @@ def service_cpp_type(base):
|
||||
return f"interfaces::srv::{base}"
|
||||
|
||||
def to_snake_case(s: str) -> str:
|
||||
out = []
|
||||
for i, ch in enumerate(s):
|
||||
if ch.isupper():
|
||||
if i != 0:
|
||||
out.append('_')
|
||||
out.append(ch.lower())
|
||||
else:
|
||||
out.append(ch)
|
||||
return ''.join(out)
|
||||
# Convert CamelCase (including initialisms like ASRRecognize) to snake_case
|
||||
s1 = re.sub(r'(.)([A-Z][a-z]+)', r'\1_\2', s)
|
||||
s2 = re.sub(r'([a-z0-9])([A-Z])', r'\1_\2', s1)
|
||||
return s2.lower()
|
||||
|
||||
|
||||
def parse_action_result_fields(action_file: Path):
|
||||
|
||||
Reference in New Issue
Block a user