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

add gen_skill_config.py

Browse Source
This commit is contained in:
david
2025-10-19 10:31:10 +08:00
parent 63e58b7897
commit a8f209bead
4 changed files with 367 additions and 1221 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/brain/config/robot_skills.yaml
View File

@@ -14,7 +14,8 @@
version: 1.0.0
description: "手臂控制"
interfaces:
- Arm.action
- name: Arm.action
default_topic: "ArmAction"
- name: WaistControl
version: 1.0.0

187
src/brain/include/brain/skill_config.hpp
View File

@@ -1,161 +1,148 @@
// Generated by scripts/gen_skill_config.py — do not edit by hand unless you know what you are doing.
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <tuple>
#include <string>
#include "interfaces/action/arm.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/vision_grasp_object.hpp"
#include "interfaces/srv/vision_object_recognition.hpp"
#include "interfaces/action/arm.hpp"
#include "interfaces/action/slam_mode.hpp"
#include "interfaces/srv/map_save.hpp"
#include "interfaces/srv/map_load.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "interfaces/action/camera_take_photo.hpp"
#include "interfaces/action/slam_mode.hpp"
#include "interfaces/action/vision_grasp_object.hpp"
#include "interfaces/action/waist_control.hpp"
#include "interfaces/srv/map_load.hpp"
#include "interfaces/srv/map_save.hpp"
#include "interfaces/srv/vision_object_recognition.hpp"
using interfaces::action::ArmSpaceControl;
using interfaces::action::Arm;
using interfaces::action::HandControl;
using interfaces::action::LegControl;
using interfaces::action::SlamMode;
using nav2_msgs::action::NavigateToPose;
using interfaces::srv::MapSave;
using interfaces::srv::MapLoad;
using interfaces::action::VisionGraspObject;
using interfaces::srv::VisionObjectRecognition;
using interfaces::action::CameraTakePhoto;
using interfaces::action::WaistControl;
namespace brain {
namespace brain
{
template<typename ActionT>
struct SkillActionTrait;
/**
* @brief Tuple listing of all supported skill action types for dispatcher iteration.
*/
using SkillActionTypes = std::tuple<
// ArmSpaceControl,
Arm,
HandControl,
LegControl,
// VisionGraspObject,
// SlamMode,
// NavigateToPose,
CameraTakePhoto,
WaistControl
>;
template<typename ServiceT>
struct SkillServiceTrait;
using SkillServiceTypes = std::tuple<
// MapSave,
// MapLoad,
// VisionObjectRecognition
using SkillActionTypes = std::tuple<
interfaces::action::ArmSpaceControl,
interfaces::action::Arm,
interfaces::action::WaistControl,
interfaces::action::CameraTakePhoto,
interfaces::action::HandControl,
interfaces::action::LegControl,
interfaces::action::SlamMode,
nav2_msgs::action::NavigateToPose,
interfaces::action::VisionGraspObject
>;
/**
* @brief Primary template (unspecialized) for skill action trait extraction.
* Specializations must define: skill_name (constexpr const char*), success(result), message(result).
*/
template<typename ActionT>
struct SkillActionTrait; // intentionally undefined primary template
using SkillServiceTypes = std::tuple<
interfaces::srv::VisionObjectRecognition,
interfaces::srv::MapSave,
interfaces::srv::MapLoad
>;
template<>
struct SkillActionTrait<ArmSpaceControl>
struct SkillActionTrait<interfaces::action::ArmSpaceControl>
{
static constexpr const char * skill_name = "ArmSpaceControl";
static constexpr const char * default_topic = "";
static bool success(const ArmSpaceControl::Result & r) {return r.success;}
static std::string message(const ArmSpaceControl::Result & r) {return r.message;}
static bool success(const interfaces::action::ArmSpaceControl::Result & r) {return r.success;}
static std::string message(const interfaces::action::ArmSpaceControl::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<Arm>
struct SkillActionTrait<interfaces::action::Arm>
{
static constexpr const char * skill_name = "Arm";
static constexpr const char * default_topic = "ArmAction";
static bool success(const Arm::Result & r) {return (r.result == 0)? true:false;}
static std::string message(const Arm::Result & r) {(void)r;return "completed";}
static bool success(const interfaces::action::Arm::Result & r) {return (r.result == 0);}
static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
};
template<>
struct SkillActionTrait<HandControl>
{
static constexpr const char * skill_name = "HandControl";
static constexpr const char * default_topic = "";
static bool success(const HandControl::Result & r) {return r.success;}
static std::string message(const HandControl::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<CameraTakePhoto>
{
static constexpr const char * skill_name = "CameraTakePhoto";
static constexpr const char * default_topic = "";
static bool success(const CameraTakePhoto::Result & r) {return r.success;}
static std::string message(const CameraTakePhoto::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<WaistControl>
struct SkillActionTrait<interfaces::action::WaistControl>
{
static constexpr const char * skill_name = "WaistControl";
static constexpr const char * default_topic = "";
static bool success(const WaistControl::Result & r) {return r.success;}
static std::string message(const WaistControl::Result & r) {return r.message;}
static bool success(const interfaces::action::WaistControl::Result & r) {return r.success;}
static std::string message(const interfaces::action::WaistControl::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<LegControl>
struct SkillActionTrait<interfaces::action::CameraTakePhoto>
{
static constexpr const char * skill_name = "CameraTakePhoto";
static constexpr const char * default_topic = "";
static bool success(const interfaces::action::CameraTakePhoto::Result & r) {return r.success;}
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) {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 LegControl::Result & r) {return r.success;}
static std::string message(const LegControl::Result & r) {return r.message;}
static bool success(const interfaces::action::LegControl::Result & r) {return r.success;}
static std::string message(const interfaces::action::LegControl::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<VisionGraspObject>
{
static constexpr const char * skill_name = "VisionGraspObject";
static constexpr const char * default_topic = "";
static bool success(const VisionGraspObject::Result & r) {return r.success;}
static std::string message(const VisionGraspObject::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<SlamMode>
struct SkillActionTrait<interfaces::action::SlamMode>
{
static constexpr const char * skill_name = "SlamMode";
static constexpr const char * default_topic = "";
static bool success(const SlamMode::Result & r) {return r.success;}
static std::string message(const SlamMode::Result & r) {return r.message;}
static bool success(const interfaces::action::SlamMode::Result & r) {return r.success;}
static std::string message(const interfaces::action::SlamMode::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<nav2_msgs::action::NavigateToPose>
{
static constexpr const char * skill_name = "NavigateToPose";
static constexpr const char * default_topic = "";
static bool success(const nav2_msgs::action::NavigateToPose::Result & r) {(void)r;return true;}
static std::string message(const nav2_msgs::action::NavigateToPose::Result & r) {(void)r;return "completed";}
static bool success(const nav2_msgs::action::NavigateToPose::Result & r) {(void)r; return true;}
static std::string message(const nav2_msgs::action::NavigateToPose::Result & r) {(void)r; return "completed";}
};
// Service trait specializations for service registrar name extraction
template<>
struct SkillServiceTrait<MapSave>
struct SkillActionTrait<interfaces::action::VisionGraspObject>
{
static constexpr const char * skill_name = "MapSave";
static constexpr const char * skill_name = "VisionGraspObject";
static constexpr const char * default_topic = "";
static bool success(const interfaces::action::VisionGraspObject::Result & r) {return r.success;}
static std::string message(const interfaces::action::VisionGraspObject::Result & r) {return r.message;}
};
template<>
struct SkillServiceTrait<MapLoad>
{
static constexpr const char * skill_name = "MapLoad";
static constexpr const char * default_topic = "";
};
template<>
struct SkillServiceTrait<VisionObjectRecognition>
struct SkillServiceTrait<interfaces::srv::VisionObjectRecognition>
{
static constexpr const char * skill_name = "VisionObjectRecognition";
static constexpr const char * default_topic = "";
};
} // namespace brain
template<>
struct SkillServiceTrait<interfaces::srv::MapSave>
{
static constexpr const char * skill_name = "MapSave";
static constexpr const char * default_topic = "";
};
template<>
struct SkillServiceTrait<interfaces::srv::MapLoad>
{
static constexpr const char * skill_name = "MapLoad";
static constexpr const char * default_topic = "";
};
} // namespace brain

278
src/scripts/gen_skill_config.py Executable file
View File

@@ -0,0 +1,278 @@
#!/usr/bin/env python3
"""
Generate C++ header `skill_config.hpp` from YAML skill descriptions and interface definitions.
Usage: ./gen_skill_config.py
- Reads `src/brain/config/robot_skills.yaml` for skill entries.
- Scans sibling package `../hivecore_robot_interfaces/src/{action,srv}` for available interfaces.
- Writes `src/brain/include/brain/skill_config.hpp` containing:
- SkillActionTypes / SkillServiceTypes tuples
- SkillActionTrait and SkillServiceTrait specializations with `skill_name` and `default_topic` fields
The script is intentionally conservative: it emits empty default_topic values; you can edit generated header or extend the YAML to include explicit default_topic overrides.
"""
import os
import sys
import yaml
from pathlib import Path
ROOT = Path(__file__).resolve().parents[2]
SKILL_YAML = ROOT / 'src' / 'brain' / 'config' / 'robot_skills.yaml'
INTERFACES_DIR = ROOT.parent / 'hivecore_robot_interfaces' / 'src'
OUT_HEADER = ROOT / 'src' / 'brain' / 'include' / 'brain' / 'skill_config.hpp'
if not SKILL_YAML.exists():
print(f"Skill YAML not found: {SKILL_YAML}")
sys.exit(1)
if not INTERFACES_DIR.exists():
print(f"Interfaces dir not found: {INTERFACES_DIR}")
sys.exit(1)
# Gather available action and srv base names from the interfaces package
action_dir = INTERFACES_DIR / 'action'
srv_dir = INTERFACES_DIR / 'srv'
actions = []
services = []
if action_dir.exists():
for p in sorted(action_dir.iterdir()):
if p.suffix == '.action':
actions.append(p.stem)
if srv_dir.exists():
for p in sorted(srv_dir.iterdir()):
if p.suffix == '.srv':
services.append(p.stem)
# Read skills YAML
with SKILL_YAML.open() as f:
skills = yaml.safe_load(f)
# Extract unique action/service types referenced by skills in the YAML
skill_action_types = []
skill_service_types = []
# allow optional default_topic per interface: map base -> default_topic
action_default_map = {}
service_default_map = {}
def parse_iface_item(item):
"""Return (base, kind, default_topic) for an interface YAML item.
Supported forms:
- "Arm.action" (string)
- { name: "Arm.action", default_topic: "ArmAction" }
- "Arm.action": { default_topic: "ArmAction" } (less common)
"""
if isinstance(item, str):
s = item
if s.endswith('.action'):
return s[:-len('.action')], 'action', ''
if s.endswith('.srv'):
return s[:-len('.srv')], 'srv', ''
return None
if isinstance(item, dict):
# dict can be either { name: ... , default_topic: ... } or { "Arm.action": { default_topic: ... } }
if 'name' in item:
name = item['name']
d = item.get('default_topic', '')
if name.endswith('.action'):
return name[:-len('.action')], 'action', d
if name.endswith('.srv'):
return name[:-len('.srv')], 'srv', d
return None
# fallback: single kv pair mapping
if len(item) == 1:
k, v = next(iter(item.items()))
if isinstance(v, dict):
d = v.get('default_topic', '')
if k.endswith('.action'):
return k[:-len('.action')], 'action', d
if k.endswith('.srv'):
return k[:-len('.srv')], 'srv', d
return None
for entry in skills:
if 'interfaces' not in entry:
continue
for iface in entry['interfaces']:
parsed = parse_iface_item(iface)
if not parsed:
continue
base, kind, dtopic = parsed
if kind == 'action':
if base not in skill_action_types:
skill_action_types.append(base)
if dtopic:
action_default_map[base] = dtopic
elif kind == 'srv':
if base not in skill_service_types:
skill_service_types.append(base)
if dtopic:
service_default_map[base] = dtopic
# 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`.
def action_cpp_type(base):
return f"interfaces::action::{base}"
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)
def parse_action_result_fields(action_file: Path):
"""Parse the Result section of an .action file and return a dict name->type.
If the file doesn't exist or parsing fails, return empty dict.
"""
if not action_file.exists():
return {}
text = action_file.read_text()
parts = [p.strip() for p in text.split('---')]
# Expect parts: [goal, result, feedback] usually. We want parts[1] if exists.
if len(parts) < 2:
return {}
result_section = parts[1]
fields = {}
for line in result_section.splitlines():
line = line.strip()
if not line or line.startswith('#'):
continue
# remove inline comments
if '#' in line:
line = line.split('#', 1)[0].strip()
toks = line.split()
if len(toks) < 2:
continue
typ = toks[0]
name = toks[1]
# strip any array or filler
name = name.split('[')[0]
fields[name] = typ
return fields
def pick_success_and_message_fields(result_fields: dict):
"""Given a mapping name->type, choose success_field and message_field heuristically.
Returns (success_field_name_or_None, message_field_name_or_None, success_kind)
success_kind: 'bool', 'int_zero', or None
"""
# normalize types
bool_names = []
int_names = []
string_names = []
for name, typ in result_fields.items():
t = typ.lower()
if 'bool' in t:
bool_names.append(name)
elif any(x in t for x in ('int8', 'int16', 'int32', 'int64', 'uint8', 'uint16', 'uint32', 'uint64', 'byte', 'char', 'long')):
int_names.append(name)
elif 'string' in t or 'char' in t:
string_names.append(name)
else:
# fallback: treat unknown as string if name suggests so
if 'msg' in name or 'message' in name or 'detail' in name:
string_names.append(name)
# choose success
if 'success' in result_fields and 'success' in bool_names:
return 'success', ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), 'bool'
if bool_names:
return bool_names[0], ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), 'bool'
if 'result' in result_fields and 'result' in int_names:
return 'result', ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), 'int_zero'
if int_names:
return int_names[0], ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), 'int_zero'
# no good numeric/bool -> fallback
return None, ( 'message' if 'message' in result_fields and 'message' in string_names else (string_names[0] if string_names else None) ), None
# Generate header
OUT_HEADER.parent.mkdir(parents=True, exist_ok=True)
with OUT_HEADER.open('w') as out:
out.write('// Generated by scripts/gen_skill_config.py — do not edit by hand unless you know what you are doing.\n')
out.write('#pragma once\n\n')
out.write('#include <tuple>\n')
out.write('#include <string>\n')
out.write('\n')
# includes
for a in sorted(skill_action_types):
out.write(f'#include "interfaces/action/{to_snake_case(a)}.hpp"\n')
for s in sorted(skill_service_types):
out.write(f'#include "interfaces/srv/{to_snake_case(s)}.hpp"\n')
out.write('\nnamespace brain {\n\n')
# forward templates
out.write('template<typename ActionT>\nstruct SkillActionTrait;\n\n')
out.write('template<typename ServiceT>\nstruct SkillServiceTrait;\n\n')
# tuples
out.write('using SkillActionTypes = std::tuple<\n')
for i, a in enumerate(skill_action_types):
sep = ',' if i + 1 < len(skill_action_types) else ''
out.write(f' {action_cpp_type(a)}{sep}\n')
out.write('>;\n\n')
out.write('using SkillServiceTypes = std::tuple<\n')
for i, s in enumerate(skill_service_types):
sep = ',' if i + 1 < len(skill_service_types) else ''
out.write(f' {service_cpp_type(s)}{sep}\n')
out.write('>;\n\n')
# specializations
for a in skill_action_types:
d = action_default_map.get(a, '')
d_esc = d.replace('"', '\\"')
out.write(f'template<>\nstruct SkillActionTrait<{action_cpp_type(a)}>\n{{\n')
out.write(f' static constexpr const char * skill_name = "{a}";\n')
out.write(f' static constexpr const char * default_topic = "{d_esc}";\n')
# attempt to parse the corresponding .action file to synthesize success/message helpers
# Try multiple filename candidates: CamelCase, snake_case, lowercase
action_dir_root = INTERFACES_DIR / 'action'
candidates = [
action_dir_root / f"{a}.action",
action_dir_root / f"{to_snake_case(a)}.action",
action_dir_root / f"{a.lower()}.action",
]
action_file_found = None
for c in candidates:
if c.exists():
action_file_found = c
break
result_fields = parse_action_result_fields(action_file_found) if action_file_found is not None else {}
success_field, message_field, success_kind = pick_success_and_message_fields(result_fields)
if success_kind == 'bool' and success_field is not None:
out.write(f' static bool success(const {action_cpp_type(a)}::Result & r) {{return r.{success_field};}}\n')
elif success_kind == 'int_zero' and success_field is not None:
out.write(f' static bool success(const {action_cpp_type(a)}::Result & r) {{return (r.{success_field} == 0);}}\n')
else:
out.write(f' static bool success(const {action_cpp_type(a)}::Result & r) {{(void)r; return true;}}\n')
if message_field is not None:
out.write(f' static std::string message(const {action_cpp_type(a)}::Result & r) {{return r.{message_field};}}\n')
else:
out.write(f' static std::string message(const {action_cpp_type(a)}::Result & r) {{(void)r; return "completed";}}\n')
out.write('};\n\n')
for s in skill_service_types:
d = service_default_map.get(s, '')
d_esc = d.replace('"', '\\"')
out.write(f'template<>\nstruct SkillServiceTrait<{service_cpp_type(s)}>\n{{\n')
out.write(f' static constexpr const char * skill_name = "{s}";\n')
out.write(f' static constexpr const char * default_topic = "{d_esc}";\n')
out.write('};\n\n')
out.write('} // namespace brain\n')
print(f'Wrote {OUT_HEADER}')
# Make script executable
os.chmod(Path(__file__), 0o755)
print('Done.')

1120
src/scripts/gen_skill_manager.py
View File

File diff suppressed because it is too large Load Diff