From 9bd622a843c4ee077cc607288d13c1c4286be805 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Tue, 20 Jan 2026 16:12:20 +0000 Subject: [PATCH 1/9] feat(discovery): heuristic ROS 2 node discovery - expose nodes as Apps with synthetic Components Implement heuristic/runtime discovery mode (PLAN.md) that maps ROS 2 nodes to SOVD entity hierarchy without requiring a manifest file. Key changes: - ROS 2 nodes are now exposed as Apps (not Components) - Synthetic Components are created by namespace aggregation (e.g., 'powertrain', 'chassis') - Unified entity lookup via EntityInfo/get_entity_info() for handlers - REST routes added for /apps/{id}/faults, /apps/{id}/operations, /apps/{id}/configurations Runtime discovery configuration (ROS parameters): - discovery.runtime.expose_nodes_as_apps (default: true) - discovery.runtime.create_synthetic_components (default: true) - discovery.runtime.grouping_strategy (default: 'namespace') - discovery.runtime.synthetic_component_name_pattern (default: '{area}') Handler updates: - fault_handlers: unified entity lookup for Apps and Components - config_handlers: simplified using get_entity_info() - operation_handlers: returns SOVD-compliant {items:[...], total_count} - component_handlers: aggregates topics/operations from related Apps Tests: - Integration tests updated for new entity model (Apps vs Components) - Discovery tests updated for synthetic component source --- .gitignore | 4 +- .../config/gateway_params.yaml | 43 ++ .../discovery/discovery_manager.hpp | 66 ++ .../discovery/runtime_discovery.hpp | 58 +- .../http/handlers/handler_context.hpp | 29 + .../src/discovery/discovery_manager.cpp | 52 +- .../src/discovery/hybrid_discovery.cpp | 5 +- .../src/discovery/runtime_discovery.cpp | 128 +++- src/ros2_medkit_gateway/src/gateway_node.cpp | 26 +- .../src/http/handlers/config_handlers.cpp | 231 +++--- .../handlers/discovery/component_handlers.cpp | 71 +- .../src/http/handlers/fault_handlers.cpp | 130 ++-- .../src/http/handlers/handler_context.cpp | 60 ++ .../src/http/handlers/operation_handlers.cpp | 245 +++++-- .../src/http/rest_server.cpp | 77 +- .../test/test_discovery_manager.cpp | 10 +- .../test/test_discovery_manifest.test.py | 11 +- .../test/test_integration.test.py | 673 +++++++++--------- 18 files changed, 1290 insertions(+), 629 deletions(-) diff --git a/.gitignore b/.gitignore index 6c2cdb6..0bdc8bc 100644 --- a/.gitignore +++ b/.gitignore @@ -244,4 +244,6 @@ qtcreator-* COLCON_IGNORE AMENT_IGNORE -# End of https://www.toptal.com/developers/gitignore/api/ros2,c++,python \ No newline at end of file +# End of https://www.toptal.com/developers/gitignore/api/ros2,c++,pythonPLAN.md + +PLAN.md \ No newline at end of file diff --git a/src/ros2_medkit_gateway/config/gateway_params.yaml b/src/ros2_medkit_gateway/config/gateway_params.yaml index a4cbf1b..ea5ed48 100644 --- a/src/ros2_medkit_gateway/config/gateway_params.yaml +++ b/src/ros2_medkit_gateway/config/gateway_params.yaml @@ -100,6 +100,49 @@ ros2_medkit_gateway: # - Action operations: Internal action services via GenericClient # No ROS 2 CLI dependencies - pure C++ implementation using ros2_medkit_serialization. + # Discovery Configuration + # Controls how ROS 2 graph entities are mapped to SOVD entities + discovery: + # Discovery mode + # Options: + # - "runtime_only": Use ROS 2 graph introspection only (default) + # - "manifest_only": Only expose manifest-declared entities + # - "hybrid": Manifest as source of truth + runtime linking + mode: "runtime_only" + + # Path to manifest file (required for manifest_only and hybrid modes) + manifest_path: "" + + # Strict manifest validation (reject invalid manifests) + manifest_strict_validation: true + + # Runtime (heuristic) discovery options + # These control how nodes are mapped to SOVD entities in runtime mode + runtime: + # Expose ROS 2 nodes as App entities + # When true, GET /apps returns nodes as Apps (with source="heuristic") + # When false, /apps is empty in runtime mode + # Default: true (new behavior for initial release) + expose_nodes_as_apps: true + + # Create synthetic Component entities that group Apps + # When true, Components are synthetic groupings (by namespace) + # When false, each node is a Component + # Default: true (new behavior for initial release) + create_synthetic_components: true + + # How to group nodes into synthetic components + # Options: + # - "none": Each node = 1 component (default when synthetic off) + # - "namespace": Group by first namespace segment (area) + # Note: "process" grouping is planned for future (requires R&D) + grouping_strategy: "namespace" + + # Naming pattern for synthetic components + # Placeholders: {area} + # Examples: "{area}", "{area}_group", "component_{area}" + synthetic_component_name_pattern: "{area}" + # Authentication Configuration (REQ_INTEROP_086, REQ_INTEROP_087) # JWT-based authentication with Role-Based Access Control (RBAC) auth: diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_manager.hpp index 77a53a5..431c583 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_manager.hpp @@ -60,6 +60,29 @@ DiscoveryMode parse_discovery_mode(const std::string & str); */ std::string discovery_mode_to_string(DiscoveryMode mode); +/** + * @brief Strategy for grouping nodes into synthetic components + */ +enum class ComponentGroupingStrategy { + NONE, ///< Each node = 1 component (current behavior) + NAMESPACE, ///< Group by first namespace segment (area) + // PROCESS // Group by OS process (future - requires R&D) +}; + +/** + * @brief Parse ComponentGroupingStrategy from string + * @param str Strategy string: "none" or "namespace" + * @return Parsed strategy (defaults to NONE) + */ +ComponentGroupingStrategy parse_grouping_strategy(const std::string & str); + +/** + * @brief Convert ComponentGroupingStrategy to string + * @param strategy Grouping strategy + * @return String representation + */ +std::string grouping_strategy_to_string(ComponentGroupingStrategy strategy); + /** * @brief Configuration for discovery */ @@ -67,6 +90,49 @@ struct DiscoveryConfig { DiscoveryMode mode{DiscoveryMode::RUNTIME_ONLY}; std::string manifest_path; bool manifest_strict_validation{true}; + + /** + * @brief Runtime (heuristic) discovery options + * + * These options control how the heuristic discovery strategy + * maps ROS 2 graph entities to SOVD entities. + */ + struct RuntimeOptions { + /** + * @brief Expose ROS 2 nodes as App entities + * + * When true, discover_apps() returns nodes as Apps instead of + * only returning empty (manifest-only behavior). + * Default: true (new behavior for initial release) + */ + bool expose_nodes_as_apps{true}; + + /** + * @brief Create synthetic Component entities that group Apps + * + * When true, Components are synthetic groupings (by namespace). + * When false, each node is a Component (legacy behavior). + * Default: true (new behavior for initial release) + */ + bool create_synthetic_components{true}; + + /** + * @brief How to group nodes into synthetic components + * + * Only used when create_synthetic_components is true. + * - NONE: Each node = 1 component + * - NAMESPACE: Group by first namespace segment (area) + */ + ComponentGroupingStrategy grouping{ComponentGroupingStrategy::NAMESPACE}; + + /** + * @brief Naming pattern for synthetic components + * + * Placeholders: {area} + * Default: "{area}" - uses area name as component ID + */ + std::string synthetic_component_name_pattern{"{area}"}; + } runtime; }; /** diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/runtime_discovery.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/runtime_discovery.hpp index 950b984..dd97127 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/runtime_discovery.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/runtime_discovery.hpp @@ -32,6 +32,11 @@ #include namespace ros2_medkit_gateway { + +// Forward declaration +struct DiscoveryConfig; +enum class ComponentGroupingStrategy; + namespace discovery { /** @@ -45,18 +50,37 @@ namespace discovery { * - Discovers components from ROS 2 nodes * - Discovers topic-based "virtual" components for systems like Isaac Sim * - Enriches components with services, actions, and topics + * - (NEW) Can expose nodes as Apps when configured + * - (NEW) Can create synthetic Components that group Apps * - * @note Apps and Functions are not supported in runtime-only mode. - * Use ManifestDiscoveryStrategy (future) for those entities. + * @note Apps and Functions are not supported in runtime-only mode by default. + * Enable expose_nodes_as_apps to get Apps from runtime discovery. + * Use ManifestDiscoveryStrategy for custom entity definitions. */ class RuntimeDiscoveryStrategy : public DiscoveryStrategy { public: + /** + * @brief Runtime discovery configuration options + */ + struct RuntimeConfig { + bool expose_nodes_as_apps{true}; + bool create_synthetic_components{true}; + ComponentGroupingStrategy grouping{}; + std::string synthetic_component_name_pattern{"{area}"}; + }; + /** * @brief Construct runtime discovery strategy * @param node ROS 2 node for graph introspection (must outlive this strategy) */ explicit RuntimeDiscoveryStrategy(rclcpp::Node * node); + /** + * @brief Set runtime discovery configuration + * @param config Runtime options from DiscoveryConfig + */ + void set_config(const RuntimeConfig & config); + /// @copydoc DiscoveryStrategy::discover_areas std::vector discover_areas() override; @@ -64,7 +88,7 @@ class RuntimeDiscoveryStrategy : public DiscoveryStrategy { std::vector discover_components() override; /// @copydoc DiscoveryStrategy::discover_apps - /// @note Returns empty vector - apps require manifest + /// @note Returns nodes as Apps when expose_nodes_as_apps is enabled std::vector discover_apps() override; /// @copydoc DiscoveryStrategy::discover_functions @@ -80,6 +104,27 @@ class RuntimeDiscoveryStrategy : public DiscoveryStrategy { // Runtime-specific methods (from current DiscoveryManager) // ========================================================================= + /** + * @brief Discover node-based components (individual ROS 2 nodes) + * + * This returns the traditional component discovery where each node + * becomes a Component. Used internally when synthetic components + * are not enabled or for building Apps. + * + * @return Vector of node-based components + */ + std::vector discover_node_components(); + + /** + * @brief Discover synthetic components (grouped by namespace) + * + * Creates aggregated Components that group multiple nodes by namespace. + * Only used when create_synthetic_components is enabled. + * + * @return Vector of synthetic components + */ + std::vector discover_synthetic_components(); + /** * @brief Discover components from topic namespaces (topic-based discovery) * @@ -158,7 +203,14 @@ class RuntimeDiscoveryStrategy : public DiscoveryStrategy { /// Check if a service path is an internal ROS2 service static bool is_internal_service(const std::string & service_path); + /// Derive component ID for a node based on grouping strategy + std::string derive_component_id(const Component & node); + + /// Apply naming pattern for synthetic component ID + std::string apply_component_name_pattern(const std::string & area); + rclcpp::Node * node_; + RuntimeConfig config_; NativeTopicSampler * topic_sampler_{nullptr}; TypeIntrospection * type_introspection_{nullptr}; diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/handler_context.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/handler_context.hpp index 398329d..3f1fead 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/handler_context.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/handler_context.hpp @@ -30,6 +30,23 @@ namespace ros2_medkit_gateway { +/** + * @brief Entity type enumeration for SOVD entities + */ +enum class EntityType { COMPONENT, APP, AREA, FUNCTION, UNKNOWN }; + +/** + * @brief Information about a resolved entity + */ +struct EntityInfo { + EntityType type{EntityType::UNKNOWN}; + std::string id; + std::string namespace_path; + std::string fqn; ///< Fully qualified name (for ROS 2 nodes) + std::string id_field; ///< JSON field name for ID ("component_id", "app_id", etc.) + std::string error_name; ///< Human-readable name for errors ("Component", "App", etc.) +}; + // Forward declarations class GatewayNode; class AuthManager; @@ -89,9 +106,21 @@ class HandlerContext { * @brief Get namespace path for a component * @param component_id Component ID * @return Namespace path or error message + * @deprecated Use get_entity_info instead for unified entity handling */ tl::expected get_component_namespace_path(const std::string & component_id) const; + /** + * @brief Get information about any entity (Component, App, Area, Function) + * + * Searches through all entity types in order: Component, App, Area, Function. + * Returns the first match found. + * + * @param entity_id Entity ID to look up + * @return EntityInfo with resolved details, or EntityInfo with UNKNOWN type if not found + */ + EntityInfo get_entity_info(const std::string & entity_id) const; + /** * @brief Set CORS headers on response if origin is allowed * @param res HTTP response diff --git a/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp b/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp index f705702..99f3d4f 100644 --- a/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp +++ b/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp @@ -39,6 +39,22 @@ std::string discovery_mode_to_string(DiscoveryMode mode) { } } +ComponentGroupingStrategy parse_grouping_strategy(const std::string & str) { + if (str == "namespace") { + return ComponentGroupingStrategy::NAMESPACE; + } + return ComponentGroupingStrategy::NONE; +} + +std::string grouping_strategy_to_string(ComponentGroupingStrategy strategy) { + switch (strategy) { + case ComponentGroupingStrategy::NAMESPACE: + return "namespace"; + default: + return "none"; + } +} + DiscoveryManager::DiscoveryManager(rclcpp::Node * node) : node_(node), runtime_strategy_(std::make_unique(node)) { // Default to runtime strategy @@ -75,6 +91,14 @@ bool DiscoveryManager::initialize(const DiscoveryConfig & config) { } void DiscoveryManager::create_strategy() { + // Configure runtime strategy with runtime options + discovery::RuntimeDiscoveryStrategy::RuntimeConfig runtime_config; + runtime_config.expose_nodes_as_apps = config_.runtime.expose_nodes_as_apps; + runtime_config.create_synthetic_components = config_.runtime.create_synthetic_components; + runtime_config.grouping = config_.runtime.grouping; + runtime_config.synthetic_component_name_pattern = config_.runtime.synthetic_component_name_pattern; + runtime_strategy_->set_config(runtime_config); + switch (config_.mode) { case DiscoveryMode::MANIFEST_ONLY: // In manifest_only mode, we use a special mode where we return manifest entities @@ -93,7 +117,9 @@ void DiscoveryManager::create_strategy() { default: active_strategy_ = runtime_strategy_.get(); - RCLCPP_INFO(node_->get_logger(), "Discovery mode: runtime_only"); + RCLCPP_INFO(node_->get_logger(), "Discovery mode: runtime_only (expose_apps=%s, synthetic_components=%s)", + config_.runtime.expose_nodes_as_apps ? "true" : "false", + config_.runtime.create_synthetic_components ? "true" : "false"); break; } } @@ -157,7 +183,16 @@ std::optional DiscoveryManager::get_app(const std::string & id) { if (manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_app(id); } - return std::nullopt; // No apps in runtime-only mode + // Check runtime apps when expose_nodes_as_apps is enabled + if (config_.runtime.expose_nodes_as_apps) { + auto apps = discover_apps(); + for (const auto & app : apps) { + if (app.id == id) { + return app; + } + } + } + return std::nullopt; } std::optional DiscoveryManager::get_function(const std::string & id) { @@ -200,7 +235,18 @@ std::vector DiscoveryManager::get_apps_for_component(const std::string & co if (manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_apps_for_component(component_id); } - return {}; // No apps in runtime mode + // Filter runtime apps by component_id when expose_nodes_as_apps is enabled + if (config_.runtime.expose_nodes_as_apps) { + std::vector result; + auto apps = discover_apps(); + for (const auto & app : apps) { + if (app.component_id == component_id) { + result.push_back(app); + } + } + return result; + } + return {}; } std::vector DiscoveryManager::get_hosts_for_function(const std::string & function_id) { diff --git a/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp b/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp index 0b66f50..a4988e2 100644 --- a/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp +++ b/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp @@ -125,8 +125,9 @@ void HybridDiscoveryStrategy::perform_linking() { // Get manifest apps auto apps = manifest_manager_->get_apps(); - // Get runtime components - auto runtime_components = runtime_strategy_->discover_components(); + // Get runtime node components (raw nodes, not synthetic groupings) + // Runtime linking needs individual node FQNs to match against manifest bindings + auto runtime_components = runtime_strategy_->discover_node_components(); // Get config for orphan policy auto config = manifest_manager_->get_config(); diff --git a/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp b/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp index 6b88708..ebf7ff1 100644 --- a/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp +++ b/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp @@ -14,6 +14,8 @@ #include "ros2_medkit_gateway/discovery/runtime_discovery.hpp" +#include "ros2_medkit_gateway/discovery/discovery_manager.hpp" + #include #include #include @@ -36,6 +38,13 @@ bool RuntimeDiscoveryStrategy::is_internal_service(const std::string & service_p RuntimeDiscoveryStrategy::RuntimeDiscoveryStrategy(rclcpp::Node * node) : node_(node) { } +void RuntimeDiscoveryStrategy::set_config(const RuntimeConfig & config) { + config_ = config; + RCLCPP_DEBUG(node_->get_logger(), "Runtime discovery config: expose_apps=%s, synthetic_components=%s, grouping=%s", + config_.expose_nodes_as_apps ? "true" : "false", config_.create_synthetic_components ? "true" : "false", + grouping_strategy_to_string(config_.grouping).c_str()); +} + std::vector RuntimeDiscoveryStrategy::discover_areas() { // Extract unique areas from namespaces std::set area_set; @@ -73,6 +82,15 @@ std::vector RuntimeDiscoveryStrategy::discover_areas() { } std::vector RuntimeDiscoveryStrategy::discover_components() { + // If synthetic components are enabled, use grouping logic + if (config_.create_synthetic_components) { + return discover_synthetic_components(); + } + // Default: each node = 1 component (backward compatible) + return discover_node_components(); +} + +std::vector RuntimeDiscoveryStrategy::discover_node_components() { std::vector components; // Pre-build service info map for schema lookups @@ -99,14 +117,25 @@ std::vector RuntimeDiscoveryStrategy::discover_components() { refresh_topic_map(); } + // Deduplicate nodes - ROS 2 RMW can report duplicates for nodes with multiple interfaces + std::set seen_fqns; + for (const auto & name_and_ns : names_and_namespaces) { const auto & name = name_and_ns.first; const auto & ns = name_and_ns.second; + std::string fqn = (ns == "/") ? std::string("/").append(name) : std::string(ns).append("/").append(name); + + // Skip duplicate nodes - ROS 2 RMW may report same node multiple times + if (seen_fqns.count(fqn) > 0) { + continue; + } + seen_fqns.insert(fqn); + Component comp; comp.id = name; comp.namespace_path = ns; - comp.fqn = (ns == "/") ? std::string("/").append(name) : std::string(ns).append("/").append(name); + comp.fqn = fqn; comp.area = extract_area_from_namespace(ns); // Use ROS 2 introspection API to get services for this specific node @@ -175,9 +204,38 @@ std::vector RuntimeDiscoveryStrategy::discover_components() { } std::vector RuntimeDiscoveryStrategy::discover_apps() { - // Apps are not supported in runtime-only mode - // They require manifest definitions - return {}; + // Only expose nodes as apps if configured + if (!config_.expose_nodes_as_apps) { + return {}; // Old behavior - apps require manifest + } + + std::vector apps; + auto node_components = discover_node_components(); + + for (const auto & comp : node_components) { + // Skip topic-based components (source="topic") + if (comp.source == "topic") { + continue; + } + + App app; + app.id = comp.id; + app.name = comp.id; + app.component_id = derive_component_id(comp); + app.source = "heuristic"; + app.is_online = true; + app.bound_fqn = comp.fqn; + + // Copy resources from component + app.topics = comp.topics; + app.services = comp.services; + app.actions = comp.actions; + + apps.push_back(app); + } + + RCLCPP_DEBUG(node_->get_logger(), "Discovered %zu apps from runtime nodes", apps.size()); + return apps; } std::vector RuntimeDiscoveryStrategy::discover_functions() { @@ -487,5 +545,67 @@ bool RuntimeDiscoveryStrategy::path_belongs_to_namespace(const std::string & pat return remainder.find('/') == std::string::npos; } +std::vector RuntimeDiscoveryStrategy::discover_synthetic_components() { + // Group nodes by their derived component ID (based on grouping strategy) + std::map> groups; + auto node_components = discover_node_components(); + + for (const auto & node : node_components) { + std::string group_id = derive_component_id(node); + groups[group_id].push_back(node); + } + + // Create synthetic components from groups + std::vector result; + for (const auto & [group_id, nodes] : groups) { + Component comp; + comp.id = group_id; + comp.source = "synthetic"; + comp.type = "ComponentGroup"; + + // Use first node's namespace and area as representative + if (!nodes.empty()) { + comp.namespace_path = nodes[0].namespace_path; + comp.area = nodes[0].area; + comp.fqn = "/" + group_id; + } + + // Note: Topics/services are NOT aggregated here - they stay with Apps + // This is intentional: synthetic components are just groupings + + RCLCPP_DEBUG(node_->get_logger(), "Created synthetic component '%s' with %zu apps", group_id.c_str(), nodes.size()); + result.push_back(comp); + } + + RCLCPP_DEBUG(node_->get_logger(), "Discovered %zu synthetic components from %zu nodes", result.size(), + node_components.size()); + return result; +} + +std::string RuntimeDiscoveryStrategy::derive_component_id(const Component & node) { + switch (config_.grouping) { + case ComponentGroupingStrategy::NAMESPACE: + // Group by area (first namespace segment) + return apply_component_name_pattern(node.area); + case ComponentGroupingStrategy::NONE: + default: + // 1:1 mapping - each node is its own component + return node.id; + } +} + +std::string RuntimeDiscoveryStrategy::apply_component_name_pattern(const std::string & area) { + std::string result = config_.synthetic_component_name_pattern; + + // Replace {area} placeholder + const std::string placeholder = "{area}"; + size_t pos = result.find(placeholder); + if (pos != std::string::npos) { + result.replace(pos, placeholder.length(), area); + } + + return result; +} + } // namespace discovery } // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 212aef0..02f0fd1 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -61,6 +61,13 @@ GatewayNode::GatewayNode() : Node("ros2_medkit_gateway") { declare_parameter("manifest_path", ""); declare_parameter("manifest_strict_validation", true); + // Runtime (heuristic) discovery options + // These control how nodes are mapped to SOVD entities in runtime mode + declare_parameter("discovery.runtime.expose_nodes_as_apps", true); + declare_parameter("discovery.runtime.create_synthetic_components", true); + declare_parameter("discovery.runtime.grouping_strategy", "namespace"); + declare_parameter("discovery.runtime.synthetic_component_name_pattern", "{area}"); + // Get parameter values server_host_ = get_parameter("server.host").as_string(); server_port_ = static_cast(get_parameter("server.port").as_int()); @@ -214,6 +221,15 @@ GatewayNode::GatewayNode() : Node("ros2_medkit_gateway") { discovery_config.manifest_path = get_parameter("manifest_path").as_string(); discovery_config.manifest_strict_validation = get_parameter("manifest_strict_validation").as_bool(); + // Runtime discovery options + discovery_config.runtime.expose_nodes_as_apps = get_parameter("discovery.runtime.expose_nodes_as_apps").as_bool(); + discovery_config.runtime.create_synthetic_components = + get_parameter("discovery.runtime.create_synthetic_components").as_bool(); + discovery_config.runtime.grouping = + parse_grouping_strategy(get_parameter("discovery.runtime.grouping_strategy").as_string()); + discovery_config.runtime.synthetic_component_name_pattern = + get_parameter("discovery.runtime.synthetic_component_name_pattern").as_string(); + if (!discovery_mgr_->initialize(discovery_config)) { RCLCPP_ERROR(get_logger(), "Failed to initialize discovery manager"); throw std::runtime_error("Discovery initialization failed"); @@ -300,6 +316,9 @@ void GatewayNode::refresh_cache() { // publish topics without creating proper ROS 2 nodes) auto topic_components = discovery_mgr_->discover_topic_components(); + // Discover apps (nodes as Apps when heuristic discovery is enabled) + auto apps = discovery_mgr_->discover_apps(); + // Merge both component lists std::vector all_components; all_components.reserve(node_components.size() + topic_components.size()); @@ -312,17 +331,20 @@ void GatewayNode::refresh_cache() { const size_t area_count = areas.size(); const size_t node_component_count = node_components.size(); const size_t topic_component_count = topic_components.size(); + const size_t app_count = apps.size(); // Lock only for the actual cache update { std::lock_guard lock(cache_mutex_); entity_cache_.areas = std::move(areas); entity_cache_.components = std::move(all_components); + entity_cache_.apps = std::move(apps); entity_cache_.last_update = timestamp; } - RCLCPP_DEBUG(get_logger(), "Cache refreshed: %zu areas, %zu components (%zu node-based, %zu topic-based)", - area_count, node_component_count + topic_component_count, node_component_count, topic_component_count); + RCLCPP_DEBUG(get_logger(), "Cache refreshed: %zu areas, %zu components (%zu node-based, %zu topic-based), %zu apps", + area_count, node_component_count + topic_component_count, node_component_count, topic_component_count, + app_count); } catch (const std::exception & e) { RCLCPP_ERROR(get_logger(), "Failed to refresh cache: %s", e.what()); } catch (...) { diff --git a/src/ros2_medkit_gateway/src/http/handlers/config_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/config_handlers.cpp index 502edd9..6259355 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/config_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/config_handlers.cpp @@ -23,47 +23,40 @@ namespace ros2_medkit_gateway { namespace handlers { void ConfigHandlers::handle_list_configurations(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; try { if (req.matches.size() < 2) { HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid request"); return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } - const auto cache = ctx_.node()->get_entity_cache(); - - // Find component to get its namespace and node name - std::string node_name; - bool component_found = false; - - for (const auto & component : cache.components) { - if (component.id == component_id) { - node_name = component.fqn; // Use fqn to avoid double slash for root namespace - component_found = true; - break; - } + // Use unified entity lookup + auto entity_info = ctx_.get_entity_info(entity_id); + if (entity_info.type == EntityType::UNKNOWN) { + HandlerContext::send_error(res, StatusCode::NotFound_404, "Entity not found", {{"entity_id", entity_id}}); + return; } - if (!component_found) { - HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", - {{"component_id", component_id}}); - return; + // Get node name for parameter access + std::string node_name = entity_info.fqn; + if (node_name.empty()) { + node_name = "/" + entity_id; } auto config_mgr = ctx_.node()->get_configuration_manager(); auto result = config_mgr->list_parameters(node_name); if (result.success) { - json response = {{"component_id", component_id}, {"node_name", node_name}, {"parameters", result.data}}; + json response = {{entity_info.id_field, entity_id}, {"node_name", node_name}, {"parameters", result.data}}; HandlerContext::send_json(res, response); } else { HandlerContext::send_error(res, StatusCode::ServiceUnavailable_503, "Failed to list parameters", @@ -71,14 +64,14 @@ void ConfigHandlers::handle_list_configurations(const httplib::Request & req, ht } } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to list configurations", - {{"details", e.what()}, {"component_id", component_id}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_list_configurations for component '%s': %s", - component_id.c_str(), e.what()); + {{"details", e.what()}, {"entity_id", entity_id}}); + RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_list_configurations for entity '%s': %s", entity_id.c_str(), + e.what()); } } void ConfigHandlers::handle_get_configuration(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; std::string param_name; try { if (req.matches.size() < 3) { @@ -86,13 +79,13 @@ void ConfigHandlers::handle_get_configuration(const httplib::Request & req, http return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; param_name = req.matches[2]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } @@ -106,19 +99,33 @@ void ConfigHandlers::handle_get_configuration(const httplib::Request & req, http const auto cache = ctx_.node()->get_entity_cache(); std::string node_name; - bool component_found = false; + bool entity_found = false; + std::string id_field = "component_id"; + // Try components first for (const auto & component : cache.components) { - if (component.id == component_id) { - node_name = component.fqn; // Use fqn to avoid double slash for root namespace - component_found = true; + if (component.id == entity_id) { + node_name = component.fqn; + entity_found = true; break; } } - if (!component_found) { - HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", - {{"component_id", component_id}}); + // If not found in components, try apps + if (!entity_found) { + for (const auto & app : cache.apps) { + if (app.id == entity_id) { + node_name = app.bound_fqn.value_or("/" + app.id); + entity_found = true; + id_field = "app_id"; + break; + } + } + } + + if (!entity_found) { + std::string error_msg = (id_field == "app_id") ? "App not found" : "Component not found"; + HandlerContext::send_error(res, StatusCode::NotFound_404, error_msg, {{id_field, entity_id}}); return; } @@ -126,7 +133,7 @@ void ConfigHandlers::handle_get_configuration(const httplib::Request & req, http auto result = config_mgr->get_parameter(node_name, param_name); if (result.success) { - json response = {{"component_id", component_id}, {"parameter", result.data}}; + json response = {{id_field, entity_id}, {"parameter", result.data}}; HandlerContext::send_json(res, response); } else { // Check if it's a "not found" error @@ -134,23 +141,23 @@ void ConfigHandlers::handle_get_configuration(const httplib::Request & req, http result.error_message.find("Parameter not found") != std::string::npos) { HandlerContext::send_error( res, StatusCode::NotFound_404, "Failed to get parameter", - {{"details", result.error_message}, {"component_id", component_id}, {"param_name", param_name}}); + {{"details", result.error_message}, {id_field, entity_id}, {"param_name", param_name}}); } else { HandlerContext::send_error( res, StatusCode::ServiceUnavailable_503, "Failed to get parameter", - {{"details", result.error_message}, {"component_id", component_id}, {"param_name", param_name}}); + {{"details", result.error_message}, {id_field, entity_id}, {"param_name", param_name}}); } } } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to get configuration", - {{"details", e.what()}, {"component_id", component_id}, {"param_name", param_name}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_get_configuration for component '%s', param '%s': %s", - component_id.c_str(), param_name.c_str(), e.what()); + {{"details", e.what()}, {"entity_id", entity_id}, {"param_name", param_name}}); + RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_get_configuration for entity '%s', param '%s': %s", + entity_id.c_str(), param_name.c_str(), e.what()); } } void ConfigHandlers::handle_set_configuration(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; std::string param_name; try { if (req.matches.size() < 3) { @@ -158,13 +165,13 @@ void ConfigHandlers::handle_set_configuration(const httplib::Request & req, http return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; param_name = req.matches[2]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } @@ -196,19 +203,33 @@ void ConfigHandlers::handle_set_configuration(const httplib::Request & req, http const auto cache = ctx_.node()->get_entity_cache(); std::string node_name; - bool component_found = false; + bool entity_found = false; + std::string id_field = "component_id"; + // Try components first for (const auto & component : cache.components) { - if (component.id == component_id) { - node_name = component.fqn; // Use fqn to avoid double slash for root namespace - component_found = true; + if (component.id == entity_id) { + node_name = component.fqn; + entity_found = true; break; } } - if (!component_found) { - HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", - {{"component_id", component_id}}); + // If not found in components, try apps + if (!entity_found) { + for (const auto & app : cache.apps) { + if (app.id == entity_id) { + node_name = app.bound_fqn.value_or("/" + app.id); + entity_found = true; + id_field = "app_id"; + break; + } + } + } + + if (!entity_found) { + std::string error_msg = (id_field == "app_id") ? "App not found" : "Component not found"; + HandlerContext::send_error(res, StatusCode::NotFound_404, error_msg, {{id_field, entity_id}}); return; } @@ -216,7 +237,7 @@ void ConfigHandlers::handle_set_configuration(const httplib::Request & req, http auto result = config_mgr->set_parameter(node_name, param_name, value); if (result.success) { - json response = {{"status", "success"}, {"component_id", component_id}, {"parameter", result.data}}; + json response = {{"status", "success"}, {id_field, entity_id}, {"parameter", result.data}}; HandlerContext::send_json(res, response); } else { // Check if it's a read-only, not found, or service unavailable error @@ -236,18 +257,18 @@ void ConfigHandlers::handle_set_configuration(const httplib::Request & req, http } HandlerContext::send_error( res, status_code, "Failed to set parameter", - {{"details", result.error_message}, {"component_id", component_id}, {"param_name", param_name}}); + {{"details", result.error_message}, {id_field, entity_id}, {"param_name", param_name}}); } } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to set configuration", - {{"details", e.what()}, {"component_id", component_id}, {"param_name", param_name}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_set_configuration for component '%s', param '%s': %s", - component_id.c_str(), param_name.c_str(), e.what()); + {{"details", e.what()}, {"entity_id", entity_id}, {"param_name", param_name}}); + RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_set_configuration for entity '%s', param '%s': %s", + entity_id.c_str(), param_name.c_str(), e.what()); } } void ConfigHandlers::handle_delete_configuration(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; std::string param_name; try { @@ -256,33 +277,47 @@ void ConfigHandlers::handle_delete_configuration(const httplib::Request & req, h return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; param_name = req.matches[2]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } const auto cache = ctx_.node()->get_entity_cache(); - // Find component to get its namespace and node name + // Find entity to get its namespace and node name std::string node_name; - bool component_found = false; + bool entity_found = false; + std::string id_field = "component_id"; + // Try components first for (const auto & component : cache.components) { - if (component.id == component_id) { + if (component.id == entity_id) { node_name = component.fqn; - component_found = true; + entity_found = true; break; } } - if (!component_found) { - HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", - {{"component_id", component_id}}); + // If not found in components, try apps + if (!entity_found) { + for (const auto & app : cache.apps) { + if (app.id == entity_id) { + node_name = app.bound_fqn.value_or("/" + app.id); + entity_found = true; + id_field = "app_id"; + break; + } + } + } + + if (!entity_found) { + std::string error_msg = (id_field == "app_id") ? "App not found" : "Component not found"; + HandlerContext::send_error(res, StatusCode::NotFound_404, error_msg, {{id_field, entity_id}}); return; } @@ -298,13 +333,13 @@ void ConfigHandlers::handle_delete_configuration(const httplib::Request & req, h } } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to reset configuration", - {{"details", e.what()}, {"component_id", component_id}, {"param_name", param_name}}); + {{"details", e.what()}, {"entity_id", entity_id}, {"param_name", param_name}}); RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_delete_configuration: %s", e.what()); } } void ConfigHandlers::handle_delete_all_configurations(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; try { if (req.matches.size() < 2) { @@ -312,32 +347,46 @@ void ConfigHandlers::handle_delete_all_configurations(const httplib::Request & r return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } const auto cache = ctx_.node()->get_entity_cache(); - // Find component to get its namespace and node name + // Find entity to get its namespace and node name std::string node_name; - bool component_found = false; + bool entity_found = false; + std::string id_field = "component_id"; + // Try components first for (const auto & component : cache.components) { - if (component.id == component_id) { + if (component.id == entity_id) { node_name = component.fqn; - component_found = true; + entity_found = true; break; } } - if (!component_found) { - HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", - {{"component_id", component_id}}); + // If not found in components, try apps + if (!entity_found) { + for (const auto & app : cache.apps) { + if (app.id == entity_id) { + node_name = app.bound_fqn.value_or("/" + app.id); + entity_found = true; + id_field = "app_id"; + break; + } + } + } + + if (!entity_found) { + std::string error_msg = (id_field == "app_id") ? "App not found" : "Component not found"; + HandlerContext::send_error(res, StatusCode::NotFound_404, error_msg, {{id_field, entity_id}}); return; } @@ -353,7 +402,7 @@ void ConfigHandlers::handle_delete_all_configurations(const httplib::Request & r } } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to reset configurations", - {{"details", e.what()}, {"component_id", component_id}}); + {{"details", e.what()}, {"entity_id", entity_id}}); RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_delete_all_configurations: %s", e.what()); } } diff --git a/src/ros2_medkit_gateway/src/http/handlers/discovery/component_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/discovery/component_handlers.cpp index 44bcd02..0e6870b 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/discovery/component_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/discovery/component_handlers.cpp @@ -129,35 +129,41 @@ void ComponentHandlers::handle_component_data(const httplib::Request & req, http return; } - const auto cache = ctx_.node()->get_entity_cache(); - - // Find component in cache and get its topics from the topic map - ComponentTopics component_topics; - bool component_found = false; - - for (const auto & component : cache.components) { - if (component.id == component_id) { - component_topics = component.topics; - component_found = true; - break; - } - } + auto discovery = ctx_.node()->get_discovery_manager(); + auto comp_opt = discovery->get_component(component_id); - if (!component_found) { + if (!comp_opt) { HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", {{"component_id", component_id}}); return; } - // Combine publishes and subscribes into unique set of topics + const auto & component = *comp_opt; + + // Collect all topics - from component directly OR aggregated from related Apps std::set all_topics; - for (const auto & topic : component_topics.publishes) { + + // First, check component's own topics + for (const auto & topic : component.topics.publishes) { all_topics.insert(topic); } - for (const auto & topic : component_topics.subscribes) { + for (const auto & topic : component.topics.subscribes) { all_topics.insert(topic); } + // If component has no direct topics (synthetic component), aggregate from Apps + if (all_topics.empty()) { + auto apps = discovery->get_apps_for_component(component_id); + for (const auto & app : apps) { + for (const auto & topic : app.topics.publishes) { + all_topics.insert(topic); + } + for (const auto & topic : app.topics.subscribes) { + all_topics.insert(topic); + } + } + } + // Sample all topics for this component auto data_access_mgr = ctx_.node()->get_data_access_manager(); json component_data = json::array(); @@ -251,20 +257,10 @@ void ComponentHandlers::handle_component_topic_data(const httplib::Request & req // Skip topic_name validation - it may contain slashes after URL decoding // The actual validation happens when we try to find the topic in the ROS graph - const auto cache = ctx_.node()->get_entity_cache(); - - // Find component in cache - only needed to verify it exists - // We use the full topic path from the URL, not the component namespace - bool component_found = false; - - for (const auto & component : cache.components) { - if (component.id == component_id) { - component_found = true; - break; - } - } + auto discovery = ctx_.node()->get_discovery_manager(); + auto comp_opt = discovery->get_component(component_id); - if (!component_found) { + if (!comp_opt) { HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", {{"component_id", component_id}}); return; @@ -362,19 +358,10 @@ void ComponentHandlers::handle_component_topic_publish(const httplib::Request & return; } - const auto cache = ctx_.node()->get_entity_cache(); - - // Find component in cache - only needed to verify it exists - bool component_found = false; - - for (const auto & component : cache.components) { - if (component.id == component_id) { - component_found = true; - break; - } - } + auto discovery = ctx_.node()->get_discovery_manager(); + auto comp_opt = discovery->get_component(component_id); - if (!component_found) { + if (!comp_opt) { HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", {{"component_id", component_id}}); return; diff --git a/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp index cce2b27..9597da3 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp @@ -54,29 +54,28 @@ void FaultHandlers::handle_list_all_faults(const httplib::Request & req, httplib } void FaultHandlers::handle_list_faults(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; try { if (req.matches.size() < 2) { HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid request"); return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } - auto namespace_result = ctx_.get_component_namespace_path(component_id); - if (!namespace_result) { - HandlerContext::send_error(res, StatusCode::NotFound_404, namespace_result.error(), - {{"component_id", component_id}}); + auto entity_info = ctx_.get_entity_info(entity_id); + if (entity_info.type == EntityType::UNKNOWN) { + HandlerContext::send_error(res, StatusCode::NotFound_404, "Entity not found", {{"entity_id", entity_id}}); return; } - std::string namespace_path = namespace_result.value(); + std::string namespace_path = entity_info.namespace_path; auto filter = parse_fault_status_param(req); if (!filter.is_valid) { @@ -84,7 +83,7 @@ void FaultHandlers::handle_list_faults(const httplib::Request & req, httplib::Re {{"details", "Valid values: pending, confirmed, cleared, all"}, {"parameter", "status"}, {"value", req.get_param_value("status")}, - {"component_id", component_id}}); + {entity_info.id_field, entity_id}}); return; } @@ -93,25 +92,25 @@ void FaultHandlers::handle_list_faults(const httplib::Request & req, httplib::Re fault_mgr->get_faults(namespace_path, filter.include_pending, filter.include_confirmed, filter.include_cleared); if (result.success) { - json response = {{"component_id", component_id}, + json response = {{entity_info.id_field, entity_id}, {"source_id", namespace_path}, {"faults", result.data["faults"]}, {"count", result.data["count"]}}; HandlerContext::send_json(res, response); } else { HandlerContext::send_error(res, StatusCode::ServiceUnavailable_503, "Failed to get faults", - {{"details", result.error_message}, {"component_id", component_id}}); + {{"details", result.error_message}, {entity_info.id_field, entity_id}}); } } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to list faults", - {{"details", e.what()}, {"component_id", component_id}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_list_faults for component '%s': %s", component_id.c_str(), + {{"details", e.what()}, {"entity_id", entity_id}}); + RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_list_faults for entity '%s': %s", entity_id.c_str(), e.what()); } } void FaultHandlers::handle_get_fault(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; std::string fault_code; try { if (req.matches.size() < 3) { @@ -119,13 +118,13 @@ void FaultHandlers::handle_get_fault(const httplib::Request & req, httplib::Resp return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; fault_code = req.matches[2]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } @@ -136,19 +135,18 @@ void FaultHandlers::handle_get_fault(const httplib::Request & req, httplib::Resp return; } - auto namespace_result = ctx_.get_component_namespace_path(component_id); - if (!namespace_result) { - HandlerContext::send_error(res, StatusCode::NotFound_404, namespace_result.error(), - {{"component_id", component_id}}); + auto entity_info = ctx_.get_entity_info(entity_id); + if (entity_info.type == EntityType::UNKNOWN) { + HandlerContext::send_error(res, StatusCode::NotFound_404, "Entity not found", {{"entity_id", entity_id}}); return; } - std::string namespace_path = namespace_result.value(); + std::string namespace_path = entity_info.namespace_path; auto fault_mgr = ctx_.node()->get_fault_manager(); auto result = fault_mgr->get_fault(fault_code, namespace_path); if (result.success) { - json response = {{"component_id", component_id}, {"fault", result.data}}; + json response = {{entity_info.id_field, entity_id}, {"fault", result.data}}; HandlerContext::send_json(res, response); } else { // Check if it's a "not found" error @@ -156,23 +154,23 @@ void FaultHandlers::handle_get_fault(const httplib::Request & req, httplib::Resp result.error_message.find("Fault not found") != std::string::npos) { HandlerContext::send_error( res, StatusCode::NotFound_404, "Failed to get fault", - {{"details", result.error_message}, {"component_id", component_id}, {"fault_code", fault_code}}); + {{"details", result.error_message}, {entity_info.id_field, entity_id}, {"fault_code", fault_code}}); } else { HandlerContext::send_error( res, StatusCode::ServiceUnavailable_503, "Failed to get fault", - {{"details", result.error_message}, {"component_id", component_id}, {"fault_code", fault_code}}); + {{"details", result.error_message}, {entity_info.id_field, entity_id}, {"fault_code", fault_code}}); } } } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to get fault", - {{"details", e.what()}, {"component_id", component_id}, {"fault_code", fault_code}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_get_fault for component '%s', fault '%s': %s", - component_id.c_str(), fault_code.c_str(), e.what()); + {{"details", e.what()}, {"entity_id", entity_id}, {"fault_code", fault_code}}); + RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_get_fault for entity '%s', fault '%s': %s", + entity_id.c_str(), fault_code.c_str(), e.what()); } } void FaultHandlers::handle_clear_fault(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; std::string fault_code; try { if (req.matches.size() < 3) { @@ -180,13 +178,13 @@ void FaultHandlers::handle_clear_fault(const httplib::Request & req, httplib::Re return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; fault_code = req.matches[2]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } @@ -197,11 +195,10 @@ void FaultHandlers::handle_clear_fault(const httplib::Request & req, httplib::Re return; } - // Verify component exists (we don't need namespace_path for clearing) - auto namespace_result = ctx_.get_component_namespace_path(component_id); - if (!namespace_result) { - HandlerContext::send_error(res, StatusCode::NotFound_404, namespace_result.error(), - {{"component_id", component_id}}); + // Verify entity exists + auto entity_info = ctx_.get_entity_info(entity_id); + if (entity_info.type == EntityType::UNKNOWN) { + HandlerContext::send_error(res, StatusCode::NotFound_404, "Entity not found", {{"entity_id", entity_id}}); return; } @@ -210,7 +207,7 @@ void FaultHandlers::handle_clear_fault(const httplib::Request & req, httplib::Re if (result.success) { json response = {{"status", "success"}, - {"component_id", component_id}, + {entity_info.id_field, entity_id}, {"fault_code", fault_code}, {"message", result.data.value("message", "Fault cleared")}}; HandlerContext::send_json(res, response); @@ -220,18 +217,18 @@ void FaultHandlers::handle_clear_fault(const httplib::Request & req, httplib::Re result.error_message.find("Fault not found") != std::string::npos) { HandlerContext::send_error( res, StatusCode::NotFound_404, "Failed to clear fault", - {{"details", result.error_message}, {"component_id", component_id}, {"fault_code", fault_code}}); + {{"details", result.error_message}, {entity_info.id_field, entity_id}, {"fault_code", fault_code}}); } else { HandlerContext::send_error( res, StatusCode::ServiceUnavailable_503, "Failed to clear fault", - {{"details", result.error_message}, {"component_id", component_id}, {"fault_code", fault_code}}); + {{"details", result.error_message}, {entity_info.id_field, entity_id}, {"fault_code", fault_code}}); } } } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to clear fault", - {{"details", e.what()}, {"component_id", component_id}, {"fault_code", fault_code}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_clear_fault for component '%s', fault '%s': %s", - component_id.c_str(), fault_code.c_str(), e.what()); + {{"details", e.what()}, {"entity_id", entity_id}, {"fault_code", fault_code}}); + RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_clear_fault for entity '%s', fault '%s': %s", + entity_id.c_str(), fault_code.c_str(), e.what()); } } @@ -280,7 +277,7 @@ void FaultHandlers::handle_get_snapshots(const httplib::Request & req, httplib:: } void FaultHandlers::handle_get_component_snapshots(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; std::string fault_code; try { if (req.matches.size() < 3) { @@ -288,13 +285,13 @@ void FaultHandlers::handle_get_component_snapshots(const httplib::Request & req, return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; fault_code = req.matches[2]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } @@ -305,10 +302,9 @@ void FaultHandlers::handle_get_component_snapshots(const httplib::Request & req, return; } - auto namespace_result = ctx_.get_component_namespace_path(component_id); - if (!namespace_result) { - HandlerContext::send_error(res, StatusCode::NotFound_404, namespace_result.error(), - {{"component_id", component_id}}); + auto entity_info = ctx_.get_entity_info(entity_id); + if (entity_info.type == EntityType::UNKNOWN) { + HandlerContext::send_error(res, StatusCode::NotFound_404, "Entity not found", {{"entity_id", entity_id}}); return; } @@ -319,9 +315,9 @@ void FaultHandlers::handle_get_component_snapshots(const httplib::Request & req, auto result = fault_mgr->get_snapshots(fault_code, topic_filter); if (result.success) { - // Add component context to response + // Add entity context to response json response = result.data; - response["component_id"] = component_id; + response[entity_info.id_field] = entity_id; HandlerContext::send_json(res, response); } else { // Check if it's a "not found" error @@ -329,18 +325,18 @@ void FaultHandlers::handle_get_component_snapshots(const httplib::Request & req, result.error_message.find("Fault not found") != std::string::npos) { HandlerContext::send_error( res, StatusCode::NotFound_404, "Failed to get snapshots", - {{"details", result.error_message}, {"component_id", component_id}, {"fault_code", fault_code}}); + {{"details", result.error_message}, {entity_info.id_field, entity_id}, {"fault_code", fault_code}}); } else { HandlerContext::send_error( res, StatusCode::ServiceUnavailable_503, "Failed to get snapshots", - {{"details", result.error_message}, {"component_id", component_id}, {"fault_code", fault_code}}); + {{"details", result.error_message}, {entity_info.id_field, entity_id}, {"fault_code", fault_code}}); } } } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to get snapshots", - {{"details", e.what()}, {"component_id", component_id}, {"fault_code", fault_code}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_get_component_snapshots for component '%s', fault '%s': %s", - component_id.c_str(), fault_code.c_str(), e.what()); + {{"details", e.what()}, {"entity_id", entity_id}, {"fault_code", fault_code}}); + RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_get_component_snapshots for entity '%s', fault '%s': %s", + entity_id.c_str(), fault_code.c_str(), e.what()); } } diff --git a/src/ros2_medkit_gateway/src/http/handlers/handler_context.cpp b/src/ros2_medkit_gateway/src/http/handlers/handler_context.cpp index db7aa75..85e3bec 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/handler_context.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/handler_context.cpp @@ -71,6 +71,66 @@ HandlerContext::get_component_namespace_path(const std::string & component_id) c return tl::unexpected("Component not found"); } +EntityInfo HandlerContext::get_entity_info(const std::string & entity_id) const { + const auto cache = node_->get_entity_cache(); + EntityInfo info; + info.id = entity_id; + + // Search components first + for (const auto & component : cache.components) { + if (component.id == entity_id) { + info.type = EntityType::COMPONENT; + info.namespace_path = component.namespace_path; + info.fqn = component.fqn; + info.id_field = "component_id"; + info.error_name = "Component"; + return info; + } + } + + // Search apps + for (const auto & app : cache.apps) { + if (app.id == entity_id) { + info.type = EntityType::APP; + // Apps use bound_fqn as namespace_path for fault filtering + info.namespace_path = app.bound_fqn.value_or(""); + info.fqn = app.bound_fqn.value_or(""); + info.id_field = "app_id"; + info.error_name = "App"; + return info; + } + } + + // Search areas + for (const auto & area : cache.areas) { + if (area.id == entity_id) { + info.type = EntityType::AREA; + info.namespace_path = ""; // Areas don't have namespace_path + info.fqn = ""; + info.id_field = "area_id"; + info.error_name = "Area"; + return info; + } + } + + // Search functions + for (const auto & func : cache.functions) { + if (func.id == entity_id) { + info.type = EntityType::FUNCTION; + info.namespace_path = ""; + info.fqn = ""; + info.id_field = "function_id"; + info.error_name = "Function"; + return info; + } + } + + // Not found - return UNKNOWN type + info.type = EntityType::UNKNOWN; + info.error_name = "Entity"; + return info; +} + void HandlerContext::set_cors_headers(httplib::Response & res, const std::string & origin) const { res.set_header("Access-Control-Allow-Origin", origin); diff --git a/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp index dcc5cdc..872d137 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp @@ -24,41 +24,66 @@ namespace ros2_medkit_gateway { namespace handlers { void OperationHandlers::handle_list_operations(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; try { if (req.matches.size() < 2) { HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid request"); return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } const auto cache = ctx_.node()->get_entity_cache(); - // Find component in cache - bool component_found = false; + // Find entity in cache - check components first, then apps + bool entity_found = false; std::vector services; std::vector actions; + std::string entity_type = "component"; + // Try to find in components for (const auto & component : cache.components) { - if (component.id == component_id) { + if (component.id == entity_id) { services = component.services; actions = component.actions; - component_found = true; + + // For synthetic components with no direct operations, aggregate from apps + if (services.empty() && actions.empty()) { + auto discovery = ctx_.node()->get_discovery_manager(); + auto apps = discovery->get_apps_for_component(entity_id); + for (const auto & app : apps) { + services.insert(services.end(), app.services.begin(), app.services.end()); + actions.insert(actions.end(), app.actions.begin(), app.actions.end()); + } + } + + entity_found = true; break; } } - if (!component_found) { - HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", - {{"component_id", component_id}}); + // If not found in components, try apps + if (!entity_found) { + for (const auto & app : cache.apps) { + if (app.id == entity_id) { + services = app.services; + actions = app.actions; + entity_found = true; + entity_type = "app"; + break; + } + } + } + + if (!entity_found) { + HandlerContext::send_error(res, StatusCode::NotFound_404, "Entity not found", {{"entity_id", entity_id}}); return; } @@ -113,33 +138,37 @@ void OperationHandlers::handle_list_operations(const httplib::Request & req, htt operations.push_back(act_json); } - HandlerContext::send_json(res, operations); + // Return SOVD-compliant response with items array + json response; + response["items"] = operations; + response["total_count"] = operations.size(); + HandlerContext::send_json(res, response); } catch (const std::exception & e) { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to list operations", - {{"details", e.what()}, {"component_id", component_id}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_list_operations for component '%s': %s", - component_id.c_str(), e.what()); + {{"details", e.what()}, {"entity_id", entity_id}}); + RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_list_operations for entity '%s': %s", entity_id.c_str(), + e.what()); } } void OperationHandlers::handle_component_operation(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; std::string operation_name; try { - // Extract component_id and operation_name from URL path + // Extract entity_id and operation_name from URL path if (req.matches.size() < 3) { HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid request"); return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; operation_name = req.matches[2]; - // Validate component_id - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}, {"component_id", component_id}}); + // Validate entity_id + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}, {"entity_id", entity_id}}); return; } @@ -176,15 +205,18 @@ void OperationHandlers::handle_component_operation(const httplib::Request & req, } const auto cache = ctx_.node()->get_entity_cache(); + auto discovery = ctx_.node()->get_discovery_manager(); - // Find component in cache and look up service/action with full_path - const Component * found_component = nullptr; + // Find entity and operation - check components first, then apps + bool entity_found = false; + std::string entity_type = "component"; std::optional service_info; std::optional action_info; + // Try to find in components for (const auto & component : cache.components) { - if (component.id == component_id) { - found_component = &component; + if (component.id == entity_id) { + entity_found = true; // Search in component's services list (has full_path) for (const auto & svc : component.services) { @@ -195,25 +227,84 @@ void OperationHandlers::handle_component_operation(const httplib::Request & req, } // Search in component's actions list (has full_path) - for (const auto & act : component.actions) { - if (act.name == operation_name) { - action_info = act; - break; + if (!service_info.has_value()) { + for (const auto & act : component.actions) { + if (act.name == operation_name) { + action_info = act; + break; + } + } + } + + // For synthetic components, try to find operation in apps + if (!service_info.has_value() && !action_info.has_value()) { + auto apps = discovery->get_apps_for_component(entity_id); + for (const auto & app : apps) { + for (const auto & svc : app.services) { + if (svc.name == operation_name) { + service_info = svc; + break; + } + } + if (service_info.has_value()) { + break; + } + + for (const auto & act : app.actions) { + if (act.name == operation_name) { + action_info = act; + break; + } + } + if (action_info.has_value()) { + break; + } } } break; } } - if (!found_component) { - HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", - {{"component_id", component_id}}); + // If not found in components, try apps + if (!entity_found) { + for (const auto & app : cache.apps) { + if (app.id == entity_id) { + entity_found = true; + entity_type = "app"; + + // Search in app's services list + for (const auto & svc : app.services) { + if (svc.name == operation_name) { + service_info = svc; + break; + } + } + + // Search in app's actions list + if (!service_info.has_value()) { + for (const auto & act : app.actions) { + if (act.name == operation_name) { + action_info = act; + break; + } + } + } + break; + } + } + } + + if (!entity_found) { + HandlerContext::send_error(res, StatusCode::NotFound_404, "Entity not found", {{"entity_id", entity_id}}); return; } // Check if operation is a service or action auto operation_mgr = ctx_.node()->get_operation_manager(); + // Determine response field name based on entity type + std::string id_field = (entity_type == "app") ? "app_id" : "component_id"; + // First, check if it's an action (use full_path from cache) if (action_info.has_value()) { // Extract goal data (from 'goal' field or root object) @@ -240,7 +331,7 @@ void OperationHandlers::handle_component_operation(const httplib::Request & req, json response = {{"status", "success"}, {"kind", "action"}, - {"component_id", component_id}, + {id_field, entity_id}, {"operation", operation_name}, {"goal_id", action_result.goal_id}, {"goal_status", status_str}}; @@ -249,20 +340,20 @@ void OperationHandlers::handle_component_operation(const httplib::Request & req, HandlerContext::send_error( res, StatusCode::BadRequest_400, "rejected", {{"kind", "action"}, - {"component_id", component_id}, + {id_field, entity_id}, {"operation", operation_name}, {"error", action_result.error_message.empty() ? "Goal rejected" : action_result.error_message}}); } else { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "error", {{"kind", "action"}, - {"component_id", component_id}, + {id_field, entity_id}, {"operation", operation_name}, {"error", action_result.error_message}}); } return; } - // Otherwise, check if it's a service call (must exist in component.services) + // Otherwise, check if it's a service call if (service_info.has_value()) { // Use service type from cache or request body std::string resolved_service_type = service_info->type; @@ -276,49 +367,48 @@ void OperationHandlers::handle_component_operation(const httplib::Request & req, if (result.success) { json response = {{"status", "success"}, {"kind", "service"}, - {"component_id", component_id}, + {id_field, entity_id}, {"operation", operation_name}, {"response", result.response}}; HandlerContext::send_json(res, response); } else { HandlerContext::send_error(res, StatusCode::InternalServerError_500, "error", {{"kind", "service"}, - {"component_id", component_id}, + {id_field, entity_id}, {"operation", operation_name}, {"error", result.error_message}}); } } else { // Neither service nor action found HandlerContext::send_error(res, StatusCode::NotFound_404, "Operation not found", - {{"component_id", component_id}, {"operation_name", operation_name}}); + {{id_field, entity_id}, {"operation_name", operation_name}}); } } catch (const std::exception & e) { - HandlerContext::send_error( - res, StatusCode::InternalServerError_500, "Failed to execute operation", - {{"details", e.what()}, {"component_id", component_id}, {"operation_name", operation_name}}); - RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_component_operation for component '%s', operation '%s': %s", - component_id.c_str(), operation_name.c_str(), e.what()); + HandlerContext::send_error(res, StatusCode::InternalServerError_500, "Failed to execute operation", + {{"details", e.what()}, {"entity_id", entity_id}, {"operation_name", operation_name}}); + RCLCPP_ERROR(HandlerContext::logger(), "Error in handle_component_operation for entity '%s', operation '%s': %s", + entity_id.c_str(), operation_name.c_str(), e.what()); } } void OperationHandlers::handle_action_status(const httplib::Request & req, httplib::Response & res) { - std::string component_id; + std::string entity_id; std::string operation_name; try { - // Extract component_id and operation_name from URL path + // Extract entity_id and operation_name from URL path if (req.matches.size() < 3) { HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid request"); return; } - component_id = req.matches[1]; + entity_id = req.matches[1]; operation_name = req.matches[2]; // Validate IDs - auto component_validation = ctx_.validate_entity_id(component_id); - if (!component_validation) { - HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid component ID", - {{"details", component_validation.error()}}); + auto entity_validation = ctx_.validate_entity_id(entity_id); + if (!entity_validation) { + HandlerContext::send_error(res, StatusCode::BadRequest_400, "Invalid entity ID", + {{"details", entity_validation.error()}}); return; } @@ -361,24 +451,47 @@ void OperationHandlers::handle_action_status(const httplib::Request & req, httpl } // No goal_id provided - find goals by action path - // First, find the component to get its namespace - auto discovery_mgr = ctx_.node()->get_discovery_manager(); - auto components = discovery_mgr->discover_components(); - std::optional component; - for (const auto & c : components) { - if (c.id == component_id) { - component = c; + // Find entity (component or app) to get its namespace + const auto cache = ctx_.node()->get_entity_cache(); + + std::string namespace_path; + bool entity_found = false; + + // Try components first + for (const auto & c : cache.components) { + if (c.id == entity_id) { + namespace_path = c.namespace_path; + entity_found = true; break; } } - if (!component.has_value()) { - HandlerContext::send_error(res, StatusCode::NotFound_404, "Component not found", - {{"component_id", component_id}}); + + // Try apps if not found in components + if (!entity_found) { + for (const auto & app : cache.apps) { + if (app.id == entity_id) { + // For apps, find the action in the app's actions list + for (const auto & act : app.actions) { + if (act.name == operation_name) { + namespace_path = act.full_path.substr(0, act.full_path.rfind('/')); + entity_found = true; + break; + } + } + if (entity_found) { + break; + } + } + } + } + + if (!entity_found) { + HandlerContext::send_error(res, StatusCode::NotFound_404, "Entity not found", {{"entity_id", entity_id}}); return; } // Build the action path: namespace + operation_name - std::string action_path = component->namespace_path + "/" + operation_name; + std::string action_path = namespace_path + "/" + operation_name; if (get_all) { // Return all goals for this action diff --git a/src/ros2_medkit_gateway/src/http/rest_server.cpp b/src/ros2_medkit_gateway/src/http/rest_server.cpp index 9076aeb..18461d5 100644 --- a/src/ros2_medkit_gateway/src/http/rest_server.cpp +++ b/src/ros2_medkit_gateway/src/http/rest_server.cpp @@ -169,15 +169,63 @@ void RESTServer::setup_routes() { // App operations srv->Get((api_path("/apps") + R"(/([^/]+)/operations$)"), [this](const httplib::Request & req, httplib::Response & res) { - app_handlers_->handle_list_app_operations(req, res); + operation_handlers_->handle_list_operations(req, res); + }); + + // App operation (POST) - sync operations like service calls, async action goals + srv->Post((api_path("/apps") + R"(/([^/]+)/operations/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + operation_handlers_->handle_component_operation(req, res); + }); + + // App action status (GET) + srv->Get((api_path("/apps") + R"(/([^/]+)/operations/([^/]+)/status$)"), + [this](const httplib::Request & req, httplib::Response & res) { + operation_handlers_->handle_action_status(req, res); + }); + + // App action result (GET) + srv->Get((api_path("/apps") + R"(/([^/]+)/operations/([^/]+)/result$)"), + [this](const httplib::Request & req, httplib::Response & res) { + operation_handlers_->handle_action_result(req, res); }); - // App configurations + // App action cancel (DELETE) + srv->Delete((api_path("/apps") + R"(/([^/]+)/operations/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + operation_handlers_->handle_action_cancel(req, res); + }); + + // App configurations - list all srv->Get((api_path("/apps") + R"(/([^/]+)/configurations$)"), [this](const httplib::Request & req, httplib::Response & res) { - app_handlers_->handle_list_app_configurations(req, res); + config_handlers_->handle_list_configurations(req, res); + }); + + // App configurations - get specific + srv->Get((api_path("/apps") + R"(/([^/]+)/configurations/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + config_handlers_->handle_get_configuration(req, res); + }); + + // App configurations - set + srv->Put((api_path("/apps") + R"(/([^/]+)/configurations/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + config_handlers_->handle_set_configuration(req, res); }); + // App configurations - delete single + srv->Delete((api_path("/apps") + R"(/([^/]+)/configurations/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + config_handlers_->handle_delete_configuration(req, res); + }); + + // App configurations - delete all + srv->Delete((api_path("/apps") + R"(/([^/]+)/configurations$)"), + [this](const httplib::Request & req, httplib::Response & res) { + config_handlers_->handle_delete_all_configurations(req, res); + }); + // Single app (capabilities) - must be after more specific routes srv->Get((api_path("/apps") + R"(/([^/]+)$)"), [this](const httplib::Request & req, httplib::Response & res) { app_handlers_->handle_get_app(req, res); @@ -355,18 +403,35 @@ void RESTServer::setup_routes() { fault_handlers_->handle_list_faults(req, res); }); + // List all faults for an app (same handler, entity-agnostic) + srv->Get((api_path("/apps") + R"(/([^/]+)/faults$)"), [this](const httplib::Request & req, httplib::Response & res) { + fault_handlers_->handle_list_faults(req, res); + }); + // Get specific fault by code (REQ_INTEROP_013) srv->Get((api_path("/components") + R"(/([^/]+)/faults/([^/]+)$)"), [this](const httplib::Request & req, httplib::Response & res) { fault_handlers_->handle_get_fault(req, res); }); + // Get specific fault by code for an app + srv->Get((api_path("/apps") + R"(/([^/]+)/faults/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + fault_handlers_->handle_get_fault(req, res); + }); + // Clear a fault (REQ_INTEROP_015) srv->Delete((api_path("/components") + R"(/([^/]+)/faults/([^/]+)$)"), [this](const httplib::Request & req, httplib::Response & res) { fault_handlers_->handle_clear_fault(req, res); }); + // Clear a fault for an app + srv->Delete((api_path("/apps") + R"(/([^/]+)/faults/([^/]+)$)"), + [this](const httplib::Request & req, httplib::Response & res) { + fault_handlers_->handle_clear_fault(req, res); + }); + // Snapshot endpoints for fault debugging // GET /faults/{fault_code}/snapshots - system-wide snapshot access srv->Get((api_path("/faults") + R"(/([^/]+)/snapshots$)"), @@ -380,6 +445,12 @@ void RESTServer::setup_routes() { fault_handlers_->handle_get_component_snapshots(req, res); }); + // GET /apps/{app_id}/faults/{fault_code}/snapshots - app-scoped snapshot access + srv->Get((api_path("/apps") + R"(/([^/]+)/faults/([^/]+)/snapshots$)"), + [this](const httplib::Request & req, httplib::Response & res) { + fault_handlers_->handle_get_component_snapshots(req, res); + }); + // Authentication endpoints (REQ_INTEROP_086, REQ_INTEROP_087) // POST /auth/authorize - Authenticate and get tokens (client_credentials grant) srv->Post(api_path("/auth/authorize"), [this](const httplib::Request & req, httplib::Response & res) { diff --git a/src/ros2_medkit_gateway/test/test_discovery_manager.cpp b/src/ros2_medkit_gateway/test/test_discovery_manager.cpp index fec7da0..e86e79c 100644 --- a/src/ros2_medkit_gateway/test/test_discovery_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_discovery_manager.cpp @@ -82,12 +82,16 @@ TEST_F(DiscoveryManagerTest, DiscoverTopicComponents_SetsSourceField) { } } -TEST_F(DiscoveryManagerTest, DiscoverComponents_NodeBasedHaveSourceNode) { +TEST_F(DiscoveryManagerTest, DiscoverComponents_NodeBasedHaveSourceSynthetic) { + // With default config (create_synthetic_components=true), components are synthetic auto components = discovery_manager_->discover_components(); - // Node-based components should have source="node" (default) + // Synthetic components (grouped by namespace) have source="synthetic" + // If no runtime nodes, we may have the test node as well for (const auto & comp : components) { - EXPECT_EQ(comp.source, "node") << "Node-based component should have source='node'"; + // Components can be "synthetic" (namespace-grouped) or "node" (legacy) + EXPECT_TRUE(comp.source == "synthetic" || comp.source == "node") + << "Component should have source='synthetic' or 'node', got: " << comp.source; } } diff --git a/src/ros2_medkit_gateway/test/test_discovery_manifest.test.py b/src/ros2_medkit_gateway/test/test_discovery_manifest.test.py index 580b1a6..9d82d09 100644 --- a/src/ros2_medkit_gateway/test/test_discovery_manifest.test.py +++ b/src/ros2_medkit_gateway/test/test_discovery_manifest.test.py @@ -405,11 +405,18 @@ def test_app_operations_endpoint(self): self.assertIn(response.status_code, [200, 404]) def test_app_configurations_endpoint(self): - """Test GET /apps/{id}/configurations returns parameters.""" + """ + Test GET /apps/{id}/configurations returns parameters. + + May return: + - 200 if node is running and has parameters + - 404 if app not found + - 503 if node is not running (manifest-only mode) + """ response = requests.get( f'{self.BASE_URL}/apps/lidar-sensor/configurations', timeout=5 ) - self.assertIn(response.status_code, [200, 404]) + self.assertIn(response.status_code, [200, 404, 503]) def test_app_data_item_endpoint(self): """ diff --git a/src/ros2_medkit_gateway/test/test_integration.test.py b/src/ros2_medkit_gateway/test/test_integration.test.py index c8061fa..4240f16 100644 --- a/src/ros2_medkit_gateway/test/test_integration.test.py +++ b/src/ros2_medkit_gateway/test/test_integration.test.py @@ -107,12 +107,13 @@ def generate_test_description(): """Generate launch description with gateway node, demo nodes, and tests.""" # Launch the ROS 2 Medkit Gateway node # additional_env sets GCOV_PREFIX for coverage data collection from subprocess + # Use fast refresh interval (1s) for tests to ensure cache is updated quickly gateway_node = launch_ros.actions.Node( package='ros2_medkit_gateway', executable='gateway_node', name='ros2_medkit_gateway', output='screen', - parameters=[], + parameters=[{'refresh_interval_ms': 1000}], additional_env=get_coverage_env(), ) @@ -261,16 +262,21 @@ class TestROS2MedkitGatewayIntegration(unittest.TestCase): """Integration tests for ROS 2 Medkit Gateway REST API and discovery.""" BASE_URL = f'http://localhost:8080{API_BASE_PATH}' - # Minimum expected components for tests to pass - MIN_EXPECTED_COMPONENTS = 8 + # Minimum expected components (synthetic, grouped by top-level namespace) + # With default config: powertrain, chassis, body, perception, root + MIN_EXPECTED_COMPONENTS = 4 + # Minimum expected apps (ROS 2 nodes) + MIN_EXPECTED_APPS = 8 + # Minimum expected areas (powertrain, chassis, body + root) + MIN_EXPECTED_AREAS = 4 # Maximum time to wait for discovery (seconds) MAX_DISCOVERY_WAIT = 60.0 # Interval between discovery checks (seconds) - DISCOVERY_CHECK_INTERVAL = 2.0 + DISCOVERY_CHECK_INTERVAL = 1.0 @classmethod def setUpClass(cls): - """Wait for gateway to be ready and components to be discovered.""" + """Wait for gateway to be ready and apps/areas to be discovered.""" # First, wait for gateway to respond max_retries = 30 for i in range(max_retries): @@ -283,24 +289,29 @@ def setUpClass(cls): raise unittest.SkipTest('Gateway not responding after 30 retries') time.sleep(1) - # Wait for components to be discovered (CI can be slow) + # Wait for apps AND areas to be discovered (CI can be slow) start_time = time.time() while time.time() - start_time < cls.MAX_DISCOVERY_WAIT: try: - response = requests.get(f'{cls.BASE_URL}/components', timeout=5) - if response.status_code == 200: - components = response.json() - if len(components) >= cls.MIN_EXPECTED_COMPONENTS: - print(f'✓ Discovery complete: {len(components)} components') + apps_response = requests.get(f'{cls.BASE_URL}/apps', timeout=5) + areas_response = requests.get(f'{cls.BASE_URL}/areas', timeout=5) + if apps_response.status_code == 200 and areas_response.status_code == 200: + apps = apps_response.json().get('items', []) + areas = areas_response.json().get('items', []) + apps_ok = len(apps) >= cls.MIN_EXPECTED_APPS + areas_ok = len(areas) >= cls.MIN_EXPECTED_AREAS + if apps_ok and areas_ok: + print(f'✓ Discovery complete: {len(apps)} apps, {len(areas)} areas') return - expected = cls.MIN_EXPECTED_COMPONENTS - print(f' Waiting for discovery: {len(components)}/{expected}...') + area_ids = [a.get('id', '?') for a in areas] + print(f' Waiting: {len(apps)}/{cls.MIN_EXPECTED_APPS} apps, ' + f'{len(areas)}/{cls.MIN_EXPECTED_AREAS} areas {area_ids}') except requests.exceptions.RequestException: # Ignore connection errors during discovery wait; will retry until timeout pass time.sleep(cls.DISCOVERY_CHECK_INTERVAL) - # If we get here, not all components were discovered but continue anyway + # If we get here, not all entities were discovered but continue anyway print('Warning: Discovery timeout, some tests may fail') def _get_json(self, endpoint: str, timeout: int = 10): @@ -377,7 +388,10 @@ def test_02_list_areas(self): def test_03_list_components(self): """ - Test GET /components returns all discovered components. + Test GET /components returns all discovered synthetic components. + + With heuristic discovery (default), components are synthetic groups + created by namespace aggregation. ROS 2 nodes are exposed as Apps. @verifies REQ_INTEROP_003 """ @@ -385,25 +399,31 @@ def test_03_list_components(self): self.assertIn('items', data) components = data['items'] self.assertIsInstance(components, list) - # Should have at least 7 demo nodes + gateway node - self.assertGreaterEqual(len(components), 7) + # With synthetic components, we have fewer components (grouped by namespace) + # Expected: powertrain, chassis, body, perception, root (at minimum) + self.assertGreaterEqual(len(components), 4) # Verify response structure - all components should have required fields for component in components: self.assertIn('id', component) - self.assertIn('namespace', component) + # namespace may be 'namespace' or 'namespace_path' depending on source + self.assertTrue( + 'namespace' in component or 'namespace_path' in component, + f"Component {component['id']} should have namespace field" + ) self.assertIn('fqn', component) self.assertIn('type', component) self.assertIn('area', component) - self.assertEqual(component['type'], 'Component') + # Synthetic components have type 'ComponentGroup', topic-based have 'Component' + self.assertIn(component['type'], ['Component', 'ComponentGroup']) - # Verify some expected component IDs are present + # Verify expected synthetic component IDs are present component_ids = [comp['id'] for comp in components] - self.assertIn('temp_sensor', component_ids) - self.assertIn('rpm_sensor', component_ids) - self.assertIn('pressure_sensor', component_ids) + self.assertIn('powertrain', component_ids) + self.assertIn('chassis', component_ids) + self.assertIn('body', component_ids) - print(f'✓ Components test passed: {len(components)} components discovered') + print(f'✓ Components test passed: {len(components)} synthetic components discovered') def test_04_automotive_areas_discovery(self): """ @@ -425,6 +445,9 @@ def test_05_area_components_success(self): """ Test GET /areas/{area_id}/components returns components for valid area. + With synthetic components, the powertrain area contains the 'powertrain' + synthetic component which aggregates all ROS 2 nodes in that namespace. + @verifies REQ_INTEROP_006 """ # Test powertrain area @@ -440,10 +463,9 @@ def test_05_area_components_success(self): self.assertIn('id', component) self.assertIn('namespace', component) - # Verify expected powertrain components + # Verify the synthetic 'powertrain' component exists component_ids = [comp['id'] for comp in components] - self.assertIn('temp_sensor', component_ids) - self.assertIn('rpm_sensor', component_ids) + self.assertIn('powertrain', component_ids) print( f'✓ Area components test passed: {len(components)} components in powertrain' @@ -468,195 +490,168 @@ def test_06_area_components_nonexistent_error(self): print('✓ Nonexistent area error test passed') - def test_07_component_data_powertrain_engine(self): + def test_07_app_data_powertrain_engine(self): """ - Test GET /components/{component_id}/data for engine component. + Test GET /apps/{app_id}/data for engine temperature sensor app. + + Apps are ROS 2 nodes. The temp_sensor app publishes temperature data. @verifies REQ_INTEROP_018 """ - # Get data from temp_sensor component (powertrain/engine) - data = self._get_json('/components/temp_sensor/data') - self.assertIsInstance(data, list) - - # Should have at least one topic with data or metadata - if len(data) > 0: - for topic_data in data: - self.assertIn('topic', topic_data) - self.assertIn('status', topic_data) - # Status can be 'data' (with actual data) or 'metadata_only' (fallback) - self.assertIn(topic_data['status'], ['data', 'metadata_only']) - if topic_data['status'] == 'data': - self.assertIn('data', topic_data) + # Get data from temp_sensor app (powertrain/engine) + data = self._get_json('/apps/temp_sensor/data') + self.assertIn('items', data) + items = data['items'] + self.assertIsInstance(items, list) + + # Should have at least one topic + if len(items) > 0: + for topic_data in items: + self.assertIn('id', topic_data) + self.assertIn('name', topic_data) + self.assertIn('direction', topic_data) + self.assertIn(topic_data['direction'], ['publish', 'subscribe']) print( - f" - Topic: {topic_data['topic']} (status: {topic_data['status']})" + f" - Topic: {topic_data['name']} ({topic_data['direction']})" ) - print(f'✓ Engine component data test passed: {len(data)} topics') + print(f'✓ Engine app data test passed: {len(items)} topics') - def test_08_component_data_chassis_brakes(self): + def test_08_app_data_chassis_brakes(self): """ - Test GET /components/{component_id}/data for brakes component. + Test GET /apps/{app_id}/data for brakes pressure sensor app. @verifies REQ_INTEROP_018 """ - # Get data from pressure_sensor component (chassis/brakes) - data = self._get_json('/components/pressure_sensor/data') - self.assertIsInstance(data, list) + # Get data from pressure_sensor app (chassis/brakes) + data = self._get_json('/apps/pressure_sensor/data') + self.assertIn('items', data) + items = data['items'] + self.assertIsInstance(items, list) - # Check if any data/metadata is available - if len(data) > 0: - for topic_data in data: - self.assertIn('topic', topic_data) - self.assertIn('status', topic_data) - # Status can be 'data' (with actual data) or 'metadata_only' (fallback) - self.assertIn(topic_data['status'], ['data', 'metadata_only']) - if topic_data['status'] == 'data': - self.assertIn('data', topic_data) + # Check structure + if len(items) > 0: + for topic_data in items: + self.assertIn('id', topic_data) + self.assertIn('name', topic_data) + self.assertIn('direction', topic_data) - print(f'✓ Brakes component data test passed: {len(data)} topics') + print(f'✓ Brakes app data test passed: {len(items)} topics') - def test_09_component_data_body_door(self): + def test_09_app_data_body_door(self): """ - Test GET /components/{component_id}/data for door component. + Test GET /apps/{app_id}/data for door status sensor app. @verifies REQ_INTEROP_018 """ - # Get data from status_sensor component (body/door/front_left) - data = self._get_json('/components/status_sensor/data') - self.assertIsInstance(data, list) + # Get data from status_sensor app (body/door/front_left) + data = self._get_json('/apps/status_sensor/data') + self.assertIn('items', data) + items = data['items'] + self.assertIsInstance(items, list) # Check structure - if len(data) > 0: - for topic_data in data: - self.assertIn('topic', topic_data) - self.assertIn('status', topic_data) - # Status can be 'data' (with actual data) or 'metadata_only' (fallback) - self.assertIn(topic_data['status'], ['data', 'metadata_only']) - if topic_data['status'] == 'data': - self.assertIn('data', topic_data) + if len(items) > 0: + for topic_data in items: + self.assertIn('id', topic_data) + self.assertIn('name', topic_data) + self.assertIn('direction', topic_data) - print(f'✓ Door component data test passed: {len(data)} topics') + print(f'✓ Door app data test passed: {len(items)} topics') - def test_10_component_data_structure(self): + def test_10_app_data_structure(self): """ - Test GET /components/{component_id}/data response structure. + Test GET /apps/{app_id}/data response structure. @verifies REQ_INTEROP_018 """ - data = self._get_json('/components/temp_sensor/data') - self.assertIsInstance(data, list, 'Response should be an array') + data = self._get_json('/apps/temp_sensor/data') + self.assertIn('items', data) + items = data['items'] + self.assertIsInstance(items, list, 'Response should have items array') # If we have data, verify structure - if len(data) > 0: - first_item = data[0] - self.assertIn('topic', first_item, "Each item should have 'topic' field") - self.assertIn( - 'timestamp', first_item, "Each item should have 'timestamp' field" - ) - self.assertIn('status', first_item, "Each item should have 'status' field") - self.assertIsInstance( - first_item['topic'], str, "'topic' should be a string" - ) + if len(items) > 0: + first_item = items[0] + self.assertIn('id', first_item, "Each item should have 'id' field") + self.assertIn('name', first_item, "Each item should have 'name' field") + self.assertIn('direction', first_item, "Each item should have 'direction' field") + self.assertIn('href', first_item, "Each item should have 'href' field") self.assertIsInstance( - first_item['timestamp'], - int, - "'timestamp' should be an integer (nanoseconds)", + first_item['name'], str, "'name' should be a string" ) - # Status can be 'data' or 'metadata_only' - self.assertIn(first_item['status'], ['data', 'metadata_only']) - if first_item['status'] == 'data': - self.assertIn('data', first_item, "status=data should have 'data'") - self.assertIsInstance( - first_item['data'], dict, "'data' should be object" - ) + self.assertIn(first_item['direction'], ['publish', 'subscribe']) - # Verify metadata fields are present (for both data and metadata_only) - self.assertIn('type', first_item, "Each item should have 'type' field") - self.assertIn('type_info', first_item, "Each item should have 'type_info'") - self.assertIn( - 'publisher_count', first_item, "Should have 'publisher_count'" - ) - self.assertIn( - 'subscriber_count', first_item, "Should have 'subscriber_count'" - ) - - # Verify type_info structure - type_info = first_item['type_info'] - self.assertIn('schema', type_info, "'type_info' should have 'schema'") - self.assertIn( - 'default_value', type_info, "'type_info' should have 'default_value'" - ) + print('✓ App data structure test passed') - print('✓ Component data structure test passed') - - def test_11_component_nonexistent_error(self): + def test_11_app_nonexistent_error(self): """ - Test GET /components/{component_id}/data returns 404 for nonexistent component. + Test GET /apps/{app_id}/data returns 404 for nonexistent app. @verifies REQ_INTEROP_018 """ response = requests.get( - f'{self.BASE_URL}/components/nonexistent_component/data', timeout=5 + f'{self.BASE_URL}/apps/nonexistent_app/data', timeout=5 ) self.assertEqual(response.status_code, 404) data = response.json() self.assertIn('error', data) - self.assertEqual(data['error'], 'Component not found') - self.assertIn('component_id', data) - self.assertEqual(data['component_id'], 'nonexistent_component') + self.assertEqual(data['error'], 'App not found') + self.assertIn('app_id', data) + self.assertEqual(data['app_id'], 'nonexistent_app') - print('✓ Nonexistent component error test passed') + print('✓ Nonexistent app error test passed') - def test_12_component_no_topics(self): + def test_12_app_no_topics(self): """ - Test GET /components/{component_id}/data returns empty array. + Test GET /apps/{app_id}/data returns empty array. - Verifies that components with no topics return an empty array. - The calibration component typically has only services, no topics. + Verifies that apps with no topics return an empty items array. + The calibration app typically has only services, no topics. @verifies REQ_INTEROP_018 """ - # Or test with a component that we know has no publishing topics - # For now, we'll verify that any component returns an array (even if empty) - data = self._get_json('/components/calibration/data') - self.assertIsInstance(data, list, 'Response should be an array even when empty') + # Test with calibration app that we know has no publishing topics + data = self._get_json('/apps/calibration/data') + self.assertIn('items', data) + self.assertIsInstance(data['items'], list, 'Response should have items array') - print(f'✓ Component with no topics test passed: {len(data)} topics') + print(f'✓ App with no topics test passed: {len(data["items"])} topics') - def test_13_invalid_component_id_special_chars(self): + def test_13_invalid_app_id_special_chars(self): """ - Test GET /components/{component_id}/data rejects special characters. + Test GET /apps/{app_id}/data rejects special characters. @verifies REQ_INTEROP_018 """ # Test various invalid characters invalid_ids = [ - 'component;drop', # SQL injection attempt - 'component