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

optimize skill manager

Browse Source
This commit is contained in:
david
2025-10-19 00:55:17 +08:00
parent 15262389cd
commit ad6202deff
3 changed files with 300 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,7 +2,7 @@
version: 1.0.0
description: "视觉识别指定物体"
interfaces:
- VisionObjectRecognition.service
- VisionObjectRecognition.srv
- name: ArmSpaceControl
version: 1.0.0
@@ -50,13 +50,13 @@
version: 1.0.0
description: "保存导航地图"
interfaces:
- MapSave.service
- MapSave.srv
- name: MapLoad
version: 1.0.0
description: "加载导航地图"
interfaces:
- MapLoad.service
- MapLoad.srv
- name: NavigateToPose
version: 1.0.0

277
src/brain/include/brain/skill_manager.hpp
View File

@@ -21,16 +21,55 @@
#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/waist_control.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
{
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
>;
/**
* @brief Specification of a skill as parsed from YAML.
*
@@ -111,6 +150,41 @@ private:
public:
// Public helpers for registrars to avoid accessing private members directly from free functions
template<typename ActionT>
void register_action_for_base(
const std::string & base,
const std::string & topic,
const std::string & internal_skill)
{
register_action_client_default<ActionT>(base, topic, internal_skill);
}
template<typename ServiceT>
void register_service_client(
const std::string & topic,
const std::string & internal_skill)
{
// Try reuse existing client if already created under either key
std::shared_ptr<rclcpp::Client<ServiceT>> typed_existing;
auto it_topic = service_client_map_.find(topic);
if (it_topic != service_client_map_.end()) {
typed_existing = std::dynamic_pointer_cast<rclcpp::Client<ServiceT>>(it_topic->second);
}
if (!typed_existing) {
auto it_name = service_client_map_.find(internal_skill);
if (it_name != service_client_map_.end()) {
typed_existing = std::dynamic_pointer_cast<rclcpp::Client<ServiceT>>(it_name->second);
}
}
auto client = typed_existing ? typed_existing : node_->create_client<ServiceT>(topic);
if (!typed_existing) {
service_clients_.push_back(client);
}
service_client_map_[internal_skill] = client;
service_client_map_[topic] = client;
}
template<typename ServiceT>
struct ServiceHookOptions
{
@@ -325,121 +399,6 @@ private:
}
};
/**
* @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
template<>
struct SkillActionTrait<interfaces::action::ArmSpaceControl>
{
static constexpr const char * skill_name = "ArmSpaceControl";
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<interfaces::action::Arm>
{
static constexpr const char * skill_name = "Arm";
static bool success(const interfaces::action::Arm::Result & r) {return (r.result == 0) ? true : false;}
static std::string message(const interfaces::action::Arm::Result & r)
{
(void)r;
return "r.message";
}
};
template<>
struct SkillActionTrait<interfaces::action::HandControl>
{
static constexpr const char * skill_name = "HandControl";
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::CameraTakePhoto>
{
static constexpr const char * skill_name = "CameraTakePhoto";
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::WaistControl>
{
static constexpr const char * skill_name = "WaistControl";
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<interfaces::action::LegControl>
{
static constexpr const char * skill_name = "LegControl";
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<interfaces::action::VisionGraspObject>
{
static constexpr const char * skill_name = "VisionGraspObject";
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 SkillActionTrait<interfaces::action::SlamMode>
{
static constexpr const char * skill_name = "SlamMode";
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 bool success(const nav2_msgs::action::NavigateToPose::Result & r)
{
(void)r;
// NavigateToPose::Result carries an empty payload; success is inferred from the
// rclcpp_action::ResultCode before this helper is invoked.
return true;
}
static std::string message(const nav2_msgs::action::NavigateToPose::Result & r)
{
(void)r;
return "NavigateToPose completed";
}
};
/**
* @brief Tuple listing of all supported skill action types for dispatcher iteration.
*/
using SkillActionTypes = std::tuple<
interfaces::action::ArmSpaceControl,
interfaces::action::Arm,
interfaces::action::HandControl,
interfaces::action::LegControl,
interfaces::action::VisionGraspObject,
interfaces::action::SlamMode,
nav2_msgs::action::NavigateToPose,
interfaces::action::CameraTakePhoto,
interfaces::action::WaistControl
>;
/**
* @brief Recursive compile-time dispatcher that matches a skill name to an action type and invokes send_and_wait.
* @tparam I Current tuple index (internal use, defaults to 0).
@@ -477,5 +436,93 @@ bool dispatch_skill_action(
}
}
/**
* @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
template<>
struct SkillActionTrait<ArmSpaceControl>
{
static constexpr const char * skill_name = "ArmSpaceControl";
static bool success(const ArmSpaceControl::Result & r) {return r.success;}
static std::string message(const ArmSpaceControl::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<Arm>
{
static constexpr const char * skill_name = "Arm";
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";}
};
template<>
struct SkillActionTrait<HandControl>
{
static constexpr const char * skill_name = "HandControl";
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 bool success(const CameraTakePhoto::Result & r) {return r.success;}
static std::string message(const CameraTakePhoto::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<WaistControl>
{
static constexpr const char * skill_name = "WaistControl";
static bool success(const WaistControl::Result & r) {return r.success;}
static std::string message(const WaistControl::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<LegControl>
{
static constexpr const char * skill_name = "LegControl";
static bool success(const LegControl::Result & r) {return r.success;}
static std::string message(const LegControl::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<VisionGraspObject>
{
static constexpr const char * skill_name = "VisionGraspObject";
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>
{
static constexpr const char * skill_name = "SlamMode";
static bool success(const SlamMode::Result & r) {return r.success;}
static std::string message(const SlamMode::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<nav2_msgs::action::NavigateToPose>
{
static constexpr const char * skill_name = "NavigateToPose";
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>
{
static constexpr const char * skill_name = "MapSave";
};
template<>
struct SkillServiceTrait<MapLoad>
{
static constexpr const char * skill_name = "MapLoad";
};
template<>
struct SkillServiceTrait<VisionObjectRecognition>
{
static constexpr const char * skill_name = "VisionObjectRecognition";
};
} // namespace brain

245
src/brain/src/skill_manager.cpp
View File

@@ -15,28 +15,6 @@
#include <utility>
#include <rclcpp_action/rclcpp_action.hpp>
#include "interfaces/action/arm_space_control.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/slam_mode.hpp"
#include "interfaces/srv/map_save.hpp"
#include "interfaces/srv/map_load.hpp"
#include "interfaces/action/arm.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "interfaces/action/waist_control.hpp"
#include "interfaces/action/camera_take_photo.hpp"
using interfaces::action::ArmSpaceControl;
using interfaces::action::Arm;
using interfaces::action::HandControl;
using interfaces::action::LegControl;
using interfaces::action::VisionGraspObject;
using interfaces::srv::VisionObjectRecognition;
using interfaces::action::CameraTakePhoto;
using interfaces::action::WaistControl;
namespace brain
{
@@ -155,6 +133,102 @@ std::vector<std::pair<std::string, std::string>> SkillManager::enumerate_action_
return out;
}
using ActionRegistrar = void (*)(SkillManager * self, const std::string & topic, const std::string & internal_skill);
using ServiceRegistrar = void (*)(SkillManager * self, const std::string & topic, const std::string & internal_skill);
// Registrar function templates (no captures, usable as function pointers)
template<typename ActionT>
static void action_register(SkillManager * self, const std::string & topic, const std::string & internal_skill)
{
self->register_action_for_base<ActionT>(SkillActionTrait<ActionT>::skill_name, topic, internal_skill);
}
template<typename ActionT>
static constexpr std::pair<const char *, ActionRegistrar> make_action_entry()
{
return {SkillActionTrait<ActionT>::skill_name, &action_register<ActionT>};
}
template<typename ServiceT>
static void service_register(SkillManager * self, const std::string & topic, const std::string & internal_skill)
{
self->register_service_client<ServiceT>(topic, internal_skill);
}
template<typename ServiceT>
static constexpr std::pair<const char *, ServiceRegistrar> make_service_entry(const char * name)
{
return {name, &service_register<ServiceT>};
}
// Helper to build the service map by expanding tuple indices (C++17-compatible).
template<typename TupleT, std::size_t... I>
static std::unordered_map<std::string, ServiceRegistrar> build_service_map(std::index_sequence<I...>) {
std::unordered_map<std::string, ServiceRegistrar> m;
(void)std::array<int, sizeof...(I)>{{(m.emplace(
std::string(SkillServiceTrait<std::tuple_element_t<I, TupleT>>::skill_name),
&service_register<std::tuple_element_t<I, TupleT>>), 0)...}};
return m;
}
// Helper to build the map by expanding tuple indices. Implemented as a template
// function rather than a templated lambda for C++17 compatibility.
template<typename TupleT, std::size_t... I>
static std::unordered_map<std::string, ActionRegistrar> build_action_map(std::index_sequence<I...>) {
std::unordered_map<std::string, ActionRegistrar> m;
// Expand the pack by using a dummy array and the comma operator to call emplace for each index.
// This avoids constructing nested initializer_list objects and is C++17 compatible.
(void)std::array<int, sizeof...(I)>{{(m.emplace(
std::string(SkillActionTrait<std::tuple_element_t<I, TupleT>>::skill_name),
&action_register<std::tuple_element_t<I, TupleT>>), 0)...}};
return m;
}
// Factory methods to access static registries
static const std::unordered_map<std::string, ActionRegistrar> & get_action_registrars()
{
// Build the registrar map by iterating the SkillActionTypes tuple at compile time.
using TupleT = SkillActionTypes;
constexpr std::size_t N = std::tuple_size<TupleT>::value;
static const std::unordered_map<std::string, ActionRegistrar> kMap = build_action_map<TupleT>(std::make_index_sequence<N>{});
return kMap;
}
static const std::unordered_map<std::string, ServiceRegistrar> & get_service_registrars()
{
using TupleT = SkillServiceTypes;
constexpr std::size_t N = std::tuple_size<TupleT>::value;
static const std::unordered_map<std::string, ServiceRegistrar> kMap = build_service_map<TupleT>(std::make_index_sequence<N>{});
return kMap;
}
// Utility: join map keys for diagnostics
template<typename T>
static std::string join_keys(const std::unordered_map<std::string, T> & m)
{
std::string out;
bool first = true;
for (const auto & kv : m) {
if (!first) {out += ", ";}
first = false;
out += kv.first;
}
return out;
}
template<size_t I = 0>
static void validate_action_registrars_complete(rclcpp::Logger logger)
{
if constexpr (I < std::tuple_size<SkillActionTypes>::value) {
using ActionT = std::tuple_element_t<I, SkillActionTypes>;
const char * name = SkillActionTrait<ActionT>::skill_name;
if (get_action_registrars().find(name) == get_action_registrars().end()) {
RCLCPP_ERROR(logger, "Missing action registrar for '%s'", name);
}
validate_action_registrars_complete<I + 1>(logger);
}
}
/**
* @brief Register action and service interfaces declared by a SkillSpec.
*
@@ -165,75 +239,24 @@ std::vector<std::pair<std::string, std::string>> SkillManager::enumerate_action_
*/
void SkillManager::register_interfaces_(const SkillSpec & s)
{
static const std::unordered_map<std::string, std::function<void(const std::string &,
const std::string &)>> action_registrars = {
{"ArmSpaceControl", [this](const std::string & topic, const std::string & internal_skill) {
register_action_client_default<ArmSpaceControl>("ArmSpaceControl", topic, internal_skill);
}},
{"Arm", [this](const std::string & topic, const std::string & internal_skill) {
(void)topic;
register_action_client_default<Arm>("Arm", "ArmAction", internal_skill);
}},
{"HandControl", [this](const std::string & topic, const std::string & internal_skill) {
register_action_client_default<HandControl>("HandControl", topic, internal_skill);
}},
{"WaistControl", [this](const std::string & topic, const std::string & internal_skill) {
register_action_client_default<WaistControl>("WaistControl", topic, internal_skill);
}},
{"CameraTakePhoto", [this](const std::string & topic, const std::string & internal_skill) {
register_action_client_default<CameraTakePhoto>("CameraTakePhoto", topic, internal_skill);
}},
{"LegControl", [this](const std::string & topic, const std::string & internal_skill) {
register_action_client_default<LegControl>("LegControl", topic, internal_skill);
}},
{"VisionGraspObject", [this](const std::string & topic, const std::string & internal_skill) {
register_action_client_default<VisionGraspObject>("VisionGraspObject", topic, internal_skill);
}},
{"SlamMode", [this](const std::string & topic, const std::string & internal_skill) {
register_action_client_default<interfaces::action::SlamMode>("SlamMode", topic, internal_skill);
}},
{"NavigateToPose", [this](const std::string & topic, const std::string & internal_skill) {
register_action_client_default<nav2_msgs::action::NavigateToPose>("NavigateToPose", topic, internal_skill);
}},
};
static const std::unordered_map<std::string, std::function<void(const std::string &,
const std::string &)>> service_registrars = {
{"VisionObjectRecognition",
[this](const std::string & topic, const std::string & internal_skill) {
auto client = node_->create_client<VisionObjectRecognition>(topic);
service_clients_.push_back(client);
service_client_map_[internal_skill] = client;
service_client_map_[topic] = client;
}},
{"MapSave",
[this](const std::string & topic, const std::string & internal_skill) {
auto client = node_->create_client<interfaces::srv::MapSave>(topic);
service_clients_.push_back(client);
service_client_map_[internal_skill] = client;
service_client_map_[topic] = client;
}},
{"MapLoad",
[this](const std::string & topic, const std::string & internal_skill) {
auto client = node_->create_client<interfaces::srv::MapLoad>(topic);
service_clients_.push_back(client);
service_client_map_[internal_skill] = client;
service_client_map_[topic] = client;
}},
};
// One-time runtime validation to ensure all actions in SkillActionTypes have a registrar.
static bool s_validated = false;
if (!s_validated) {
validate_action_registrars_complete(node_->get_logger());
s_validated = true;
}
auto join_topic = [](const std::string & prefix, const std::string & base) {
if (prefix.empty()) {return base;}
if (prefix == "/") {return std::string{"/"} + base;}
if (prefix.back() == '/') {return prefix + base;}
return prefix + "/" + base;
};
if (prefix.empty()) {return base;}
if (prefix == "/") {return std::string{"/"} + base;}
if (prefix.back() == '/') {return prefix + base;}
return prefix + "/" + base;
};
for (const auto & iface : s.interfaces) {
auto parsed = parse_interface_entry(iface);
if (!parsed) {
RCLCPP_WARN(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
RCLCPP_ERROR(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
continue;
}
@@ -242,38 +265,40 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
const std::string base_snake = to_snake_case(parsed->base);
auto resolve_topic_name = [&](const std::string & kind_suffix) {
const std::string default_topic = join_topic(prefix_value, base_snake);
const std::string param_name = base_snake + "." + kind_suffix + "_name";
std::string topic_override;
if (node_->get_parameter(param_name, topic_override)) {
if (topic_override.empty()) {
topic_override = default_topic;
}
return topic_override;
// Arm 动作默认话题使用 "ArmAction"，其余沿用 prefix + base_snake
const bool is_arm_action = (parsed->base == "Arm" && kind_suffix == "action");
const std::string default_topic = is_arm_action ? std::string("ArmAction") : join_topic(prefix_value, base_snake);
const std::string param_name = base_snake + "." + kind_suffix + "_name";
std::string topic_override;
if (node_->get_parameter(param_name, topic_override)) {
if (topic_override.empty()) {
topic_override = default_topic;
}
node_->declare_parameter<std::string>(param_name, default_topic);
return default_topic;
};
return topic_override;
}
node_->declare_parameter<std::string>(param_name, default_topic);
return default_topic;
};
if (parsed->kind == "action") {
const std::string topic_name = resolve_topic_name("action");
auto it = action_registrars.find(parsed->base);
if (it != action_registrars.end()) {
it->second(topic_name, s.name);
const auto & action_regs = get_action_registrars();
auto it = action_regs.find(parsed->base);
if (it != action_regs.end()) {
it->second(this, topic_name, s.name);
} else {
RCLCPP_WARN(
node_->get_logger(), "No action registrar for interface base '%s'",
parsed->base.c_str());
RCLCPP_ERROR(node_->get_logger(), "No action registrar for interface base '%s'. Available: [%s]",
parsed->base.c_str(), join_keys(action_regs).c_str());
}
} else if (parsed->kind == "service") {
const std::string topic_name = resolve_topic_name("service");
auto it = service_registrars.find(parsed->base);
if (it != service_registrars.end()) {
it->second(topic_name, s.name);
} else if (parsed->kind == "srv") {
const std::string topic_name = resolve_topic_name("srv");
const auto & service_regs = get_service_registrars();
auto it = service_regs.find(parsed->base);
if (it != service_regs.end()) {
it->second(this, topic_name, s.name);
} else {
RCLCPP_WARN(
node_->get_logger(), "No service registrar for interface base '%s'",
parsed->base.c_str());
RCLCPP_ERROR(node_->get_logger(), "No service registrar for interface base '%s'. Available: [%s]",
parsed->base.c_str(), join_keys(service_regs).c_str());
}
}
}