diff --git a/docs/config/discovery-options.rst b/docs/config/discovery-options.rst index 4d5a307f..d8cc5bcd 100644 --- a/docs/config/discovery-options.rst +++ b/docs/config/discovery-options.rst @@ -87,6 +87,98 @@ The ``min_topics_for_component`` parameter (default: 1) sets the minimum number of topics required before creating a component. This can filter out namespaces with only a few stray topics. +Merge Pipeline Options (Hybrid Mode) +------------------------------------- + +When using ``hybrid`` mode, the merge pipeline controls how entities from +different discovery layers are combined. The ``merge_pipeline`` section +configures gap-fill behavior for runtime-discovered entities. + +Gap-Fill Configuration +^^^^^^^^^^^^^^^^^^^^^^ + +In hybrid mode, the manifest is the source of truth. Runtime (heuristic) discovery +fills gaps - entities that exist at runtime but aren't in the manifest. Gap-fill +controls restrict what the runtime layer is allowed to create: + +.. code-block:: yaml + + discovery: + merge_pipeline: + gap_fill: + allow_heuristic_areas: true + allow_heuristic_components: true + allow_heuristic_apps: true + allow_heuristic_functions: false + namespace_whitelist: [] + namespace_blacklist: [] + +.. list-table:: Gap-Fill Options + :header-rows: 1 + :widths: 35 15 50 + + * - Parameter + - Default + - Description + * - ``allow_heuristic_areas`` + - ``true`` + - Allow runtime layer to create Area entities not in the manifest + * - ``allow_heuristic_components`` + - ``true`` + - Allow runtime layer to create Component entities not in the manifest + * - ``allow_heuristic_apps`` + - ``true`` + - Allow runtime layer to create App entities not in the manifest + * - ``allow_heuristic_functions`` + - ``false`` + - Allow runtime layer to create Function entities (rarely useful at runtime) + * - ``namespace_whitelist`` + - ``[]`` + - If non-empty, only allow gap-fill from these ROS 2 namespaces (Areas and Components only) + * - ``namespace_blacklist`` + - ``[]`` + - Exclude gap-fill from these ROS 2 namespaces (Areas and Components only) + +When both whitelist and blacklist are empty, all namespaces are eligible for gap-fill. +If whitelist is non-empty, only listed namespaces pass. Blacklist is always applied. + +Namespace matching uses path-segment boundaries: ``/nav`` matches ``/nav`` and ``/nav/sub`` +but NOT ``/navigation``. Both lists only filter Areas and Components (which carry +``namespace_path``). Apps and Functions are not namespace-filtered. + + +Merge Policies +^^^^^^^^^^^^^^ + +Each discovery layer declares a ``MergePolicy`` per field group. When two layers +provide the same entity (matched by ID), policies determine which values win: + +.. list-table:: Merge Policies + :header-rows: 1 + :widths: 25 75 + + * - Policy + - Description + * - ``AUTHORITATIVE`` + - This layer's value wins over lower-priority layers. + If two AUTHORITATIVE layers conflict, a warning is logged and the + higher-priority (earlier) layer wins. + * - ``ENRICHMENT`` + - Fill empty fields from this layer. Non-empty target values are preserved. + Two ENRICHMENT layers merge additively (collections are unioned). + * - ``FALLBACK`` + - Use only if no other layer provides the value. Two FALLBACK layers + merge additively (first non-empty fills gaps). + +**Built-in layer policies:** + +- **ManifestLayer** (priority 1): IDENTITY, HIERARCHY, METADATA are AUTHORITATIVE. + LIVE_DATA is ENRICHMENT (runtime topics/services take precedence). + STATUS is FALLBACK (manifest cannot know online state). +- **RuntimeLayer** (priority 2): LIVE_DATA and STATUS are AUTHORITATIVE. + METADATA is ENRICHMENT. IDENTITY and HIERARCHY are FALLBACK. +- **PluginLayer** (priority 3+): All field groups ENRICHMENT + Configuration Example --------------------- @@ -100,9 +192,6 @@ Complete YAML configuration for runtime discovery: mode: "runtime_only" runtime: - # Map nodes to Apps - expose_nodes_as_apps: true - # Group Apps into Components by namespace create_synthetic_components: true grouping_strategy: "namespace" @@ -112,6 +201,16 @@ Complete YAML configuration for runtime discovery: topic_only_policy: "create_component" min_topics_for_component: 2 # Require at least 2 topics + # Note: merge_pipeline settings only apply when mode is "hybrid" + merge_pipeline: + gap_fill: + allow_heuristic_areas: true + allow_heuristic_components: true + allow_heuristic_apps: true + allow_heuristic_functions: false + namespace_whitelist: [] + namespace_blacklist: ["/rosout", "/parameter_events"] + Command Line Override --------------------- @@ -128,3 +227,5 @@ See Also - :doc:`manifest-schema` - Manifest-based configuration - :doc:`/tutorials/heuristic-apps` - Tutorial on runtime discovery +- :doc:`/tutorials/manifest-discovery` - Hybrid mode tutorial +- :doc:`/tutorials/plugin-system` - Plugin layer integration diff --git a/docs/config/manifest-schema.rst b/docs/config/manifest-schema.rst index 1aa09b79..d66f3249 100644 --- a/docs/config/manifest-schema.rst +++ b/docs/config/manifest-schema.rst @@ -441,7 +441,9 @@ ros_binding Fields **Matching behavior:** -1. **Exact match** (default): ``node_name`` and ``namespace`` must match exactly +1. **Name and namespace match** (default): ``node_name`` must match exactly. + ``namespace`` uses path-segment-boundary matching: ``/nav`` matches ``/nav`` + and ``/nav/sub`` but NOT ``/navigation``. 2. **Wildcard namespace**: Set ``namespace: "*"`` to match node in any namespace 3. **Topic namespace**: Match nodes by their published topic prefix diff --git a/docs/tutorials/manifest-discovery.rst b/docs/tutorials/manifest-discovery.rst index 021be5d3..1a45369e 100644 --- a/docs/tutorials/manifest-discovery.rst +++ b/docs/tutorials/manifest-discovery.rst @@ -119,9 +119,9 @@ Option 1: Using Launch File package='ros2_medkit_gateway', executable='gateway_node', parameters=[{ - 'manifest.enabled': True, - 'manifest.file_path': '/path/to/system_manifest.yaml', - 'manifest.mode': 'hybrid', + 'discovery.mode': 'hybrid', + 'discovery.manifest_path': '/path/to/system_manifest.yaml', + 'discovery.manifest_strict_validation': True, }] ) ]) @@ -137,18 +137,15 @@ Add to your ``gateway_params.yaml``: ros__parameters: # ... existing parameters ... - manifest: - # Enable manifest-based discovery - enabled: true - - # Path to manifest YAML file - file_path: "/path/to/system_manifest.yaml" - + discovery: # Discovery mode: "runtime_only", "hybrid", or "manifest_only" mode: "hybrid" + # Path to manifest YAML file (required for hybrid and manifest_only) + manifest_path: "/path/to/system_manifest.yaml" + # Strict validation: fail on any validation error - strict_validation: true + manifest_strict_validation: true Then launch with: @@ -163,9 +160,8 @@ Option 3: Command Line Parameters .. code-block:: bash ros2 run ros2_medkit_gateway gateway_node --ros-args \ - -p manifest.enabled:=true \ - -p manifest.file_path:=/path/to/system_manifest.yaml \ - -p manifest.mode:=hybrid + -p discovery.mode:=hybrid \ + -p discovery.manifest_path:=/path/to/system_manifest.yaml Verifying the Configuration --------------------------- @@ -207,16 +203,67 @@ List functions: curl http://localhost:8080/api/v1/functions -Understanding Runtime Linking ------------------------------ +Understanding Hybrid Mode +------------------------- + +In hybrid mode, discovery uses a **merge pipeline** that combines entities from +multiple discovery layers: + +1. **ManifestLayer** (highest priority) - entities from the YAML manifest +2. **RuntimeLayer** - entities discovered via ROS 2 graph introspection +3. **PluginLayers** (optional) - entities from gateway plugins + +The pipeline merges entities by ID. When the same entity appears in multiple layers, +per-field-group merge policies determine which values win. See +:doc:`/config/discovery-options` for details on merge policies and gap-fill configuration. + +After merging, the **RuntimeLinker** binds manifest apps to running ROS 2 nodes: + +1. **Discovery**: All layers produce entities +2. **Merging**: Pipeline merges entities by ID, applying field-group policies +3. **Linking**: For each manifest app, checks ``ros_binding`` configuration +4. **Binding**: If match found, copies runtime resources (topics, services, actions) +5. **Status**: Apps with matched nodes are marked ``is_online: true`` + +Merge Report +~~~~~~~~~~~~ + +After each pipeline execution, the gateway produces a ``MergeReport`` available +via the health endpoint (``GET /health``). The report includes: -In hybrid mode, manifest apps are automatically linked to running ROS 2 nodes. -The linking process: +- Layer names and ordering +- Total entity count, enrichment count +- Conflict details (which layers disagreed on which field groups) +- Cross-type ID collision warnings +- Gap-fill filtering statistics -1. **Discovery**: Gateway discovers running ROS 2 nodes -2. **Matching**: For each manifest app, checks ``ros_binding`` configuration -3. **Linking**: If match found, copies runtime resources (topics, services, actions) -4. **Status**: Apps with matched nodes are marked ``is_online: true`` +In hybrid mode, the ``GET /health`` response includes full discovery diagnostics: + +.. code-block:: json + + { + "discovery": { + "mode": "hybrid", + "strategy": "hybrid", + "pipeline": { + "layers": ["manifest", "runtime"], + "total_entities": 12, + "enriched_count": 8, + "conflict_count": 0, + "id_collisions": 0 + }, + "linking": { + "linked_count": 5, + "orphan_count": 1, + "binding_conflicts": 0, + "warnings": ["Orphan node: /unmanifested_node"] + } + } + } + + +Runtime Linking +~~~~~~~~~~~~~~~ ROS Binding Configuration ~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -237,7 +284,8 @@ The ``ros_binding`` section specifies how to match an app to a ROS 2 node: **Match Types:** -- **Exact match** (default): Node name and namespace must match exactly +- **Name and namespace match** (default): Node name must match exactly. Namespace uses + path-segment-boundary matching (``/nav`` matches ``/nav`` and ``/nav/sub`` but NOT ``/navigation``) - **Wildcard namespace**: Use ``namespace: "*"`` to match any namespace - **Topic namespace**: Match nodes by their topic prefix @@ -360,6 +408,12 @@ in the manifest. The ``config.unmanifested_nodes`` setting controls this: - ``error``: Fail startup if orphan nodes detected - ``include_as_orphan``: Include with ``source: "orphan"`` +.. note:: + In hybrid mode with gap-fill configuration (see :doc:`/config/discovery-options`), + namespace filtering controls which runtime entities enter the pipeline. + ``unmanifested_nodes`` controls how runtime nodes that passed gap-fill + but did not match any manifest app are handled by the RuntimeLinker. + Hot Reloading ------------- diff --git a/docs/tutorials/migration-to-manifest.rst b/docs/tutorials/migration-to-manifest.rst index 33e353f3..263993c3 100644 --- a/docs/tutorials/migration-to-manifest.rst +++ b/docs/tutorials/migration-to-manifest.rst @@ -306,9 +306,8 @@ Step 7: Test in Hybrid Mode .. code-block:: bash ros2 run ros2_medkit_gateway gateway_node --ros-args \ - -p manifest.enabled:=true \ - -p manifest.file_path:=/path/to/system_manifest.yaml \ - -p manifest.mode:=hybrid + -p discovery.mode:=hybrid \ + -p discovery.manifest_path:=/path/to/system_manifest.yaml 3. **Check manifest status**: @@ -378,9 +377,9 @@ Run validation: # Start gateway with strict validation ros2 run ros2_medkit_gateway gateway_node --ros-args \ - -p manifest.enabled:=true \ - -p manifest.file_path:=/path/to/system_manifest.yaml \ - -p manifest.strict_validation:=true + -p discovery.mode:=hybrid \ + -p discovery.manifest_path:=/path/to/system_manifest.yaml \ + -p discovery.manifest_strict_validation:=true Common Issues ------------- diff --git a/docs/tutorials/plugin-system.rst b/docs/tutorials/plugin-system.rst index a4f811ce..5f1b74c2 100644 --- a/docs/tutorials/plugin-system.rst +++ b/docs/tutorials/plugin-system.rst @@ -10,8 +10,9 @@ Overview Plugins implement the ``GatewayPlugin`` C++ base class plus one or more typed provider interfaces: - **UpdateProvider** - software update backend (CRUD, prepare/execute, automated, status) -- **IntrospectionProvider** *(preview)* - enriches discovered entities with platform-specific metadata. - This interface is defined and can be implemented, but is not yet wired into the discovery cycle. +- **IntrospectionProvider** - enriches discovered entities with platform-specific metadata + via the merge pipeline. In hybrid mode, each IntrospectionProvider is wrapped as a + ``PluginLayer`` and added to the pipeline with ENRICHMENT merge policy. A single plugin can implement multiple provider interfaces. For example, a "systemd" plugin could provide both introspection (discover systemd units) and updates (manage service restarts). @@ -300,7 +301,12 @@ Multiple Plugins Multiple plugins can be loaded simultaneously: - **UpdateProvider**: Only one plugin's UpdateProvider is used (first in config order) -- **IntrospectionProvider**: All plugins' results are merged *(preview - not yet wired)* +- **IntrospectionProvider**: All plugins are added as PluginLayers to the merge pipeline. + Each plugin's entities are merged with ENRICHMENT policy - they fill empty fields but + never override manifest or runtime values. Plugins are added after all built-in layers, + and the pipeline is refreshed once after all plugins are registered (batch registration). + The ``introspect()`` method receives an ``IntrospectionInput`` populated with all entities + from previous layers (manifest + runtime), enabling context-aware metadata and discovery. - **Custom routes**: All plugins can register endpoints (use unique path prefixes) Error Handling diff --git a/src/ros2_medkit_fault_manager/CMakeLists.txt b/src/ros2_medkit_fault_manager/CMakeLists.txt index ac1e7f88..196b6488 100644 --- a/src/ros2_medkit_fault_manager/CMakeLists.txt +++ b/src/ros2_medkit_fault_manager/CMakeLists.txt @@ -192,12 +192,14 @@ if(BUILD_TESTING) TARGET test_integration TIMEOUT 60 ) + set_tests_properties(test_integration PROPERTIES LABELS "integration") add_launch_test( test/test_rosbag_integration.test.py TARGET test_rosbag_integration TIMEOUT 90 ) + set_tests_properties(test_rosbag_integration PROPERTIES LABELS "integration") endif() ament_package() diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index 835d05ec..7d74a15c 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -92,6 +92,10 @@ add_library(gateway_lib STATIC src/discovery/discovery_manager.cpp src/discovery/runtime_discovery.cpp src/discovery/hybrid_discovery.cpp + src/discovery/merge_pipeline.cpp + src/discovery/layers/manifest_layer.cpp + src/discovery/layers/runtime_layer.cpp + src/discovery/layers/plugin_layer.cpp # Discovery models (with .cpp serialization) src/discovery/models/app.cpp src/discovery/models/function.cpp @@ -184,6 +188,7 @@ target_precompile_headers(gateway_lib PRIVATE ) +set_target_properties(gateway_lib PROPERTIES POSITION_INDEPENDENT_CODE ON) # Gateway node executable add_executable(gateway_node src/main.cpp) @@ -325,6 +330,10 @@ if(BUILD_TESTING) ament_add_gtest(test_runtime_linker test/test_runtime_linker.cpp) target_link_libraries(test_runtime_linker gateway_lib) + # Add merge pipeline tests + ament_add_gtest(test_merge_pipeline test/test_merge_pipeline.cpp) + target_link_libraries(test_merge_pipeline gateway_lib) + # Add capability builder tests ament_add_gtest(test_capability_builder test/test_capability_builder.cpp) target_link_libraries(test_capability_builder gateway_lib) @@ -488,6 +497,8 @@ if(BUILD_TESTING) test_plugin_manager test_log_manager test_log_handlers + test_merge_pipeline + test_runtime_linker ) foreach(_target ${_test_targets}) target_compile_options(${_target} PRIVATE --coverage -O0 -g) diff --git a/src/ros2_medkit_gateway/config/gateway_params.yaml b/src/ros2_medkit_gateway/config/gateway_params.yaml index f85cb0ff..3af7d64d 100644 --- a/src/ros2_medkit_gateway/config/gateway_params.yaml +++ b/src/ros2_medkit_gateway/config/gateway_params.yaml @@ -172,6 +172,36 @@ ros2_medkit_gateway: # Default: 1 (create component for any namespace with topics) min_topics_for_component: 1 + # Merge pipeline configuration (only used in hybrid mode) + # Controls how manifest, runtime, and plugin layers merge entities + merge_pipeline: + # Gap-fill: what runtime discovery can create when manifest is incomplete + gap_fill: + allow_heuristic_areas: true + allow_heuristic_components: true + allow_heuristic_apps: true + allow_heuristic_functions: false + # namespace_blacklist: ["/rosout"] + # namespace_whitelist: [] + + # Per-layer merge policy overrides (optional, empty string = use layer default) + # Values: "authoritative", "enrichment", "fallback" + # Defaults: manifest=AUTH for identity/hierarchy/metadata, ENRICH for live_data, FALLBACK for status + # runtime=AUTH for live_data/status, ENRICH for metadata, FALLBACK for identity/hierarchy + layers: + manifest: + identity: "" + hierarchy: "" + live_data: "" + status: "" + metadata: "" + runtime: + identity: "" + hierarchy: "" + live_data: "" + status: "" + metadata: "" + # 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/design/architecture.puml b/src/ros2_medkit_gateway/design/architecture.puml index 0d5a581b..3e8ebb62 100644 --- a/src/ros2_medkit_gateway/design/architecture.puml +++ b/src/ros2_medkit_gateway/design/architecture.puml @@ -73,8 +73,29 @@ package "ros2_medkit_gateway" { class EntityCache { + areas: vector + components: vector + + apps: vector + last_update: time_point } + + class MergePipeline { + + add_layer(): void + + execute(): MergeResult + + set_linker(): void + } + + interface DiscoveryLayer <> { + + name(): string + + discover(): LayerOutput + + policy_for(FieldGroup): MergePolicy + } + + class ManifestLayer + class RuntimeLayer + class PluginLayer + + class RuntimeLinker { + + link(): LinkingResult + } } package "External Libraries" { @@ -115,6 +136,13 @@ EntityCache o-right-> Component : contains many DiscoveryManager ..> Area : creates DiscoveryManager ..> Component : creates +' MergePipeline layer architecture +MergePipeline o--> DiscoveryLayer : ordered layers +MergePipeline --> RuntimeLinker : post-merge linking +ManifestLayer .up.|> DiscoveryLayer : implements +RuntimeLayer .up.|> DiscoveryLayer : implements +PluginLayer .up.|> DiscoveryLayer : implements + ' REST Server uses HTTP library RESTServer *--> HTTPLibServer : owns @@ -123,4 +151,3 @@ Area ..> JSON : serializes to Component ..> JSON : serializes to @enduml - diff --git a/src/ros2_medkit_gateway/design/index.rst b/src/ros2_medkit_gateway/design/index.rst index ea0ad6ae..120b4b50 100644 --- a/src/ros2_medkit_gateway/design/index.rst +++ b/src/ros2_medkit_gateway/design/index.rst @@ -55,7 +55,6 @@ The following diagram shows the relationships between the main components of the } class RuntimeDiscoveryStrategy { - + discover_node_components(): vector + discover_synthetic_components(): vector + discover_topic_components(): vector - config_: RuntimeConfig @@ -67,8 +66,41 @@ The following diagram shows the relationships between the main components of the } class HybridDiscoveryStrategy { - - primary_: ManifestDiscoveryStrategy - - runtime_: RuntimeDiscoveryStrategy + - pipeline_: MergePipeline + + refresh(): void + + add_layer(): void + + get_merge_report(): MergeReport + } + + class MergePipeline { + + add_layer(): void + + execute(): MergeResult + + set_linker(): void + + get_last_report(): MergeReport + - merge_entities(): vector + } + + interface DiscoveryLayer <> { + + name(): string + + discover(): LayerOutput + + policy_for(FieldGroup): MergePolicy + } + + class ManifestLayer { + - manifest_mgr_: ManifestManager* + } + + class RuntimeLayer { + - strategy_: RuntimeDiscoveryStrategy* + - gap_fill_config_: GapFillConfig + } + + class PluginLayer { + - provider_: IntrospectionProvider* + } + + class RuntimeLinker { + + link(): LinkingResult } class OperationManager { @@ -229,8 +261,15 @@ The following diagram shows the relationships between the main components of the RuntimeDiscoveryStrategy .up.|> DiscoveryStrategy : implements ManifestDiscoveryStrategy .up.|> DiscoveryStrategy : implements HybridDiscoveryStrategy .up.|> DiscoveryStrategy : implements - HybridDiscoveryStrategy --> ManifestDiscoveryStrategy : delegates - HybridDiscoveryStrategy --> RuntimeDiscoveryStrategy : delegates + HybridDiscoveryStrategy *--> MergePipeline : owns + + ' MergePipeline layer architecture + MergePipeline o--> DiscoveryLayer : ordered layers + MergePipeline --> RuntimeLinker : post-merge linking + ManifestLayer .up.|> DiscoveryLayer : implements + RuntimeLayer .up.|> DiscoveryLayer : implements + PluginLayer .up.|> DiscoveryLayer : implements + RuntimeLayer --> RuntimeDiscoveryStrategy : delegates ' REST Server uses HTTP library RESTServer *--> HTTPLibServer : owns @@ -270,9 +309,31 @@ Main Components - **ManifestDiscoveryStrategy** - Static discovery from YAML manifest - Provides stable, semantic entity IDs - Supports offline detection of failed components - - **HybridDiscoveryStrategy** - Combines manifest + runtime - - Manifest defines structure, runtime links to live nodes - - Best for production systems requiring stability + live status + - **HybridDiscoveryStrategy** - Combines manifest + runtime via MergePipeline + - Delegates to ``MergePipeline`` which orchestrates ordered discovery layers + - Supports dynamic plugin layers added at runtime + - Thread-safe: mutex protects cached results, returns by value + + **Merge Pipeline:** + + The ``MergePipeline`` is the core engine for hybrid discovery. It: + + - Maintains an ordered list of ``DiscoveryLayer`` instances (first = highest priority) + - Executes all layers, collects entities by ID, and merges them per-field-group + - Each layer declares a ``MergePolicy`` per ``FieldGroup``: AUTHORITATIVE (wins), ENRICHMENT (fills empty), FALLBACK (last resort) + - Runs ``RuntimeLinker`` post-merge to bind manifest apps to live ROS 2 nodes + - Produces a ``MergeReport`` with conflict diagnostics, enrichment counts, and ID collision detection + + **Built-in Layers:** + + - ``ManifestLayer`` - Wraps ManifestManager; IDENTITY/HIERARCHY/METADATA are AUTHORITATIVE, + LIVE_DATA is ENRICHMENT (runtime wins for topics/services), STATUS is FALLBACK + - ``RuntimeLayer`` - Wraps RuntimeDiscoveryStrategy; LIVE_DATA/STATUS are AUTHORITATIVE, + METADATA is ENRICHMENT, IDENTITY/HIERARCHY are FALLBACK. + Supports ``GapFillConfig`` to control which heuristic entities are allowed when manifest is present + - ``PluginLayer`` - Wraps IntrospectionProvider; all fields ENRICHMENT (plugins enrich, they don't override). + Before each layer's ``discover()`` call, the pipeline populates ``IntrospectionInput`` with entities + from all previous layers, so plugins see the current manifest + runtime entity set 3. **OperationManager** - Executes ROS 2 operations (services and actions) using native APIs - Calls ROS 2 services via ``rclcpp::GenericClient`` with native serialization diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_layer.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_layer.hpp new file mode 100644 index 00000000..5a612e4e --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_layer.hpp @@ -0,0 +1,79 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "ros2_medkit_gateway/discovery/merge_types.hpp" +#include "ros2_medkit_gateway/discovery/models/app.hpp" +#include "ros2_medkit_gateway/discovery/models/area.hpp" +#include "ros2_medkit_gateway/discovery/models/component.hpp" +#include "ros2_medkit_gateway/discovery/models/function.hpp" + +#include + +#include +#include +#include + +namespace ros2_medkit_gateway { + +// Forward declaration +struct IntrospectionInput; + +namespace discovery { + +/** + * @brief Output produced by a discovery layer + */ +struct LayerOutput { + std::vector areas; + std::vector components; + std::vector apps; + std::vector functions; + std::unordered_map entity_metadata; ///< entity id -> metadata +}; + +/** + * @brief Interface for a pluggable discovery data source + * + * Each layer produces entities and declares how its data should be merged + * with other layers via per-field-group MergePolicy. + */ +class DiscoveryLayer { + public: + virtual ~DiscoveryLayer() = default; + + /// Human-readable layer name (e.g., "manifest", "runtime", plugin name) + virtual std::string name() const = 0; + + /// Discover entities from this layer's source + virtual LayerOutput discover() = 0; + + /// Merge policy this layer uses for the given field group + virtual MergePolicy policy_for(FieldGroup group) const = 0; + + /// Whether this layer provides runtime apps for post-merge linking. + /// Only RuntimeLayer (or test doubles) should return true. + virtual bool provides_runtime_apps() const { + return false; + } + + /// Provide the current discovery context (entities from previous layers). + /// Called by MergePipeline before discover(). Default no-op. + virtual void set_discovery_context(const IntrospectionInput & /*context*/) { + } +}; + +} // namespace discovery +} // namespace ros2_medkit_gateway 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 0b33f35a..53eef734 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 @@ -18,6 +18,7 @@ #include "ros2_medkit_gateway/discovery/discovery_strategy.hpp" #include "ros2_medkit_gateway/discovery/hybrid_discovery.hpp" #include "ros2_medkit_gateway/discovery/manifest/manifest_manager.hpp" +#include "ros2_medkit_gateway/discovery/merge_types.hpp" #include "ros2_medkit_gateway/discovery/models/app.hpp" #include "ros2_medkit_gateway/discovery/models/area.hpp" #include "ros2_medkit_gateway/discovery/models/common.hpp" @@ -25,6 +26,7 @@ #include "ros2_medkit_gateway/discovery/models/function.hpp" #include "ros2_medkit_gateway/discovery/runtime_discovery.hpp" +#include #include #include #include @@ -36,6 +38,7 @@ namespace ros2_medkit_gateway { // Forward declarations class NativeTopicSampler; class TypeIntrospection; +class IntrospectionProvider; /** * @brief Configuration for discovery @@ -97,6 +100,16 @@ struct DiscoveryConfig { */ int min_topics_for_component{1}; } runtime; + + /** + * @brief Merge pipeline configuration (hybrid mode only) + */ + struct MergePipelineConfig { + discovery::GapFillConfig gap_fill; + /// Per-layer merge policy overrides: layer_name -> (field_group_name -> policy_name) + /// e.g. {"manifest": {"live_data": "authoritative"}, "runtime": {"identity": "authoritative"}} + std::map> layer_policies; + } merge_pipeline; }; /** @@ -303,19 +316,28 @@ class DiscoveryManager { // ========================================================================= /** - * @brief Get the manifest manager - * @return Pointer to manifest manager (nullptr if not using manifest) + * @brief Add a plugin layer to the merge pipeline + * + * Wraps an IntrospectionProvider as a PluginLayer and adds it to + * the pipeline. Only works in HYBRID mode. + * + * @param plugin_name Name of the plugin + * @param provider Non-owning pointer to IntrospectionProvider */ - discovery::ManifestManager * get_manifest_manager(); + void add_plugin_layer(const std::string & plugin_name, IntrospectionProvider * provider); /** - * @brief Reload manifest from file - * - * Only works if a manifest was loaded during initialize(). + * @brief Re-execute the merge pipeline (hybrid mode only) * - * @return true if reload succeeded + * Call after adding plugin layers to trigger a single pipeline refresh. + */ + void refresh_pipeline(); + + /** + * @brief Get the manifest manager + * @return Pointer to manifest manager (nullptr if not using manifest) */ - bool reload_manifest(); + discovery::ManifestManager * get_manifest_manager(); // ========================================================================= // Status @@ -335,12 +357,30 @@ class DiscoveryManager { */ std::string get_strategy_name() const; + /** + * @brief Get the last merge pipeline report (hybrid mode only) + * @return MergeReport if in hybrid mode, nullopt otherwise + */ + std::optional get_merge_report() const; + + /** + * @brief Get the last linking result (hybrid mode only) + * @return LinkingResult if in hybrid mode, nullopt otherwise + */ + std::optional get_linking_result() const; + private: /** * @brief Create and activate the appropriate strategy */ void create_strategy(); + /** + * @brief Apply user-configured merge policy overrides to a layer + */ + template + void apply_layer_policy_overrides(const std::string & layer_name, LayerT & layer); + rclcpp::Node * node_; DiscoveryConfig config_; diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/hybrid_discovery.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/hybrid_discovery.hpp index 1a42afc9..75c6db43 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/hybrid_discovery.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/hybrid_discovery.hpp @@ -14,10 +14,9 @@ #pragma once +#include "ros2_medkit_gateway/discovery/discovery_layer.hpp" #include "ros2_medkit_gateway/discovery/discovery_strategy.hpp" -#include "ros2_medkit_gateway/discovery/manifest/manifest_manager.hpp" -#include "ros2_medkit_gateway/discovery/manifest/runtime_linker.hpp" -#include "ros2_medkit_gateway/discovery/runtime_discovery.hpp" +#include "ros2_medkit_gateway/discovery/merge_pipeline.hpp" #include @@ -30,103 +29,60 @@ namespace ros2_medkit_gateway { namespace discovery { /** - * @brief Hybrid discovery combining manifest and runtime discovery + * @brief Hybrid discovery using a MergePipeline * - * Uses manifest as source of truth for entity IDs and hierarchy while - * linking to runtime ROS 2 nodes for live data (topics, services, actions). - * - * Behavior: - * - Areas: From manifest (runtime areas not exposed unless orphan policy allows) - * - Components: From manifest, enriched with runtime data if linked - * - Apps: From manifest, bound to runtime nodes via RuntimeLinker - * - Functions: From manifest only - * - * The hybrid strategy maintains a RuntimeLinker that binds manifest Apps - * to actual ROS 2 nodes discovered at runtime. + * Thin wrapper around MergePipeline that caches the merged result + * and exposes it through the DiscoveryStrategy interface. + * The pipeline orchestrates ManifestLayer, RuntimeLayer, and any + * PluginLayers with per-field-group merge policies. */ class HybridDiscoveryStrategy : public DiscoveryStrategy { public: /** * @brief Construct hybrid discovery strategy * @param node ROS 2 node for logging - * @param manifest_manager Manifest manager (must be loaded before use) - * @param runtime_strategy Runtime discovery strategy for ROS graph introspection + * @param pipeline Pre-configured merge pipeline */ - HybridDiscoveryStrategy(rclcpp::Node * node, ManifestManager * manifest_manager, - RuntimeDiscoveryStrategy * runtime_strategy); + HybridDiscoveryStrategy(rclcpp::Node * node, MergePipeline pipeline); - /** - * @brief Discover areas from manifest - * @return Areas defined in manifest - */ std::vector discover_areas() override; - - /** - * @brief Discover components from manifest, linked to runtime - * @return Components with runtime data if linked - */ std::vector discover_components() override; - - /** - * @brief Discover apps from manifest, linked to runtime nodes - * @return Apps with is_online and bound_fqn set - */ std::vector discover_apps() override; - - /** - * @brief Discover functions from manifest - * @return Functions defined in manifest - */ std::vector discover_functions() override; - /** - * @brief Get strategy name - * @return "hybrid" - */ std::string get_name() const override { return "hybrid"; } /** - * @brief Refresh runtime linking - * - * Call this after runtime discovery refresh to update app-node bindings. - * This will re-run the RuntimeLinker with fresh runtime component data. + * @brief Re-execute the pipeline and cache the result */ - void refresh_linking(); + void refresh(); /** - * @brief Get the last linking result - * @return Reference to last linking result + * @brief Get the last merge report (returned by value for thread safety) */ - const LinkingResult & get_linking_result() const { - return linking_result_; - } + MergeReport get_merge_report() const; /** - * @brief Get orphan nodes from last linking - * @return Vector of node FQNs not bound to any manifest app + * @brief Get the last linking result (returned by value for thread safety) */ - const std::vector & get_orphan_nodes() const { - return linking_result_.orphan_nodes; - } + LinkingResult get_linking_result() const; - private: /** - * @brief Perform initial linking on construction + * @brief Get orphan nodes from last linking */ - void perform_linking(); + std::vector get_orphan_nodes() const; /** - * @brief Log message at info level + * @brief Add a discovery layer to the pipeline (e.g., plugin layers) */ - void log_info(const std::string & msg) const; + void add_layer(std::unique_ptr layer); + private: rclcpp::Node * node_; - ManifestManager * manifest_manager_; - RuntimeDiscoveryStrategy * runtime_strategy_; - RuntimeLinker linker_; - LinkingResult linking_result_; + MergePipeline pipeline_; + MergeResult cached_result_; mutable std::mutex mutex_; }; diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/layers/manifest_layer.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/layers/manifest_layer.hpp new file mode 100644 index 00000000..97bc0ede --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/layers/manifest_layer.hpp @@ -0,0 +1,49 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "ros2_medkit_gateway/discovery/discovery_layer.hpp" +#include "ros2_medkit_gateway/discovery/manifest/manifest_manager.hpp" + +#include + +namespace ros2_medkit_gateway { +namespace discovery { + +/** + * @brief Discovery layer wrapping ManifestManager + * + * Default policies: IDENTITY=AUTH, HIERARCHY=AUTH, LIVE_DATA=ENRICH, + * STATUS=FALLBACK, METADATA=AUTH + */ +class ManifestLayer : public DiscoveryLayer { + public: + explicit ManifestLayer(ManifestManager * manifest_manager); + + std::string name() const override { + return "manifest"; + } + LayerOutput discover() override; + MergePolicy policy_for(FieldGroup group) const override; + + void set_policy(FieldGroup group, MergePolicy policy); + + private: + ManifestManager * manifest_manager_; + std::unordered_map policies_; +}; + +} // namespace discovery +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/layers/plugin_layer.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/layers/plugin_layer.hpp new file mode 100644 index 00000000..dbf80a9d --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/layers/plugin_layer.hpp @@ -0,0 +1,62 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "ros2_medkit_gateway/discovery/discovery_layer.hpp" +#include "ros2_medkit_gateway/providers/introspection_provider.hpp" + +#include +#include + +#include +#include + +namespace ros2_medkit_gateway { +namespace discovery { + +/** + * @brief Discovery layer wrapping an IntrospectionProvider plugin + * + * Default policies: all ENRICHMENT (plugins enrich, they don't override) + */ +class PluginLayer : public DiscoveryLayer { + public: + PluginLayer(std::string plugin_name, IntrospectionProvider * provider); + + std::string name() const override { + return name_; + } + LayerOutput discover() override; + MergePolicy policy_for(FieldGroup group) const override; + void set_discovery_context(const IntrospectionInput & context) override; + + void set_policy(FieldGroup group, MergePolicy policy); + + /// Get per-entity metadata from last discover() call + const std::unordered_map & get_metadata() const { + return last_metadata_; + } + + private: + std::string name_; + IntrospectionProvider * provider_; + rclcpp::Logger logger_; + std::unordered_map policies_; + std::unordered_map last_metadata_; + IntrospectionInput discovery_context_; +}; + +} // namespace discovery +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/layers/runtime_layer.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/layers/runtime_layer.hpp new file mode 100644 index 00000000..da276606 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/layers/runtime_layer.hpp @@ -0,0 +1,65 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "ros2_medkit_gateway/discovery/discovery_layer.hpp" +#include "ros2_medkit_gateway/discovery/merge_types.hpp" +#include "ros2_medkit_gateway/discovery/runtime_discovery.hpp" + +#include + +namespace ros2_medkit_gateway { +namespace discovery { + +/** + * @brief Discovery layer wrapping RuntimeDiscoveryStrategy + * + * Default policies: IDENTITY=FALLBACK, HIERARCHY=FALLBACK, LIVE_DATA=AUTH, + * STATUS=AUTH, METADATA=ENRICH + */ +class RuntimeLayer : public DiscoveryLayer { + public: + explicit RuntimeLayer(RuntimeDiscoveryStrategy * runtime_strategy); + + std::string name() const override { + return "runtime"; + } + bool provides_runtime_apps() const override { + return true; + } + LayerOutput discover() override; + MergePolicy policy_for(FieldGroup group) const override; + + void set_policy(FieldGroup group, MergePolicy policy); + void set_gap_fill_config(GapFillConfig config); + + /// Number of entities filtered by gap-fill config in last discover() + size_t last_filtered_count() const { + return last_filtered_count_; + } + + /// Direct access to runtime services (for operation/data endpoints, not part of pipeline) + std::vector discover_services(); + std::vector discover_actions(); + + private: + RuntimeDiscoveryStrategy * runtime_strategy_; + std::unordered_map policies_; + GapFillConfig gap_fill_config_; + size_t last_filtered_count_{0}; +}; + +} // namespace discovery +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/manifest/runtime_linker.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/manifest/runtime_linker.hpp index 93cf4492..1a9ed2a4 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/manifest/runtime_linker.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/manifest/runtime_linker.hpp @@ -16,7 +16,6 @@ #include "ros2_medkit_gateway/discovery/manifest/manifest.hpp" #include "ros2_medkit_gateway/discovery/models/app.hpp" -#include "ros2_medkit_gateway/discovery/models/component.hpp" #include @@ -51,6 +50,15 @@ struct LinkingResult { /// Mapping from node FQN to App ID (reverse lookup) std::unordered_map node_to_app; + /// Number of times two apps competed for the same node + size_t binding_conflicts{0}; + + /// Number of wildcard bindings that matched >1 node + size_t wildcard_multi_match{0}; + + /// Human-readable diagnostic warnings + std::vector warnings; + /// Check if linking produced any errors based on policy bool has_errors(ManifestConfig::UnmanifestedNodePolicy policy) const { return policy == ManifestConfig::UnmanifestedNodePolicy::ERROR && !orphan_nodes.empty(); @@ -58,8 +66,12 @@ struct LinkingResult { /// Get statistics summary std::string summary() const { - return std::to_string(app_to_node.size()) + " linked, " + std::to_string(unlinked_app_ids.size()) + " unlinked, " + - std::to_string(orphan_nodes.size()) + " orphan nodes"; + std::string s = std::to_string(app_to_node.size()) + " linked, " + std::to_string(unlinked_app_ids.size()) + + " unlinked, " + std::to_string(orphan_nodes.size()) + " orphan nodes"; + if (binding_conflicts > 0) { + s += ", " + std::to_string(binding_conflicts) + " binding conflict(s)"; + } + return s; } }; @@ -86,14 +98,14 @@ class RuntimeLinker { explicit RuntimeLinker(rclcpp::Node * node = nullptr); /** - * @brief Link manifest apps to runtime nodes + * @brief Link manifest apps to runtime apps (nodes) * - * @param apps Apps from manifest - * @param runtime_components Components discovered from ROS graph + * @param manifest_apps Apps from manifest + * @param runtime_apps Apps discovered from ROS graph (each node is an App) * @param config Manifest config with orphan policy * @return LinkingResult with linked apps and orphan info */ - LinkingResult link(const std::vector & apps, const std::vector & runtime_components, + LinkingResult link(const std::vector & manifest_apps, const std::vector & runtime_apps, const ManifestConfig & config); /** @@ -139,17 +151,17 @@ class RuntimeLinker { /** * @brief Try to match by topic namespace * @param topic_namespace Topic namespace pattern from binding - * @param component Component with topic info + * @param runtime_app Runtime app with topic info * @return true if any topic matches the prefix */ - bool matches_topic_namespace(const std::string & topic_namespace, const Component & component) const; + bool matches_topic_namespace(const std::string & topic_namespace, const App & runtime_app) const; /** - * @brief Enrich app with runtime data from matched component + * @brief Enrich manifest app with runtime data from matched node * @param app App to enrich (modified in place) - * @param component Component with runtime data + * @param runtime_app Runtime app with live data */ - void enrich_app(App & app, const Component & component); + void enrich_app(App & app, const App & runtime_app); /** * @brief Log message at info level diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/merge_pipeline.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/merge_pipeline.hpp new file mode 100644 index 00000000..e193a298 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/merge_pipeline.hpp @@ -0,0 +1,102 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "ros2_medkit_gateway/discovery/discovery_layer.hpp" +#include "ros2_medkit_gateway/discovery/manifest/manifest.hpp" +#include "ros2_medkit_gateway/discovery/manifest/runtime_linker.hpp" +#include "ros2_medkit_gateway/discovery/merge_types.hpp" + +#include +#include + +#include +#include + +namespace ros2_medkit_gateway { +namespace discovery { + +/** + * @brief Result of a merge pipeline execution + */ +struct MergeResult { + std::vector areas; + std::vector components; + std::vector apps; + std::vector functions; + MergeReport report; + LinkingResult linking_result; +}; + +/** + * @brief Orchestrates multiple discovery layers with configurable merge policies + * + * Layers are added in priority order (first added = highest priority). + * Each layer produces entities and declares per-field-group MergePolicy. + * The pipeline merges entities by ID, resolving conflicts per policy. + */ +class MergePipeline { + public: + explicit MergePipeline(rclcpp::Logger logger = rclcpp::get_logger("merge_pipeline")); + + /** + * @brief Add a discovery layer to the pipeline + * @param layer Layer to add (priority = insertion order, first = highest) + */ + void add_layer(std::unique_ptr layer); + + /** + * @brief Execute all layers and merge results + * @return Merged entities with diagnostics report + */ + MergeResult execute(); + + /** + * @brief Get the last merge report (returned by value for thread safety) + */ + MergeReport get_last_report() const { + return last_report_; + } + + /** + * @brief Set RuntimeLinker for post-merge app-to-node binding + * @param linker RuntimeLinker instance + * @param config ManifestConfig needed by RuntimeLinker::link() for unmanifested node policy + */ + void set_linker(std::unique_ptr linker, const ManifestConfig & config); + + /** + * @brief Get the last linking result (returned by value for thread safety) + */ + LinkingResult get_linking_result() const { + return linking_result_; + } + + private: + /// Merge a vector of entities from multiple layers by ID + template + std::vector merge_entities(std::vector>> & layer_entities, + MergeReport & report); + + rclcpp::Logger logger_; + std::vector> layers_; + MergeReport last_report_; + std::unique_ptr linker_; + ManifestConfig manifest_config_; + LinkingResult linking_result_; +}; + +} // namespace discovery +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/merge_types.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/merge_types.hpp new file mode 100644 index 00000000..c7d7daee --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/merge_types.hpp @@ -0,0 +1,155 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace ros2_medkit_gateway { +namespace discovery { + +/** + * @brief Merge precedence policy for a discovery layer's field group + */ +enum class MergePolicy { + AUTHORITATIVE, ///< This layer's value wins over lower-priority layers + ENRICHMENT, ///< Fill empty fields only, don't override existing values + FALLBACK ///< Use only if no other layer provides the value +}; + +/** + * @brief Logical groupings of entity fields with the same merge behavior + */ +enum class FieldGroup { + IDENTITY, ///< id, name, translation_id, description, tags + HIERARCHY, ///< area, component_id, parent_*, depends_on, hosts + LIVE_DATA, ///< topics, services, actions + STATUS, ///< is_online, bound_fqn + METADATA ///< source, x-medkit extensions, custom fields +}; + +inline const char * field_group_to_string(FieldGroup fg) { + switch (fg) { + case FieldGroup::IDENTITY: + return "IDENTITY"; + case FieldGroup::HIERARCHY: + return "HIERARCHY"; + case FieldGroup::LIVE_DATA: + return "LIVE_DATA"; + case FieldGroup::STATUS: + return "STATUS"; + case FieldGroup::METADATA: + return "METADATA"; + } + return "UNKNOWN"; +} + +/// Parse a FieldGroup from its lowercase string form (e.g. "identity", "live_data") +inline std::optional field_group_from_string(const std::string & s) { + if (s == "identity") { + return FieldGroup::IDENTITY; + } + if (s == "hierarchy") { + return FieldGroup::HIERARCHY; + } + if (s == "live_data") { + return FieldGroup::LIVE_DATA; + } + if (s == "status") { + return FieldGroup::STATUS; + } + if (s == "metadata") { + return FieldGroup::METADATA; + } + return std::nullopt; +} + +/// Parse a MergePolicy from its lowercase string form (e.g. "authoritative", "enrichment", "fallback") +inline std::optional merge_policy_from_string(const std::string & s) { + if (s == "authoritative") { + return MergePolicy::AUTHORITATIVE; + } + if (s == "enrichment") { + return MergePolicy::ENRICHMENT; + } + if (s == "fallback") { + return MergePolicy::FALLBACK; + } + return std::nullopt; +} + +/** + * @brief Record of a merge conflict between two layers + */ +struct MergeConflict { + std::string entity_id; + FieldGroup field_group; + std::string winning_layer; + std::string losing_layer; +}; + +/** + * @brief Diagnostics report from a merge pipeline execution + */ +struct MergeReport { + std::vector layers; + std::vector conflicts; + std::unordered_map entity_source; ///< entity id -> primary layer name + size_t total_entities{0}; + size_t enriched_count{0}; + size_t conflict_count{0}; + size_t id_collision_count{0}; + size_t filtered_by_gap_fill{0}; + + nlohmann::json to_json() const { + nlohmann::json conflict_list = nlohmann::json::array(); + for (const auto & c : conflicts) { + conflict_list.push_back({{"entity_id", c.entity_id}, + {"field_group", field_group_to_string(c.field_group)}, + {"winning_layer", c.winning_layer}, + {"losing_layer", c.losing_layer}}); + } + return {{"layers", layers}, + {"total_entities", total_entities}, + {"enriched_count", enriched_count}, + {"conflict_count", conflict_count}, + {"conflicts", conflict_list}, + {"id_collisions", id_collision_count}, + {"filtered_by_gap_fill", filtered_by_gap_fill}}; + } +}; + +/** + * @brief Controls what heuristic (runtime) discovery is allowed to create + * + * When manifest is present, runtime entities fill gaps. This struct + * controls which entity types and namespaces are eligible for gap-fill. + */ +struct GapFillConfig { + bool allow_heuristic_areas{true}; + bool allow_heuristic_components{true}; + bool allow_heuristic_apps{true}; + bool allow_heuristic_functions{false}; + std::vector namespace_whitelist; + std::vector namespace_blacklist; +}; + +} // namespace discovery +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/models/area.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/models/area.hpp index f5aec631..de2c34e2 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/models/area.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/models/area.hpp @@ -37,6 +37,7 @@ struct Area { std::string description; ///< Human-readable description std::vector tags; ///< Tags for filtering std::string parent_area_id; ///< Parent area ID for sub-areas + std::string source; ///< Origin of this area (e.g., "manifest", "heuristic") /** * @brief Convert to JSON representation @@ -67,6 +68,9 @@ struct Area { if (!parent_area_id.empty()) { x_medkit["parentAreaId"] = parent_area_id; } + if (!source.empty()) { + x_medkit["source"] = source; + } j["x-medkit"] = x_medkit; return j; 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 2cea3521..a0c6e658 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 @@ -86,6 +86,9 @@ class RuntimeDiscoveryStrategy : public DiscoveryStrategy { /// @copydoc DiscoveryStrategy::discover_components std::vector discover_components() override; + /// Discover components using pre-discovered apps (avoids redundant graph introspection) + std::vector discover_components(const std::vector & apps); + /// @copydoc DiscoveryStrategy::discover_apps /// @note Returns nodes as Apps in runtime discovery std::vector discover_apps() override; @@ -103,26 +106,16 @@ 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. + * Groups runtime apps by namespace into aggregated Component entities. + * Uses provided apps to avoid re-querying the ROS 2 graph. * + * @param apps Pre-discovered apps (from discover_apps()) * @return Vector of synthetic components */ - std::vector discover_synthetic_components(); + std::vector discover_synthetic_components(const std::vector & apps); /** * @brief Discover components from topic namespaces (topic-based discovery) @@ -203,7 +196,7 @@ class RuntimeDiscoveryStrategy : public DiscoveryStrategy { 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); + std::string derive_component_id(const std::string & node_id, const std::string & area); /// Apply naming pattern for synthetic component ID std::string apply_component_name_pattern(const std::string & area); diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/plugins/plugin_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/plugins/plugin_manager.hpp index 6e0b4347..11d7f66b 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/plugins/plugin_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/plugins/plugin_manager.hpp @@ -127,6 +127,12 @@ class PluginManager { */ std::vector get_log_observers() const; + /** + * @brief Get all introspection providers with their plugin names + * @return (plugin_name, provider) pairs for all IntrospectionProvider plugins + */ + std::vector> get_named_introspection_providers() const; + // ---- Capability queries (used by discovery handlers) ---- /// Get plugin context (for capability queries from discovery handlers) diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/providers/introspection_provider.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/providers/introspection_provider.hpp index 95271e3e..05cbfa09 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/providers/introspection_provider.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/providers/introspection_provider.hpp @@ -49,8 +49,9 @@ struct NewEntities { * @brief Result returned by IntrospectionProvider::introspect() */ struct IntrospectionResult { - /// Per-entity metadata enrichment. Key = entity_id. - /// Values are deep-merged into the entity's x-medkit vendor extension. + /// Per-entity metadata for plugin-internal use. Key = entity_id. + /// Plugins serve this data as SOVD vendor extension resources + /// via register_routes() and register_capability(). std::unordered_map metadata; /// New entities discovered by this provider @@ -73,9 +74,10 @@ class IntrospectionProvider { /** * @brief Core introspection method * - * Called after each discovery cycle, before EntityCache update. + * Called during each discovery cycle by the merge pipeline. + * Input contains entities from all higher-priority layers (manifest + runtime). * - * @param input Snapshot of currently discovered entities + * @param input Snapshot of entities discovered by previous layers * @return Metadata enrichments and new entities */ virtual IntrospectionResult introspect(const IntrospectionInput & input) = 0; diff --git a/src/ros2_medkit_gateway/src/bulk_data_store.cpp b/src/ros2_medkit_gateway/src/bulk_data_store.cpp index 3b0b0842..831b75ac 100644 --- a/src/ros2_medkit_gateway/src/bulk_data_store.cpp +++ b/src/ros2_medkit_gateway/src/bulk_data_store.cpp @@ -14,7 +14,6 @@ #include "ros2_medkit_gateway/bulk_data_store.hpp" -#include #include #include #include @@ -75,6 +74,8 @@ std::string BulkDataStore::generate_id(const std::string & category) { auto ns = std::chrono::duration_cast(now.time_since_epoch()).count(); // Generate 8 hex chars from random + // gateway_lib.a is compiled with POSITION_INDEPENDENT_CODE (fPIC), so the compiler + // emits global-dynamic TLS which works in both executables and shared objects. static thread_local std::mt19937 gen(std::random_device{}()); std::uniform_int_distribution dist(0, 0xFFFFFFFF); uint32_t rand_val = dist(gen); diff --git a/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp b/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp index f54d3809..213d1b10 100644 --- a/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp +++ b/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp @@ -15,6 +15,11 @@ #include "ros2_medkit_gateway/discovery/discovery_manager.hpp" #include "ros2_medkit_gateway/discovery/hybrid_discovery.hpp" +#include "ros2_medkit_gateway/discovery/layers/manifest_layer.hpp" +#include "ros2_medkit_gateway/discovery/layers/plugin_layer.hpp" +#include "ros2_medkit_gateway/discovery/layers/runtime_layer.hpp" +#include "ros2_medkit_gateway/discovery/manifest/runtime_linker.hpp" +#include "ros2_medkit_gateway/discovery/merge_pipeline.hpp" namespace ros2_medkit_gateway { @@ -34,17 +39,32 @@ bool DiscoveryManager::initialize(const DiscoveryConfig & config) { if (config.mode == DiscoveryMode::MANIFEST_ONLY || config.mode == DiscoveryMode::HYBRID) { manifest_manager_ = std::make_unique(node_); - if (!config.manifest_path.empty()) { - if (!manifest_manager_->load_manifest(config.manifest_path, config.manifest_strict_validation)) { - if (config.mode == DiscoveryMode::MANIFEST_ONLY) { - RCLCPP_ERROR(node_->get_logger(), "Manifest load failed and mode is manifest_only. Cannot proceed."); - return false; - } - RCLCPP_WARN(node_->get_logger(), "Manifest load failed. Falling back to runtime-only discovery."); + if (config.manifest_path.empty()) { + if (config.mode == DiscoveryMode::HYBRID) { + RCLCPP_WARN(node_->get_logger(), + "No manifest_path set for hybrid mode. Falling back to runtime_only. " + "This fallback is deprecated and will be removed in a future release."); config_.mode = DiscoveryMode::RUNTIME_ONLY; + create_strategy(); + return true; } - } else if (config.mode == DiscoveryMode::MANIFEST_ONLY) { - RCLCPP_ERROR(node_->get_logger(), "Manifest path required for manifest_only mode."); + RCLCPP_ERROR(node_->get_logger(), "Manifest path required for %s mode. Set discovery.manifest_path.", + discovery_mode_to_string(config.mode).c_str()); + return false; + } + + if (!manifest_manager_->load_manifest(config.manifest_path, config.manifest_strict_validation)) { + if (config.mode == DiscoveryMode::HYBRID) { + RCLCPP_WARN(node_->get_logger(), + "Manifest load failed in hybrid mode. Falling back to runtime_only. " + "This fallback is deprecated and will be removed in a future release."); + config_.mode = DiscoveryMode::RUNTIME_ONLY; + manifest_manager_.reset(); + create_strategy(); + return true; + } + RCLCPP_ERROR(node_->get_logger(), "Manifest load failed in %s mode. Cannot proceed.", + discovery_mode_to_string(config.mode).c_str()); return false; } } @@ -53,6 +73,26 @@ bool DiscoveryManager::initialize(const DiscoveryConfig & config) { return true; } +template +void DiscoveryManager::apply_layer_policy_overrides(const std::string & layer_name, LayerT & layer) { + auto it = config_.merge_pipeline.layer_policies.find(layer_name); + if (it == config_.merge_pipeline.layer_policies.end()) { + return; + } + for (const auto & [fg_str, policy_str] : it->second) { + auto fg = discovery::field_group_from_string(fg_str); + auto policy = discovery::merge_policy_from_string(policy_str); + if (fg && policy) { + layer.set_policy(*fg, *policy); + RCLCPP_INFO(node_->get_logger(), "Layer '%s': override %s = %s", layer_name.c_str(), fg_str.c_str(), + policy_str.c_str()); + } else { + RCLCPP_WARN(node_->get_logger(), "Layer '%s': ignoring invalid override %s = %s", layer_name.c_str(), + fg_str.c_str(), policy_str.c_str()); + } + } +} + void DiscoveryManager::create_strategy() { // Configure runtime strategy with runtime options discovery::RuntimeDiscoveryStrategy::RuntimeConfig runtime_config; @@ -75,12 +115,29 @@ void DiscoveryManager::create_strategy() { RCLCPP_INFO(node_->get_logger(), "Discovery mode: manifest_only"); break; - case DiscoveryMode::HYBRID: - hybrid_strategy_ = - std::make_unique(node_, manifest_manager_.get(), runtime_strategy_.get()); + case DiscoveryMode::HYBRID: { + discovery::MergePipeline pipeline(node_->get_logger()); + + auto manifest_layer = std::make_unique(manifest_manager_.get()); + // Apply per-layer policy overrides for manifest + apply_layer_policy_overrides("manifest", *manifest_layer); + pipeline.add_layer(std::move(manifest_layer)); + + auto runtime_layer = std::make_unique(runtime_strategy_.get()); + runtime_layer->set_gap_fill_config(config_.merge_pipeline.gap_fill); + // Apply per-layer policy overrides for runtime + apply_layer_policy_overrides("runtime", *runtime_layer); + pipeline.add_layer(std::move(runtime_layer)); + + // Set up RuntimeLinker for post-merge app-to-node binding + auto manifest_config = manifest_manager_ ? manifest_manager_->get_config() : discovery::ManifestConfig{}; + pipeline.set_linker(std::make_unique(node_), manifest_config); + + hybrid_strategy_ = std::make_unique(node_, std::move(pipeline)); active_strategy_ = hybrid_strategy_.get(); - RCLCPP_INFO(node_->get_logger(), "Discovery mode: hybrid"); + RCLCPP_INFO(node_->get_logger(), "Discovery mode: hybrid (merge pipeline)"); break; + } default: active_strategy_ = runtime_strategy_.get(); @@ -119,10 +176,11 @@ std::vector DiscoveryManager::discover_functions() { } std::optional DiscoveryManager::get_area(const std::string & id) { - if (manifest_manager_ && manifest_manager_->is_manifest_active()) { + // In MANIFEST_ONLY mode, use direct manifest lookup (O(1)) + if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_area(id); } - // Fallback to runtime lookup + // For HYBRID and RUNTIME modes, scan the strategy's output (cached for HYBRID) auto areas = discover_areas(); for (const auto & a : areas) { if (a.id == id) { @@ -133,7 +191,7 @@ std::optional DiscoveryManager::get_area(const std::string & id) { } std::optional DiscoveryManager::get_component(const std::string & id) { - if (manifest_manager_ && manifest_manager_->is_manifest_active()) { + if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_component(id); } auto components = discover_components(); @@ -146,10 +204,9 @@ std::optional DiscoveryManager::get_component(const std::string & id) } std::optional DiscoveryManager::get_app(const std::string & id) { - if (manifest_manager_ && manifest_manager_->is_manifest_active()) { + if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_app(id); } - // Check runtime apps auto apps = discover_apps(); for (const auto & app : apps) { if (app.id == id) { @@ -160,34 +217,53 @@ std::optional DiscoveryManager::get_app(const std::string & id) { } std::optional DiscoveryManager::get_function(const std::string & id) { - if (manifest_manager_ && manifest_manager_->is_manifest_active()) { + if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_function(id); } - return std::nullopt; // No functions in runtime-only mode + auto functions = discover_functions(); + for (const auto & f : functions) { + if (f.id == id) { + return f; + } + } + return std::nullopt; } std::vector DiscoveryManager::get_subareas(const std::string & area_id) { - if (manifest_manager_ && manifest_manager_->is_manifest_active()) { + if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_subareas(area_id); } - return {}; // No subareas in runtime mode + // HYBRID: filter from pipeline-merged output; RUNTIME: no subareas + std::vector result; + for (const auto & a : discover_areas()) { + if (a.parent_area_id == area_id) { + result.push_back(a); + } + } + return result; } std::vector DiscoveryManager::get_subcomponents(const std::string & component_id) { - if (manifest_manager_ && manifest_manager_->is_manifest_active()) { + if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_subcomponents(component_id); } - return {}; // No subcomponents in runtime mode + // HYBRID: filter from pipeline-merged output; RUNTIME: no subcomponents + std::vector result; + for (const auto & c : discover_components()) { + if (c.parent_component_id == component_id) { + result.push_back(c); + } + } + return result; } std::vector DiscoveryManager::get_components_for_area(const std::string & area_id) { - if (manifest_manager_ && manifest_manager_->is_manifest_active()) { + if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_components_for_area(area_id); } - // Fallback: filter runtime components by area + // HYBRID: filter from pipeline-merged output; RUNTIME: filter by area std::vector result; - auto all = discover_components(); - for (const auto & c : all) { + for (const auto & c : discover_components()) { if (c.area == area_id) { result.push_back(c); } @@ -196,13 +272,12 @@ std::vector DiscoveryManager::get_components_for_area(const std::stri } std::vector DiscoveryManager::get_apps_for_component(const std::string & component_id) { - if (manifest_manager_ && manifest_manager_->is_manifest_active()) { + if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_apps_for_component(component_id); } - // Filter runtime apps by component_id + // HYBRID: filter from pipeline-merged output; RUNTIME: filter by component std::vector result; - auto apps = discover_apps(); - for (const auto & app : apps) { + for (const auto & app : discover_apps()) { if (app.component_id == component_id) { result.push_back(app); } @@ -250,7 +325,7 @@ void DiscoveryManager::set_type_introspection(TypeIntrospection * introspection) void DiscoveryManager::refresh_topic_map() { runtime_strategy_->refresh_topic_map(); if (hybrid_strategy_) { - hybrid_strategy_->refresh_linking(); + hybrid_strategy_->refresh(); } } @@ -258,20 +333,23 @@ bool DiscoveryManager::is_topic_map_ready() const { return runtime_strategy_->is_topic_map_ready(); } -discovery::ManifestManager * DiscoveryManager::get_manifest_manager() { - return manifest_manager_.get(); +void DiscoveryManager::add_plugin_layer(const std::string & plugin_name, IntrospectionProvider * provider) { + if (!hybrid_strategy_) { + RCLCPP_WARN(node_->get_logger(), "Cannot add plugin layer '%s': not in hybrid mode", plugin_name.c_str()); + return; + } + hybrid_strategy_->add_layer(std::make_unique(plugin_name, provider)); + RCLCPP_INFO(node_->get_logger(), "Added plugin layer '%s' to merge pipeline", plugin_name.c_str()); } -bool DiscoveryManager::reload_manifest() { - if (!manifest_manager_) { - RCLCPP_WARN(node_->get_logger(), "No manifest manager to reload"); - return false; - } - bool result = manifest_manager_->reload_manifest(); - if (result && hybrid_strategy_) { - hybrid_strategy_->refresh_linking(); +void DiscoveryManager::refresh_pipeline() { + if (hybrid_strategy_) { + hybrid_strategy_->refresh(); } - return result; +} + +discovery::ManifestManager * DiscoveryManager::get_manifest_manager() { + return manifest_manager_.get(); } std::string DiscoveryManager::get_strategy_name() const { @@ -281,4 +359,18 @@ std::string DiscoveryManager::get_strategy_name() const { return "unknown"; } +std::optional DiscoveryManager::get_merge_report() const { + if (hybrid_strategy_) { + return hybrid_strategy_->get_merge_report(); + } + return std::nullopt; +} + +std::optional DiscoveryManager::get_linking_result() const { + if (hybrid_strategy_) { + return hybrid_strategy_->get_linking_result(); + } + return std::nullopt; +} + } // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp b/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp index a4988e2e..4a4ee1cc 100644 --- a/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp +++ b/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp @@ -14,134 +14,64 @@ #include "ros2_medkit_gateway/discovery/hybrid_discovery.hpp" +#include + namespace ros2_medkit_gateway { namespace discovery { -HybridDiscoveryStrategy::HybridDiscoveryStrategy(rclcpp::Node * node, ManifestManager * manifest_manager, - RuntimeDiscoveryStrategy * runtime_strategy) - : node_(node), manifest_manager_(manifest_manager), runtime_strategy_(runtime_strategy), linker_(node) { - // Perform initial linking if manifest is loaded - if (manifest_manager_ && manifest_manager_->is_manifest_active()) { - perform_linking(); - } +HybridDiscoveryStrategy::HybridDiscoveryStrategy(rclcpp::Node * node, MergePipeline pipeline) + : node_(node), pipeline_(std::move(pipeline)) { + // Initial pipeline execution + refresh(); } std::vector HybridDiscoveryStrategy::discover_areas() { std::lock_guard lock(mutex_); - - if (!manifest_manager_ || !manifest_manager_->is_manifest_active()) { - // Fallback to runtime if no manifest - return runtime_strategy_->discover_areas(); - } - - return manifest_manager_->get_areas(); + return cached_result_.areas; } std::vector HybridDiscoveryStrategy::discover_components() { std::lock_guard lock(mutex_); - - if (!manifest_manager_ || !manifest_manager_->is_manifest_active()) { - return runtime_strategy_->discover_components(); - } - - // Get manifest components - std::vector result = manifest_manager_->get_components(); - - // Get runtime components for enrichment - auto runtime_components = runtime_strategy_->discover_components(); - - // Build a map of runtime components by FQN for quick lookup - std::unordered_map runtime_map; - for (const auto & comp : runtime_components) { - runtime_map[comp.fqn] = ∁ - } - - // Enrich manifest components with runtime data if they match - for (auto & comp : result) { - auto it = runtime_map.find(comp.fqn); - if (it != runtime_map.end()) { - // Copy runtime data - comp.topics = it->second->topics; - comp.services = it->second->services; - comp.actions = it->second->actions; - } - } - - // Handle orphan nodes based on policy - auto config = manifest_manager_->get_config(); - if (config.unmanifested_nodes == ManifestConfig::UnmanifestedNodePolicy::INCLUDE_AS_ORPHAN) { - // Add orphan nodes as components - std::set manifest_fqns; - for (const auto & comp : result) { - manifest_fqns.insert(comp.fqn); - } - - for (const auto & runtime_comp : runtime_components) { - if (manifest_fqns.find(runtime_comp.fqn) == manifest_fqns.end()) { - Component orphan = runtime_comp; - orphan.source = "orphan"; - result.push_back(orphan); - } - } - } - - return result; + return cached_result_.components; } std::vector HybridDiscoveryStrategy::discover_apps() { std::lock_guard lock(mutex_); - - if (!manifest_manager_ || !manifest_manager_->is_manifest_active()) { - // No apps in runtime-only mode - return {}; - } - - // Return linked apps from last linking result - return linking_result_.linked_apps; + return cached_result_.apps; } std::vector HybridDiscoveryStrategy::discover_functions() { std::lock_guard lock(mutex_); + return cached_result_.functions; +} - if (!manifest_manager_ || !manifest_manager_->is_manifest_active()) { - // No functions in runtime-only mode - return {}; +void HybridDiscoveryStrategy::refresh() { + // Hold mutex for the full execute()+swap to prevent data races with add_layer() + std::lock_guard lock(mutex_); + cached_result_ = pipeline_.execute(); + if (node_) { + RCLCPP_INFO(node_->get_logger(), "Hybrid discovery refreshed: %zu entities", cached_result_.report.total_entities); } - - return manifest_manager_->get_functions(); } -void HybridDiscoveryStrategy::refresh_linking() { +MergeReport HybridDiscoveryStrategy::get_merge_report() const { std::lock_guard lock(mutex_); - perform_linking(); + return cached_result_.report; } -void HybridDiscoveryStrategy::perform_linking() { - if (!manifest_manager_ || !manifest_manager_->is_manifest_active()) { - log_info("Cannot perform linking: no active manifest"); - return; - } - - // Get manifest apps - auto apps = manifest_manager_->get_apps(); - - // 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(); - - // Perform linking - linking_result_ = linker_.link(apps, runtime_components, config); +LinkingResult HybridDiscoveryStrategy::get_linking_result() const { + std::lock_guard lock(mutex_); + return cached_result_.linking_result; +} - log_info("Hybrid linking complete: " + linking_result_.summary()); +std::vector HybridDiscoveryStrategy::get_orphan_nodes() const { + std::lock_guard lock(mutex_); + return cached_result_.linking_result.orphan_nodes; } -void HybridDiscoveryStrategy::log_info(const std::string & msg) const { - if (node_) { - RCLCPP_INFO(node_->get_logger(), "%s", msg.c_str()); - } +void HybridDiscoveryStrategy::add_layer(std::unique_ptr layer) { + std::lock_guard lock(mutex_); + pipeline_.add_layer(std::move(layer)); } } // namespace discovery diff --git a/src/ros2_medkit_gateway/src/discovery/layers/manifest_layer.cpp b/src/ros2_medkit_gateway/src/discovery/layers/manifest_layer.cpp new file mode 100644 index 00000000..dbbd43c2 --- /dev/null +++ b/src/ros2_medkit_gateway/src/discovery/layers/manifest_layer.cpp @@ -0,0 +1,53 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_medkit_gateway/discovery/layers/manifest_layer.hpp" + +namespace ros2_medkit_gateway { +namespace discovery { + +ManifestLayer::ManifestLayer(ManifestManager * manifest_manager) : manifest_manager_(manifest_manager) { + policies_ = {{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}, + {FieldGroup::HIERARCHY, MergePolicy::AUTHORITATIVE}, + {FieldGroup::LIVE_DATA, MergePolicy::ENRICHMENT}, + {FieldGroup::STATUS, MergePolicy::FALLBACK}, + {FieldGroup::METADATA, MergePolicy::AUTHORITATIVE}}; +} + +LayerOutput ManifestLayer::discover() { + LayerOutput output; + if (!manifest_manager_ || !manifest_manager_->is_manifest_active()) { + return output; + } + output.areas = manifest_manager_->get_areas(); + output.components = manifest_manager_->get_components(); + output.apps = manifest_manager_->get_apps(); + output.functions = manifest_manager_->get_functions(); + return output; +} + +MergePolicy ManifestLayer::policy_for(FieldGroup group) const { + auto it = policies_.find(group); + if (it != policies_.end()) { + return it->second; + } + return MergePolicy::ENRICHMENT; +} + +void ManifestLayer::set_policy(FieldGroup group, MergePolicy policy) { + policies_[group] = policy; +} + +} // namespace discovery +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/discovery/layers/plugin_layer.cpp b/src/ros2_medkit_gateway/src/discovery/layers/plugin_layer.cpp new file mode 100644 index 00000000..35d07b9e --- /dev/null +++ b/src/ros2_medkit_gateway/src/discovery/layers/plugin_layer.cpp @@ -0,0 +1,100 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_medkit_gateway/discovery/layers/plugin_layer.hpp" + +#include +#include +#include + +namespace ros2_medkit_gateway { +namespace discovery { + +namespace { + +bool is_valid_entity_id(const std::string & id) { + if (id.empty() || id.size() > 256) { + return false; + } + return std::all_of(id.begin(), id.end(), [](char c) { + return std::isalnum(static_cast(c)) || c == '_' || c == '-'; + }); +} + +template +void validate_entities(std::vector & entities, const std::string & layer_name, const rclcpp::Logger & logger) { + auto it = std::remove_if(entities.begin(), entities.end(), [&](const T & e) { + if (!is_valid_entity_id(e.id)) { + RCLCPP_WARN(logger, "Plugin '%s': dropping entity with invalid ID '%s'", layer_name.c_str(), e.id.c_str()); + return true; + } + return false; + }); + entities.erase(it, entities.end()); +} + +} // namespace + +PluginLayer::PluginLayer(std::string plugin_name, IntrospectionProvider * provider) + : name_(std::move(plugin_name)), provider_(provider), logger_(rclcpp::get_logger("plugin_layer." + name_)) { + policies_ = {{FieldGroup::IDENTITY, MergePolicy::ENRICHMENT}, + {FieldGroup::HIERARCHY, MergePolicy::ENRICHMENT}, + {FieldGroup::LIVE_DATA, MergePolicy::ENRICHMENT}, + {FieldGroup::STATUS, MergePolicy::ENRICHMENT}, + {FieldGroup::METADATA, MergePolicy::ENRICHMENT}}; +} + +LayerOutput PluginLayer::discover() { + LayerOutput output; + if (!provider_) { + return output; + } + + auto result = provider_->introspect(discovery_context_); + + // Map new_entities to LayerOutput (no functions - plugins cannot create functions) + output.areas = std::move(result.new_entities.areas); + output.components = std::move(result.new_entities.components); + output.apps = std::move(result.new_entities.apps); + + // Validate entity IDs from plugin + validate_entities(output.areas, name_, logger_); + validate_entities(output.components, name_, logger_); + validate_entities(output.apps, name_, logger_); + + // Store metadata and pass through LayerOutput for pipeline consumption + last_metadata_ = result.metadata; + output.entity_metadata = result.metadata; + + return output; +} + +MergePolicy PluginLayer::policy_for(FieldGroup group) const { + auto it = policies_.find(group); + if (it != policies_.end()) { + return it->second; + } + return MergePolicy::ENRICHMENT; +} + +void PluginLayer::set_discovery_context(const IntrospectionInput & context) { + discovery_context_ = context; +} + +void PluginLayer::set_policy(FieldGroup group, MergePolicy policy) { + policies_[group] = policy; +} + +} // namespace discovery +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/discovery/layers/runtime_layer.cpp b/src/ros2_medkit_gateway/src/discovery/layers/runtime_layer.cpp new file mode 100644 index 00000000..ff36ae55 --- /dev/null +++ b/src/ros2_medkit_gateway/src/discovery/layers/runtime_layer.cpp @@ -0,0 +1,161 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_medkit_gateway/discovery/layers/runtime_layer.hpp" + +#include +#include +#include + +namespace ros2_medkit_gateway { +namespace discovery { + +namespace { + +// Check if a namespace is allowed by the gap-fill config +bool is_namespace_allowed(const std::string & ns, const GapFillConfig & config) { + // If whitelist is non-empty, namespace must match + if (!config.namespace_whitelist.empty()) { + bool found = + std::any_of(config.namespace_whitelist.begin(), config.namespace_whitelist.end(), [&ns](const std::string & w) { + return ns == w || ns.find(w + "/") == 0; + }); + if (!found) { + return false; + } + } + // Check blacklist + for (const auto & b : config.namespace_blacklist) { + if (ns == b || ns.find(b + "/") == 0) { + return false; + } + } + return true; +} + +// Filter entities with namespace_path by gap-fill config, returns count of removed entities +template +size_t filter_by_namespace(std::vector & entities, const GapFillConfig & config) { + size_t before = entities.size(); + entities.erase(std::remove_if(entities.begin(), entities.end(), + [&config](const Entity & e) { + return !is_namespace_allowed(e.namespace_path, config); + }), + entities.end()); + return before - entities.size(); +} + +// Extract namespace from a fully-qualified node name (e.g. "/ns/sub/node" -> "/ns/sub") +std::string namespace_from_fqn(const std::string & fqn) { + auto pos = fqn.rfind('/'); + if (pos == std::string::npos || pos == 0) { + return "/"; + } + return fqn.substr(0, pos); +} + +// Filter apps by namespace derived from bound_fqn +size_t filter_apps_by_namespace(std::vector & apps, const GapFillConfig & config) { + size_t before = apps.size(); + apps.erase(std::remove_if(apps.begin(), apps.end(), + [&config](const App & a) { + if (!a.bound_fqn.has_value()) { + return false; // Keep unbound apps + } + return !is_namespace_allowed(namespace_from_fqn(*a.bound_fqn), config); + }), + apps.end()); + return before - apps.size(); +} + +} // namespace + +RuntimeLayer::RuntimeLayer(RuntimeDiscoveryStrategy * runtime_strategy) : runtime_strategy_(runtime_strategy) { + policies_ = {{FieldGroup::IDENTITY, MergePolicy::FALLBACK}, + {FieldGroup::HIERARCHY, MergePolicy::FALLBACK}, + {FieldGroup::LIVE_DATA, MergePolicy::AUTHORITATIVE}, + {FieldGroup::STATUS, MergePolicy::AUTHORITATIVE}, + {FieldGroup::METADATA, MergePolicy::ENRICHMENT}}; +} + +LayerOutput RuntimeLayer::discover() { + LayerOutput output; + last_filtered_count_ = 0; + if (!runtime_strategy_) { + return output; + } + + if (gap_fill_config_.allow_heuristic_areas) { + output.areas = runtime_strategy_->discover_areas(); + last_filtered_count_ += filter_by_namespace(output.areas, gap_fill_config_); + } + + // Discover apps once - used by both components (synthetic grouping) and apps output + auto apps = runtime_strategy_->discover_apps(); + + if (gap_fill_config_.allow_heuristic_components) { + output.components = runtime_strategy_->discover_components(apps); + + // Topic components are discovered separately and must be included + auto topic_components = runtime_strategy_->discover_topic_components(); + output.components.insert(output.components.end(), std::make_move_iterator(topic_components.begin()), + std::make_move_iterator(topic_components.end())); + + last_filtered_count_ += filter_by_namespace(output.components, gap_fill_config_); + } + + if (gap_fill_config_.allow_heuristic_apps) { + output.apps = std::move(apps); + last_filtered_count_ += filter_apps_by_namespace(output.apps, gap_fill_config_); + } + + if (gap_fill_config_.allow_heuristic_functions) { + output.functions = runtime_strategy_->discover_functions(); + } + + return output; +} + +MergePolicy RuntimeLayer::policy_for(FieldGroup group) const { + auto it = policies_.find(group); + if (it != policies_.end()) { + return it->second; + } + return MergePolicy::ENRICHMENT; +} + +void RuntimeLayer::set_policy(FieldGroup group, MergePolicy policy) { + policies_[group] = policy; +} + +void RuntimeLayer::set_gap_fill_config(GapFillConfig config) { + gap_fill_config_ = std::move(config); +} + +std::vector RuntimeLayer::discover_services() { + if (!runtime_strategy_) { + return {}; + } + return runtime_strategy_->discover_services(); +} + +std::vector RuntimeLayer::discover_actions() { + if (!runtime_strategy_) { + return {}; + } + return runtime_strategy_->discover_actions(); +} + +} // namespace discovery +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/discovery/manifest/manifest_parser.cpp b/src/ros2_medkit_gateway/src/discovery/manifest/manifest_parser.cpp index 857f9303..492af515 100644 --- a/src/ros2_medkit_gateway/src/discovery/manifest/manifest_parser.cpp +++ b/src/ros2_medkit_gateway/src/discovery/manifest/manifest_parser.cpp @@ -136,6 +136,7 @@ void ManifestParser::parse_area_recursive(const YAML::Node & node, const std::st area.tags = get_string_vector(node, "tags"); // Set parent from recursive call, or from explicit parent_area field area.parent_area_id = parent_id.empty() ? get_string(node, "parent_area") : parent_id; + area.source = "manifest"; areas.push_back(area); diff --git a/src/ros2_medkit_gateway/src/discovery/manifest/runtime_linker.cpp b/src/ros2_medkit_gateway/src/discovery/manifest/runtime_linker.cpp index 4f30360a..e63b38ef 100644 --- a/src/ros2_medkit_gateway/src/discovery/manifest/runtime_linker.cpp +++ b/src/ros2_medkit_gateway/src/discovery/manifest/runtime_linker.cpp @@ -19,24 +19,64 @@ namespace ros2_medkit_gateway { namespace discovery { +namespace { + +/// Path-segment-boundary namespace match: "/nav" matches "/nav" and "/nav/sub" but NOT "/navigation" +bool namespace_matches(const std::string & actual_ns, const std::string & expected_ns) { + if (actual_ns == expected_ns) { + return true; + } + if (actual_ns.size() > expected_ns.size() && actual_ns.compare(0, expected_ns.size(), expected_ns) == 0 && + actual_ns[expected_ns.size()] == '/') { + return true; + } + return false; +} + +/// Extract the last path segment from a FQN (e.g., "/ns/node_name" -> "node_name") +std::string extract_node_name(const std::string & fqn) { + auto pos = fqn.rfind('/'); + if (pos == std::string::npos) { + return fqn; + } + return fqn.substr(pos + 1); +} + +/// Extract namespace from a FQN (e.g., "/ns/sub/node" -> "/ns/sub", "/node" -> "/") +std::string extract_namespace(const std::string & fqn) { + auto pos = fqn.rfind('/'); + if (pos == std::string::npos || pos == 0) { + return "/"; + } + return fqn.substr(0, pos); +} + +/// Path-segment-boundary topic match: "/state" matches "/state/x" but NOT "/statement/x" +bool topic_path_matches(const std::string & topic, const std::string & topic_namespace) { + if (topic == topic_namespace) { + return true; + } + if (topic.size() > topic_namespace.size() && topic.compare(0, topic_namespace.size(), topic_namespace) == 0 && + topic[topic_namespace.size()] == '/') { + return true; + } + return false; +} + +} // namespace + RuntimeLinker::RuntimeLinker(rclcpp::Node * node) : node_(node) { } -LinkingResult RuntimeLinker::link(const std::vector & apps, const std::vector & runtime_components, +LinkingResult RuntimeLinker::link(const std::vector & manifest_apps, const std::vector & runtime_apps, const ManifestConfig & config) { LinkingResult result; - // Build a map of node FQN -> Component for quick lookup - std::unordered_map fqn_to_component; - for (const auto & comp : runtime_components) { - fqn_to_component[comp.fqn] = ∁ - } - // Track which runtime nodes have been matched std::set matched_nodes; // Process each manifest app - for (const auto & manifest_app : apps) { + for (const auto & manifest_app : manifest_apps) { App linked_app = manifest_app; // Copy linked_app.is_online = false; @@ -56,41 +96,73 @@ LinkingResult RuntimeLinker::link(const std::vector & apps, const std::vect const auto & binding = manifest_app.ros_binding.value(); bool found = false; - // Try to find matching runtime node - for (const auto & comp : runtime_components) { - // Extract node name and namespace from component - std::string node_name = comp.id; - std::string node_ns = comp.namespace_path; + // Collect candidates, excluding already-bound nodes + std::vector candidates; - if (matches_binding(binding, comp.fqn, node_name, node_ns)) { - // Match found! - linked_app.bound_fqn = comp.fqn; - linked_app.is_online = true; - enrich_app(linked_app, comp); + for (const auto & rt_app : runtime_apps) { + if (!rt_app.bound_fqn.has_value()) { + continue; + } + const auto & fqn = rt_app.bound_fqn.value(); + if (matched_nodes.count(fqn)) { + continue; // Node already bound to another app + } + auto node_name = extract_node_name(fqn); + auto node_ns = extract_namespace(fqn); + if (matches_binding(binding, fqn, node_name, node_ns)) { + candidates.push_back(&rt_app); + } else if (!binding.topic_namespace.empty() && matches_topic_namespace(binding.topic_namespace, rt_app)) { + candidates.push_back(&rt_app); + } + } - result.app_to_node[manifest_app.id] = comp.fqn; - result.node_to_app[comp.fqn] = manifest_app.id; - matched_nodes.insert(comp.fqn); - found = true; + // Check if any candidates were excluded due to binding conflicts + if (candidates.empty()) { + // Check if there WOULD have been a match without exclusivity + for (const auto & rt_app : runtime_apps) { + if (!rt_app.bound_fqn.has_value()) { + continue; + } + const auto & fqn = rt_app.bound_fqn.value(); + if (matched_nodes.count(fqn)) { + auto node_name = extract_node_name(fqn); + auto node_ns = extract_namespace(fqn); + if (matches_binding(binding, fqn, node_name, node_ns) || + (!binding.topic_namespace.empty() && matches_topic_namespace(binding.topic_namespace, rt_app))) { + result.binding_conflicts++; + result.warnings.push_back("App '" + manifest_app.id + "' cannot bind to '" + fqn + + "' - already bound to app '" + result.node_to_app.at(fqn) + "'"); + log_warn(result.warnings.back()); + break; + } + } + } + } - log_debug("Linked app '" + manifest_app.id + "' to node '" + comp.fqn + "'"); - break; + // Sort candidates by FQN for deterministic selection + std::sort(candidates.begin(), candidates.end(), [](const App * a, const App * b) { + return a->bound_fqn.value() < b->bound_fqn.value(); + }); + + if (!candidates.empty()) { + if (candidates.size() > 1 && binding.namespace_pattern == "*") { + result.wildcard_multi_match++; + log_warn("App '" + manifest_app.id + "' wildcard matched " + std::to_string(candidates.size()) + + " nodes, selecting '" + candidates[0]->bound_fqn.value() + "'"); } - // Try topic namespace matching - if (!binding.topic_namespace.empty() && matches_topic_namespace(binding.topic_namespace, comp)) { - linked_app.bound_fqn = comp.fqn; - linked_app.is_online = true; - enrich_app(linked_app, comp); + const auto & match = *candidates[0]; + const auto & match_fqn = match.bound_fqn.value(); + linked_app.bound_fqn = match_fqn; + linked_app.is_online = true; + enrich_app(linked_app, match); - result.app_to_node[manifest_app.id] = comp.fqn; - result.node_to_app[comp.fqn] = manifest_app.id; - matched_nodes.insert(comp.fqn); - found = true; + result.app_to_node[manifest_app.id] = match_fqn; + result.node_to_app[match_fqn] = manifest_app.id; + matched_nodes.insert(match_fqn); + found = true; - log_debug("Linked app '" + manifest_app.id + "' to node '" + comp.fqn + "' (topic namespace)"); - break; - } + log_debug("Linked app '" + manifest_app.id + "' to node '" + match_fqn + "'"); } if (!found) { @@ -101,10 +173,10 @@ LinkingResult RuntimeLinker::link(const std::vector & apps, const std::vect result.linked_apps.push_back(linked_app); } - // Find orphan nodes (runtime nodes not matching any manifest app) - for (const auto & comp : runtime_components) { - if (matched_nodes.find(comp.fqn) == matched_nodes.end()) { - result.orphan_nodes.push_back(comp.fqn); + // Find orphan nodes (runtime apps not matching any manifest app) + for (const auto & rt_app : runtime_apps) { + if (rt_app.bound_fqn.has_value() && matched_nodes.find(rt_app.bound_fqn.value()) == matched_nodes.end()) { + result.orphan_nodes.push_back(rt_app.bound_fqn.value()); } } @@ -139,17 +211,14 @@ LinkingResult RuntimeLinker::link(const std::vector & apps, const std::vect } bool RuntimeLinker::matches_binding(const App::RosBinding & binding, const std::string & node_fqn, - const std::string & node_name, const std::string & node_namespace) const { - // Check node name match + const std::string & /*node_name*/, const std::string & node_namespace) const { + // Check node name match using last FQN segment (exact match only) if (binding.node_name.empty()) { return false; } - // Node name can be simple or with subpath (e.g., "local_costmap/local_costmap") - // Check if binding.node_name matches node_name or is contained in fqn - bool name_matches = (node_name == binding.node_name) || (node_fqn.find("/" + binding.node_name) != std::string::npos); - - if (!name_matches) { + std::string actual_name = extract_node_name(node_fqn); + if (actual_name != binding.node_name) { return false; } @@ -159,45 +228,33 @@ bool RuntimeLinker::matches_binding(const App::RosBinding & binding, const std:: return true; } - // Exact namespace match - std::string expected_ns = binding.namespace_pattern; - if (expected_ns.empty()) { - expected_ns = "/"; - } - // Normalize namespaces for comparison - std::string actual_ns = node_namespace; - if (actual_ns.empty()) { - actual_ns = "/"; - } + std::string expected_ns = binding.namespace_pattern.empty() ? "/" : binding.namespace_pattern; + std::string actual_ns = node_namespace.empty() ? "/" : node_namespace; - return actual_ns == expected_ns || actual_ns.find(expected_ns) == 0; // Prefix match + // Path-segment-boundary match + return namespace_matches(actual_ns, expected_ns); } -bool RuntimeLinker::matches_topic_namespace(const std::string & topic_namespace, const Component & component) const { - // Check if any topic starts with the given namespace - for (const auto & topic : component.topics.publishes) { - if (topic.find(topic_namespace) == 0) { +bool RuntimeLinker::matches_topic_namespace(const std::string & topic_ns, const App & runtime_app) const { + // Check if any topic matches with path-segment boundary + for (const auto & topic : runtime_app.topics.publishes) { + if (topic_path_matches(topic, topic_ns)) { return true; } } - for (const auto & topic : component.topics.subscribes) { - if (topic.find(topic_namespace) == 0) { + for (const auto & topic : runtime_app.topics.subscribes) { + if (topic_path_matches(topic, topic_ns)) { return true; } } return false; } -void RuntimeLinker::enrich_app(App & app, const Component & component) { - // Copy topics - app.topics = component.topics; - - // Copy services - app.services = component.services; - - // Copy actions - app.actions = component.actions; +void RuntimeLinker::enrich_app(App & app, const App & runtime_app) { + app.topics = runtime_app.topics; + app.services = runtime_app.services; + app.actions = runtime_app.actions; } bool RuntimeLinker::is_app_online(const std::string & app_id) const { diff --git a/src/ros2_medkit_gateway/src/discovery/merge_pipeline.cpp b/src/ros2_medkit_gateway/src/discovery/merge_pipeline.cpp new file mode 100644 index 00000000..674bd3fb --- /dev/null +++ b/src/ros2_medkit_gateway/src/discovery/merge_pipeline.cpp @@ -0,0 +1,513 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_medkit_gateway/discovery/merge_pipeline.hpp" + +#include "ros2_medkit_gateway/discovery/layers/runtime_layer.hpp" +#include "ros2_medkit_gateway/providers/introspection_provider.hpp" + +#include +#include +#include +#include + +namespace ros2_medkit_gateway { +namespace discovery { + +namespace { + +// Policy priority: AUTH=2, ENRICH=1, FALLBACK=0 +int policy_priority(MergePolicy p) { + switch (p) { + case MergePolicy::AUTHORITATIVE: + return 2; + case MergePolicy::ENRICHMENT: + return 1; + case MergePolicy::FALLBACK: + return 0; + } + return 0; +} + +enum class MergeWinner { TARGET, SOURCE, BOTH }; + +struct MergeResolution { + MergeWinner scalar; + MergeWinner collection; + bool is_conflict{false}; +}; + +MergeResolution resolve_policies(MergePolicy target_policy, MergePolicy source_policy) { + int tp = policy_priority(target_policy); + int sp = policy_priority(source_policy); + + if (tp > sp) { + return {MergeWinner::TARGET, MergeWinner::TARGET, false}; + } else if (sp > tp) { + return {MergeWinner::SOURCE, MergeWinner::SOURCE, false}; + } else { + // Same level + if (target_policy == MergePolicy::AUTHORITATIVE) { + return {MergeWinner::TARGET, MergeWinner::TARGET, true}; // AUTH vs AUTH -> conflict + } else if (target_policy == MergePolicy::ENRICHMENT) { + return {MergeWinner::BOTH, MergeWinner::BOTH, false}; // mutual enrichment + } else { + return {MergeWinner::BOTH, MergeWinner::BOTH, false}; // FALLBACK vs FALLBACK: fill gaps + } + } +} + +void merge_scalar(std::string & target, const std::string & source, MergeWinner winner) { + switch (winner) { + case MergeWinner::SOURCE: + if (!source.empty()) { + target = source; + } + break; + case MergeWinner::BOTH: + // First non-empty (target preferred since higher priority) + if (target.empty() && !source.empty()) { + target = source; + } + break; + case MergeWinner::TARGET: + break; + } +} + +void merge_bool(bool & target, bool source, MergeWinner winner) { + switch (winner) { + case MergeWinner::SOURCE: + target = source; + break; + case MergeWinner::BOTH: + // OR semantics: once true, stays true. This is intentional for status flags + // like is_online (any layer reporting online = online). For classification + // bools like external, an incorrect true from a lower layer is sticky - + // use AUTHORITATIVE policy on the correcting layer to override. + target = target || source; + break; + case MergeWinner::TARGET: + break; + } +} + +void merge_collection(std::vector & target, const std::vector & source, MergeWinner winner) { + switch (winner) { + case MergeWinner::SOURCE: + target = source; + break; + case MergeWinner::BOTH: { + std::unordered_set seen(target.begin(), target.end()); + for (const auto & s : source) { + if (seen.insert(s).second) { + target.push_back(s); + } + } + break; + } + case MergeWinner::TARGET: + break; + } +} + +template +void merge_by_key(std::vector & target, const std::vector & source, KeyFn key_fn, MergeWinner winner) { + switch (winner) { + case MergeWinner::SOURCE: + target = source; + break; + case MergeWinner::BOTH: { + std::unordered_set seen; + for (const auto & t : target) { + seen.insert(key_fn(t)); + } + for (const auto & s : source) { + if (seen.insert(key_fn(s)).second) { + target.push_back(s); + } + } + break; + } + case MergeWinner::TARGET: + break; + } +} + +template +void merge_optional(std::optional & target, const std::optional & source, MergeWinner winner) { + switch (winner) { + case MergeWinner::SOURCE: + if (source.has_value()) { + target = source; + } + break; + case MergeWinner::BOTH: + if (!target.has_value() && source.has_value()) { + target = source; + } + break; + case MergeWinner::TARGET: + break; + } +} + +void merge_topics(ComponentTopics & target, const ComponentTopics & source, MergeWinner winner) { + merge_collection(target.publishes, source.publishes, winner); + merge_collection(target.subscribes, source.subscribes, winner); +} + +// Per-entity-type field-group merge dispatch +template +void apply_field_group_merge(Entity & target, const Entity & source, FieldGroup group, const MergeResolution & res) { + if constexpr (std::is_same_v) { + switch (group) { + case FieldGroup::IDENTITY: + merge_scalar(target.name, source.name, res.scalar); + merge_scalar(target.translation_id, source.translation_id, res.scalar); + merge_scalar(target.description, source.description, res.scalar); + merge_collection(target.tags, source.tags, res.collection); + break; + case FieldGroup::HIERARCHY: + merge_scalar(target.namespace_path, source.namespace_path, res.scalar); + merge_scalar(target.parent_area_id, source.parent_area_id, res.scalar); + break; + case FieldGroup::METADATA: + merge_scalar(target.source, source.source, res.scalar); + break; + default: + break; + } + } else if constexpr (std::is_same_v) { + switch (group) { + case FieldGroup::IDENTITY: + merge_scalar(target.name, source.name, res.scalar); + merge_scalar(target.translation_id, source.translation_id, res.scalar); + merge_scalar(target.description, source.description, res.scalar); + merge_collection(target.tags, source.tags, res.collection); + break; + case FieldGroup::HIERARCHY: + merge_scalar(target.namespace_path, source.namespace_path, res.scalar); + merge_scalar(target.fqn, source.fqn, res.scalar); + merge_scalar(target.area, source.area, res.scalar); + merge_scalar(target.parent_component_id, source.parent_component_id, res.scalar); + merge_collection(target.depends_on, source.depends_on, res.collection); + break; + case FieldGroup::LIVE_DATA: + merge_topics(target.topics, source.topics, res.collection); + merge_by_key( + target.services, source.services, + [](const ServiceInfo & s) { + return s.full_path; + }, + res.collection); + merge_by_key( + target.actions, source.actions, + [](const ActionInfo & a) { + return a.full_path; + }, + res.collection); + break; + case FieldGroup::METADATA: + merge_scalar(target.source, source.source, res.scalar); + merge_scalar(target.variant, source.variant, res.scalar); + break; + default: + break; + } + } else if constexpr (std::is_same_v) { + switch (group) { + case FieldGroup::IDENTITY: + merge_scalar(target.name, source.name, res.scalar); + merge_scalar(target.translation_id, source.translation_id, res.scalar); + merge_scalar(target.description, source.description, res.scalar); + merge_collection(target.tags, source.tags, res.collection); + break; + case FieldGroup::HIERARCHY: + merge_scalar(target.component_id, source.component_id, res.scalar); + merge_collection(target.depends_on, source.depends_on, res.collection); + break; + case FieldGroup::LIVE_DATA: + merge_topics(target.topics, source.topics, res.collection); + merge_by_key( + target.services, source.services, + [](const ServiceInfo & s) { + return s.full_path; + }, + res.collection); + merge_by_key( + target.actions, source.actions, + [](const ActionInfo & a) { + return a.full_path; + }, + res.collection); + break; + case FieldGroup::STATUS: + merge_bool(target.is_online, source.is_online, res.scalar); + merge_optional(target.bound_fqn, source.bound_fqn, res.scalar); + merge_bool(target.external, source.external, res.scalar); + break; + case FieldGroup::METADATA: + merge_scalar(target.source, source.source, res.scalar); + merge_optional(target.ros_binding, source.ros_binding, res.scalar); + break; + } + } else if constexpr (std::is_same_v) { + switch (group) { + case FieldGroup::IDENTITY: + merge_scalar(target.name, source.name, res.scalar); + merge_scalar(target.translation_id, source.translation_id, res.scalar); + merge_scalar(target.description, source.description, res.scalar); + merge_collection(target.tags, source.tags, res.collection); + break; + case FieldGroup::HIERARCHY: + merge_collection(target.hosts, source.hosts, res.collection); + merge_collection(target.depends_on, source.depends_on, res.collection); + break; + case FieldGroup::METADATA: + merge_scalar(target.source, source.source, res.scalar); + break; + default: + break; + } + } +} + +constexpr FieldGroup ALL_FIELD_GROUPS[] = {FieldGroup::IDENTITY, FieldGroup::HIERARCHY, FieldGroup::LIVE_DATA, + FieldGroup::STATUS, FieldGroup::METADATA}; + +} // namespace + +MergePipeline::MergePipeline(rclcpp::Logger logger) : logger_(std::move(logger)) { +} + +void MergePipeline::add_layer(std::unique_ptr layer) { + layers_.push_back(std::move(layer)); +} + +void MergePipeline::set_linker(std::unique_ptr linker, const ManifestConfig & config) { + linker_ = std::move(linker); + manifest_config_ = config; +} + +template +std::vector MergePipeline::merge_entities(std::vector>> & layer_entities, + MergeReport & report) { + // Collect all entities by ID with their layer index + struct LayerEntity { + size_t layer_idx; + Entity entity; + }; + std::unordered_map> by_id; + std::vector insertion_order; + + for (auto & [layer_idx, entities] : layer_entities) { + for (auto & entity : entities) { + if (by_id.find(entity.id) == by_id.end()) { + insertion_order.push_back(entity.id); + } + auto id = entity.id; // copy id before move + by_id[id].push_back({layer_idx, std::move(entity)}); + } + } + + std::vector result; + result.reserve(insertion_order.size()); + + for (const auto & id : insertion_order) { + auto & entries = by_id[id]; + + // Start with highest-priority layer's entity as base + Entity merged = std::move(entries[0].entity); + size_t owner_layer_idx = entries[0].layer_idx; + report.entity_source[id] = layers_[owner_layer_idx]->name(); + + // Track current owning layer per field group (initially all owned by first layer) + std::array fg_owner; + fg_owner.fill(owner_layer_idx); + + // Merge with each subsequent (lower-priority) layer + for (size_t i = 1; i < entries.size(); i++) { + size_t source_layer_idx = entries[i].layer_idx; + report.enriched_count++; + + for (size_t fg_idx = 0; fg_idx < fg_owner.size(); ++fg_idx) { + auto fg = ALL_FIELD_GROUPS[fg_idx]; + size_t current_owner = fg_owner[fg_idx]; + auto target_policy = layers_[current_owner]->policy_for(fg); + auto source_policy = layers_[source_layer_idx]->policy_for(fg); + auto res = resolve_policies(target_policy, source_policy); + + if (res.is_conflict) { + report.conflicts.push_back({id, fg, layers_[current_owner]->name(), layers_[source_layer_idx]->name()}); + report.conflict_count++; + } + + apply_field_group_merge(merged, entries[i].entity, fg, res); + + // If source won with a strictly higher-priority policy, it becomes + // the owner of this field group for subsequent merge comparisons. + if (!res.is_conflict && policy_priority(source_policy) > policy_priority(target_policy)) { + fg_owner[fg_idx] = source_layer_idx; + } + } + } + + result.push_back(std::move(merged)); + } + + return result; +} + +MergeResult MergePipeline::execute() { + MergeReport report; + for (const auto & layer : layers_) { + report.layers.push_back(layer->name()); + } + + // Collect outputs from all layers + std::vector>> area_layers; + std::vector>> component_layers; + std::vector>> app_layers; + std::vector>> function_layers; + + // RuntimeLinker needs runtime-only apps (nodes as Apps, not merged with + // manifest apps). Manifest apps lack bound_fqn until linked. + std::vector runtime_apps; + + for (size_t i = 0; i < layers_.size(); ++i) { + // Build discovery context from entities collected so far (for plugin layers) + IntrospectionInput context; + for (const auto & [idx, entities] : area_layers) { + context.areas.insert(context.areas.end(), entities.begin(), entities.end()); + } + for (const auto & [idx, entities] : component_layers) { + context.components.insert(context.components.end(), entities.begin(), entities.end()); + } + for (const auto & [idx, entities] : app_layers) { + context.apps.insert(context.apps.end(), entities.begin(), entities.end()); + } + for (const auto & [idx, entities] : function_layers) { + context.functions.insert(context.functions.end(), entities.begin(), entities.end()); + } + layers_[i]->set_discovery_context(context); + + LayerOutput output; + try { + output = layers_[i]->discover(); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "Layer '%s' threw exception during discover(): %s", layers_[i]->name().c_str(), e.what()); + continue; + } catch (...) { + RCLCPP_ERROR(logger_, "Layer '%s' threw unknown exception during discover()", layers_[i]->name().c_str()); + continue; + } + + // Collect gap-fill filtering stats from RuntimeLayer + auto * runtime_layer = dynamic_cast(layers_[i].get()); + if (runtime_layer) { + report.filtered_by_gap_fill += runtime_layer->last_filtered_count(); + } + // Save runtime apps for the linker BEFORE they are moved into app_layers below. + if (runtime_layer || layers_[i]->provides_runtime_apps()) { + runtime_apps = output.apps; + } + + if (!output.areas.empty()) { + area_layers.emplace_back(i, std::move(output.areas)); + } + if (!output.components.empty()) { + component_layers.emplace_back(i, std::move(output.components)); + } + if (!output.apps.empty()) { + app_layers.emplace_back(i, std::move(output.apps)); + } + if (!output.functions.empty()) { + function_layers.emplace_back(i, std::move(output.functions)); + } + // entity_metadata is not consumed here - plugins serve their metadata + // as SOVD vendor extension resources via register_routes() and register_capability(). + } + + MergeResult result; + result.areas = merge_entities(area_layers, report); + result.components = merge_entities(component_layers, report); + result.apps = merge_entities(app_layers, report); + result.functions = merge_entities(function_layers, report); + + report.total_entities = result.areas.size() + result.components.size() + result.apps.size() + result.functions.size(); + + // Cross-type ID collision detection + std::unordered_map global_ids; + auto check_ids = [&](const auto & entities, const std::string & type) { + for (const auto & e : entities) { + auto [it, inserted] = global_ids.emplace(e.id, type); + if (!inserted && it->second != type) { + report.id_collision_count++; + RCLCPP_ERROR(logger_, "ID collision: '%s' used by both %s and %s", e.id.c_str(), it->second.c_str(), + type.c_str()); + } + } + }; + check_ids(result.areas, "Area"); + check_ids(result.components, "Component"); + check_ids(result.apps, "App"); + check_ids(result.functions, "Function"); + + // Post-merge linking: bind manifest apps to runtime nodes (Apps, not Components) + if (linker_) { + auto linking = linker_->link(result.apps, runtime_apps, manifest_config_); + + // Replace apps with linked versions (have is_online, bound_fqn set) + result.apps = std::move(linking.linked_apps); + + // Record linking stats in report + report.total_entities = + result.areas.size() + result.components.size() + result.apps.size() + result.functions.size(); + + linking_result_ = linker_->get_last_result(); + result.linking_result = linking_result_; + } + + RCLCPP_INFO(logger_, "MergePipeline: %zu entities from %zu layers, %zu enriched, %zu conflicts", + report.total_entities, report.layers.size(), report.enriched_count, report.conflict_count); + if (report.conflict_count > 0) { + RCLCPP_WARN(logger_, + "MergePipeline: %zu merge conflicts (higher-priority layer wins in all cases). " + "Details available via GET /health.", + report.conflict_count); + } + for (const auto & conflict : report.conflicts) { + RCLCPP_DEBUG(logger_, "Merge conflict: entity '%s' field_group %s - '%s' wins over '%s'", + conflict.entity_id.c_str(), field_group_to_string(conflict.field_group), + conflict.winning_layer.c_str(), conflict.losing_layer.c_str()); + } + + last_report_ = report; + result.report = std::move(report); + return result; +} + +// Explicit template instantiations +template std::vector MergePipeline::merge_entities(std::vector>> &, + MergeReport &); +template std::vector +MergePipeline::merge_entities(std::vector>> &, MergeReport &); +template std::vector MergePipeline::merge_entities(std::vector>> &, + MergeReport &); +template std::vector +MergePipeline::merge_entities(std::vector>> &, MergeReport &); + +} // namespace discovery +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp b/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp index 4eed8daf..c7113f24 100644 --- a/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp +++ b/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp @@ -94,6 +94,7 @@ std::vector RuntimeDiscoveryStrategy::discover_areas() { Area area; area.id = area_name; area.namespace_path = (area_name == "root") ? "/" : "/" + area_name; + area.source = "heuristic"; areas.push_back(area); } @@ -101,19 +102,36 @@ 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(); + return discover_components(discover_apps()); +} + +std::vector RuntimeDiscoveryStrategy::discover_components(const std::vector & apps) { + if (!config_.create_synthetic_components) { + // Legacy mode: each App becomes its own Component (1:1 mapping) + std::vector components; + components.reserve(apps.size()); + for (const auto & app : apps) { + Component comp; + comp.id = app.id; + comp.source = "heuristic"; + if (app.bound_fqn.has_value()) { + comp.fqn = app.bound_fqn.value(); + auto slash_pos = comp.fqn.rfind('/'); + comp.namespace_path = (slash_pos == std::string::npos || slash_pos == 0) ? "/" : comp.fqn.substr(0, slash_pos); + comp.area = extract_area_from_namespace(comp.namespace_path); + } + components.push_back(std::move(comp)); + } + return components; } - // Default: each node = 1 component (backward compatible) - return discover_node_components(); + + return discover_synthetic_components(apps); } -std::vector RuntimeDiscoveryStrategy::discover_node_components() { - std::vector components; +std::vector RuntimeDiscoveryStrategy::discover_apps() { + std::vector apps; // Pre-build service info map for schema lookups - // Key: service full path, Value: ServiceInfo with type info std::unordered_map service_info_map; auto all_services = discover_services(); for (const auto & svc : all_services) { @@ -121,7 +139,6 @@ std::vector RuntimeDiscoveryStrategy::discover_node_components() { } // Pre-build action info map for schema lookups - // Key: action full path, Value: ActionInfo with type info std::unordered_map action_info_map; auto all_actions = discover_actions(); for (const auto & act : all_actions) { @@ -145,62 +162,57 @@ std::vector RuntimeDiscoveryStrategy::discover_node_components() { 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 + // Skip duplicate nodes if (seen_fqns.count(fqn) > 0) { RCLCPP_DEBUG(node_->get_logger(), "Skipping duplicate node: %s", fqn.c_str()); continue; } seen_fqns.insert(fqn); - Component comp; - comp.id = name; - comp.namespace_path = ns; - comp.fqn = fqn; - comp.area = extract_area_from_namespace(ns); + App app; + app.id = name; + app.name = name; + app.source = "heuristic"; + app.is_online = true; + app.bound_fqn = fqn; - // Use ROS 2 introspection API to get services for this specific node - // This is more accurate than grouping by parent namespace + std::string area = extract_area_from_namespace(ns); + app.component_id = derive_component_id(name, area); + + // Introspect services and actions for this node try { auto node_services = node_->get_service_names_and_types_by_node(name, ns); for (const auto & [service_path, types] : node_services) { - // Skip internal ROS2 services (parameter services, action internals, etc.) if (is_internal_service(service_path)) { continue; } - - // Use pre-built service info map to get enriched info (with schema) auto it = service_info_map.find(service_path); if (it != service_info_map.end()) { - comp.services.push_back(it->second); + app.services.push_back(it->second); } else { - // Fallback: create basic ServiceInfo ServiceInfo info; info.full_path = service_path; info.name = extract_name_from_path(service_path); info.type = types.empty() ? "" : types[0]; - comp.services.push_back(info); + app.services.push_back(info); } } - // Detect actions owned by this node by checking for /_action/send_goal services + // Detect actions by checking for /_action/send_goal services for (const auto & [service_path, types] : node_services) { const std::string action_suffix = "/_action/send_goal"; if (service_path.length() > action_suffix.length() && service_path.compare(service_path.length() - action_suffix.length(), action_suffix.length(), action_suffix) == 0) { - // Extract action path by removing /_action/send_goal suffix std::string action_path = service_path.substr(0, service_path.length() - action_suffix.length()); - - // Use pre-built action info map to get enriched info (with schema) auto it = action_info_map.find(action_path); if (it != action_info_map.end()) { - comp.actions.push_back(it->second); + app.actions.push_back(it->second); } else { - // Fallback: create basic ActionInfo ActionInfo info; info.full_path = action_path; info.name = extract_name_from_path(action_path); - comp.actions.push_back(info); + app.actions.push_back(info); } } } @@ -211,41 +223,12 @@ std::vector RuntimeDiscoveryStrategy::discover_node_components() { // Populate topics from cached map if (topic_sampler_) { - auto it = cached_topic_map_.find(comp.fqn); + auto it = cached_topic_map_.find(fqn); if (it != cached_topic_map_.end()) { - comp.topics = it->second; + app.topics = it->second; } } - components.push_back(comp); - } - - return components; -} - -std::vector RuntimeDiscoveryStrategy::discover_apps() { - 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); } @@ -587,52 +570,49 @@ 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(); +std::vector RuntimeDiscoveryStrategy::discover_synthetic_components(const std::vector & apps) { + // Group runtime apps by their component_id (already derived during discover_apps) + std::map> groups; - for (const auto & node : node_components) { - std::string group_id = derive_component_id(node); - groups[group_id].push_back(node); + for (const auto & app : apps) { + groups[app.component_id].push_back(&app); } // Create synthetic components from groups std::vector result; - for (const auto & [group_id, nodes] : groups) { + for (const auto & [group_id, group_apps] : 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; + // Use first app's FQN to derive namespace and area + if (!group_apps.empty() && group_apps[0]->bound_fqn.has_value()) { + const auto & fqn = group_apps[0]->bound_fqn.value(); + auto pos = fqn.rfind('/'); + comp.namespace_path = (pos == std::string::npos || pos == 0) ? "/" : fqn.substr(0, pos); + comp.area = extract_area_from_namespace(comp.namespace_path); comp.fqn = "/" + group_id; } - // Note: Topics/services are NOT aggregated here - they stay with Apps - // This is intentional: synthetic components are just groupings + // Topics/services stay with Apps - synthetic components are just groupings - RCLCPP_DEBUG(node_->get_logger(), "Created synthetic component '%s' with %zu apps", group_id.c_str(), nodes.size()); + RCLCPP_DEBUG(node_->get_logger(), "Created synthetic component '%s' with %zu apps", group_id.c_str(), + group_apps.size()); result.push_back(comp); } - RCLCPP_DEBUG(node_->get_logger(), "Discovered %zu synthetic components from %zu nodes", result.size(), - node_components.size()); + RCLCPP_DEBUG(node_->get_logger(), "Discovered %zu synthetic components from %zu nodes", result.size(), apps.size()); return result; } -std::string RuntimeDiscoveryStrategy::derive_component_id(const Component & node) { +std::string RuntimeDiscoveryStrategy::derive_component_id(const std::string & node_id, const std::string & area) { switch (config_.grouping) { case ComponentGroupingStrategy::NAMESPACE: - // Group by area (first namespace segment) - return apply_component_name_pattern(node.area); + return apply_component_name_pattern(area); case ComponentGroupingStrategy::NONE: default: - // 1:1 mapping - each node is its own component - return node.id; + return node_id; } } diff --git a/src/ros2_medkit_gateway/src/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 7bfb89f0..234e42b7 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -123,9 +123,9 @@ GatewayNode::GatewayNode() : Node("ros2_medkit_gateway") { declare_parameter("rate_limiting.client_max_idle_seconds", 600); // Discovery mode parameters - declare_parameter("discovery_mode", "runtime_only"); // runtime_only, manifest_only, hybrid - declare_parameter("manifest_path", ""); - declare_parameter("manifest_strict_validation", true); + declare_parameter("discovery.mode", "runtime_only"); // runtime_only, manifest_only, hybrid + declare_parameter("discovery.manifest_path", ""); + declare_parameter("discovery.manifest_strict_validation", true); // Software updates parameters declare_parameter("updates.enabled", false); @@ -146,6 +146,21 @@ GatewayNode::GatewayNode() : Node("ros2_medkit_gateway") { declare_parameter("discovery.runtime.topic_only_policy", "create_component"); declare_parameter("discovery.runtime.min_topics_for_component", 1); + // Merge pipeline configuration (hybrid mode only) + declare_parameter("discovery.merge_pipeline.gap_fill.allow_heuristic_areas", true); + declare_parameter("discovery.merge_pipeline.gap_fill.allow_heuristic_components", true); + declare_parameter("discovery.merge_pipeline.gap_fill.allow_heuristic_apps", true); + declare_parameter("discovery.merge_pipeline.gap_fill.allow_heuristic_functions", false); + declare_parameter("discovery.merge_pipeline.gap_fill.namespace_whitelist", std::vector{}); + declare_parameter("discovery.merge_pipeline.gap_fill.namespace_blacklist", std::vector{}); + + // Per-layer merge policy overrides (optional, empty string = use layer default) + for (const auto & layer : {"manifest", "runtime"}) { + for (const auto & fg : {"identity", "hierarchy", "live_data", "status", "metadata"}) { + declare_parameter(std::string("discovery.merge_pipeline.layers.") + layer + "." + fg, std::string("")); + } + } + // Get parameter values server_host_ = get_parameter("server.host").as_string(); server_port_ = static_cast(get_parameter("server.port").as_int()); @@ -340,22 +355,63 @@ GatewayNode::GatewayNode() : Node("ros2_medkit_gateway") { // Configure and initialize discovery manager DiscoveryConfig discovery_config; - discovery_config.mode = parse_discovery_mode(get_parameter("discovery_mode").as_string()); - discovery_config.manifest_path = get_parameter("manifest_path").as_string(); - discovery_config.manifest_strict_validation = get_parameter("manifest_strict_validation").as_bool(); + + auto mode_str = get_parameter("discovery.mode").as_string(); + discovery_config.mode = parse_discovery_mode(mode_str); + if (mode_str != "runtime_only" && mode_str != "manifest_only" && mode_str != "hybrid") { + RCLCPP_WARN(get_logger(), "Unknown discovery.mode '%s', defaulting to 'runtime_only'", mode_str.c_str()); + } + + discovery_config.manifest_path = get_parameter("discovery.manifest_path").as_string(); + discovery_config.manifest_strict_validation = get_parameter("discovery.manifest_strict_validation").as_bool(); // Runtime discovery options 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()); + + auto grouping_str = get_parameter("discovery.runtime.grouping_strategy").as_string(); + discovery_config.runtime.grouping = parse_grouping_strategy(grouping_str); + if (grouping_str != "none" && grouping_str != "namespace") { + RCLCPP_WARN(get_logger(), "Unknown grouping_strategy '%s', defaulting to 'none'", grouping_str.c_str()); + } + discovery_config.runtime.synthetic_component_name_pattern = get_parameter("discovery.runtime.synthetic_component_name_pattern").as_string(); - discovery_config.runtime.topic_only_policy = - parse_topic_only_policy(get_parameter("discovery.runtime.topic_only_policy").as_string()); + + auto topic_policy_str = get_parameter("discovery.runtime.topic_only_policy").as_string(); + discovery_config.runtime.topic_only_policy = parse_topic_only_policy(topic_policy_str); + if (topic_policy_str != "ignore" && topic_policy_str != "create_component" && + topic_policy_str != "create_area_only") { + RCLCPP_WARN(get_logger(), "Unknown topic_only_policy '%s', defaulting to 'create_component'", + topic_policy_str.c_str()); + } discovery_config.runtime.min_topics_for_component = static_cast(get_parameter("discovery.runtime.min_topics_for_component").as_int()); + // Merge pipeline gap-fill configuration (hybrid mode) + discovery_config.merge_pipeline.gap_fill.allow_heuristic_areas = + get_parameter("discovery.merge_pipeline.gap_fill.allow_heuristic_areas").as_bool(); + discovery_config.merge_pipeline.gap_fill.allow_heuristic_components = + get_parameter("discovery.merge_pipeline.gap_fill.allow_heuristic_components").as_bool(); + discovery_config.merge_pipeline.gap_fill.allow_heuristic_apps = + get_parameter("discovery.merge_pipeline.gap_fill.allow_heuristic_apps").as_bool(); + discovery_config.merge_pipeline.gap_fill.allow_heuristic_functions = + get_parameter("discovery.merge_pipeline.gap_fill.allow_heuristic_functions").as_bool(); + discovery_config.merge_pipeline.gap_fill.namespace_whitelist = + get_parameter("discovery.merge_pipeline.gap_fill.namespace_whitelist").as_string_array(); + discovery_config.merge_pipeline.gap_fill.namespace_blacklist = + get_parameter("discovery.merge_pipeline.gap_fill.namespace_blacklist").as_string_array(); + + // Read per-layer merge policy overrides + for (const auto & layer : {"manifest", "runtime"}) { + for (const auto & fg : {"identity", "hierarchy", "live_data", "status", "metadata"}) { + auto val = get_parameter(std::string("discovery.merge_pipeline.layers.") + layer + "." + fg).as_string(); + if (!val.empty()) { + discovery_config.merge_pipeline.layer_policies[layer][fg] = val; + } + } + } + if (!discovery_mgr_->initialize(discovery_config)) { RCLCPP_ERROR(get_logger(), "Failed to initialize discovery manager"); throw std::runtime_error("Discovery initialization failed"); @@ -423,6 +479,17 @@ GatewayNode::GatewayNode() : Node("ros2_medkit_gateway") { plugin_ctx_ = make_gateway_plugin_context(this, fault_mgr_.get()); plugin_mgr_->set_context(*plugin_ctx_); RCLCPP_INFO(get_logger(), "Loaded %zu plugin(s)", loaded); + + // Register IntrospectionProvider plugins as pipeline layers (hybrid mode only) + if (discovery_mgr_->get_mode() == DiscoveryMode::HYBRID) { + auto providers = plugin_mgr_->get_named_introspection_providers(); + for (auto & [name, provider] : providers) { + discovery_mgr_->add_plugin_layer(name, provider); + } + if (!providers.empty()) { + discovery_mgr_->refresh_pipeline(); + } + } } // Initialize log manager (subscribes to /rosout, delegates to plugin if available) @@ -543,45 +610,35 @@ void GatewayNode::refresh_cache() { RCLCPP_DEBUG(get_logger(), "Refreshing entity cache..."); try { - // Refresh topic map first (rebuilds the cached map) + // Refresh topic map first (rebuilds the cached map, triggers pipeline in hybrid mode) discovery_mgr_->refresh_topic_map(); - // Discover data outside the lock to minimize lock time + // Discover entities - in HYBRID mode the pipeline merges all sources, + // in RUNTIME_ONLY mode we manually merge node + topic components auto areas = discovery_mgr_->discover_areas(); - - // Discover node-based components (standard ROS 2 nodes) - auto node_components = discovery_mgr_->discover_components(); - - // Discover topic-based components (for systems like Isaac Sim that - // 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(); - - // Discover functions (from manifest in manifest_only/hybrid mode) auto functions = discovery_mgr_->discover_functions(); - // Merge both component lists std::vector all_components; - all_components.reserve(node_components.size() + topic_components.size()); - all_components.insert(all_components.end(), node_components.begin(), node_components.end()); - all_components.insert(all_components.end(), topic_components.begin(), topic_components.end()); + if (discovery_mgr_->get_mode() == DiscoveryMode::HYBRID) { + // Pipeline already merges node and topic components + all_components = discovery_mgr_->discover_components(); + } else { + auto node_components = discovery_mgr_->discover_components(); + auto topic_components = discovery_mgr_->discover_topic_components(); + all_components.reserve(node_components.size() + topic_components.size()); + all_components.insert(all_components.end(), node_components.begin(), node_components.end()); + all_components.insert(all_components.end(), topic_components.begin(), topic_components.end()); + } - // Capture sizes before move for logging + // Capture sizes for logging 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 component_count = all_components.size(); const size_t app_count = apps.size(); const size_t function_count = functions.size(); - // Update ThreadSafeEntityCache (primary) with copies - // This provides O(1) lookups and proper thread safety - thread_safe_cache_.update_all(areas, // copy - all_components, // copy - apps, // copy - functions // copy - ); + // Update ThreadSafeEntityCache with copies + thread_safe_cache_.update_all(areas, all_components, apps, functions); // Update topic type cache (avoids expensive ROS graph queries on /data requests) if (data_access_mgr_) { @@ -597,11 +654,8 @@ void GatewayNode::refresh_cache() { thread_safe_cache_.update_topic_types(std::move(topic_types)); } - RCLCPP_DEBUG( - get_logger(), - "Cache refreshed: %zu areas, %zu components (%zu node-based, %zu topic-based), %zu apps, %zu functions", - area_count, node_component_count + topic_component_count, node_component_count, topic_component_count, - app_count, function_count); + RCLCPP_DEBUG(get_logger(), "Cache refreshed: %zu areas, %zu components, %zu apps, %zu functions", area_count, + component_count, app_count, function_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/health_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/health_handlers.cpp index 4fe5ff95..04353583 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/health_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/health_handlers.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 bburda +// Copyright 2025-2026 bburda // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,6 +17,9 @@ #include #include "ros2_medkit_gateway/auth/auth_models.hpp" +#include "ros2_medkit_gateway/discovery/discovery_enums.hpp" +#include "ros2_medkit_gateway/discovery/discovery_manager.hpp" +#include "ros2_medkit_gateway/gateway_node.hpp" #include "ros2_medkit_gateway/http/error_codes.hpp" #include "ros2_medkit_gateway/http/http_utils.hpp" @@ -31,6 +34,31 @@ void HealthHandlers::handle_health(const httplib::Request & req, httplib::Respon try { json response = {{"status", "healthy"}, {"timestamp", std::chrono::system_clock::now().time_since_epoch().count()}}; + // Add discovery info + auto * dm = ctx_.node() ? ctx_.node()->get_discovery_manager() : nullptr; + if (dm) { + json discovery_info = {{"mode", discovery_mode_to_string(dm->get_mode())}, {"strategy", dm->get_strategy_name()}}; + + auto report = dm->get_merge_report(); + if (report) { + discovery_info["pipeline"] = report->to_json(); + } + + auto linking = dm->get_linking_result(); + if (linking) { + json linking_info; + linking_info["linked_count"] = linking->node_to_app.size(); + linking_info["orphan_count"] = linking->orphan_nodes.size(); + linking_info["binding_conflicts"] = linking->binding_conflicts; + if (!linking->warnings.empty()) { + linking_info["warnings"] = linking->warnings; + } + discovery_info["linking"] = linking_info; + } + + response["discovery"] = std::move(discovery_info); + } + HandlerContext::send_json(res, response); } catch (const std::exception & e) { HandlerContext::send_error(res, 500, ERR_INTERNAL_ERROR, "Internal server error"); diff --git a/src/ros2_medkit_gateway/src/plugins/plugin_manager.cpp b/src/ros2_medkit_gateway/src/plugins/plugin_manager.cpp index 3ef41c5d..e0cb5a32 100644 --- a/src/ros2_medkit_gateway/src/plugins/plugin_manager.cpp +++ b/src/ros2_medkit_gateway/src/plugins/plugin_manager.cpp @@ -283,6 +283,20 @@ std::vector PluginManager::get_log_observers() const { return result; } +std::vector> PluginManager::get_named_introspection_providers() const { + std::shared_lock lock(plugins_mutex_); + std::vector> result; + for (const auto & lp : plugins_) { + if (!lp.load_result.plugin) { + continue; + } + if (lp.introspection_provider) { + result.emplace_back(lp.load_result.plugin->name(), lp.introspection_provider); + } + } + return result; +} + bool PluginManager::has_plugins() const { std::shared_lock lock(plugins_mutex_); for (const auto & lp : plugins_) { diff --git a/src/ros2_medkit_gateway/test/demo_nodes/test_gateway_plugin.cpp b/src/ros2_medkit_gateway/test/demo_nodes/test_gateway_plugin.cpp index 5f48f73f..7dbd13a5 100644 --- a/src/ros2_medkit_gateway/test/demo_nodes/test_gateway_plugin.cpp +++ b/src/ros2_medkit_gateway/test/demo_nodes/test_gateway_plugin.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "ros2_medkit_gateway/plugins/gateway_plugin.hpp" +#include "ros2_medkit_gateway/plugins/plugin_context.hpp" #include "ros2_medkit_gateway/plugins/plugin_types.hpp" #include "ros2_medkit_gateway/providers/introspection_provider.hpp" #include "ros2_medkit_gateway/providers/update_provider.hpp" @@ -29,6 +30,10 @@ using namespace ros2_medkit_gateway; * * Used by test_plugin_loader to verify dlopen/dlsym-based loading, * API version checking, and extern "C" provider query functions. + * + * Also demonstrates vendor extension endpoints via register_capability() + * and entity-scoped routes. The plugin registers an "x-medkit-diagnostics" + * capability on all Components and serves diagnostic data per entity. */ class TestGatewayPlugin : public GatewayPlugin, public UpdateProvider, public IntrospectionProvider { public: @@ -40,12 +45,38 @@ class TestGatewayPlugin : public GatewayPlugin, public UpdateProvider, public In void configure(const nlohmann::json & /*config*/) override { } + void set_context(PluginContext & context) override { + ctx_ = &context; + // Register vendor extension capability for all Components + ctx_->register_capability(SovdEntityType::COMPONENT, "x-medkit-diagnostics"); + } + void register_routes(httplib::Server & server, const std::string & api_prefix) override { + // Global vendor extension endpoint server.Get((api_prefix + "/x-test/ping").c_str(), [](const httplib::Request &, httplib::Response & res) { res.set_content("pong", "text/plain"); }); + + // Entity-scoped vendor extension: GET /components/{id}/x-medkit-diagnostics + server.Get((api_prefix + R"(/components/([^/]+)/x-medkit-diagnostics)").c_str(), + [this](const httplib::Request & req, httplib::Response & res) { + auto entity_id = req.matches[1].str(); + auto entity = ctx_->validate_entity_for_route(req, res, entity_id); + if (!entity) { + return; + } + nlohmann::json data = {{"entity_id", entity->id}, + {"plugin", "test_plugin"}, + {"cpu_usage", 42.5}, + {"memory_mb", 128}, + {"uptime_seconds", 3600}}; + PluginContext::send_json(res, data); + }); } + private: + PluginContext * ctx_{nullptr}; + // --- UpdateProvider --- tl::expected, UpdateBackendErrorInfo> list_updates(const UpdateFilter &) override { return std::vector{}; diff --git a/src/ros2_medkit_gateway/test/test_auth_manager.cpp b/src/ros2_medkit_gateway/test/test_auth_manager.cpp index 33e11d51..23ee6eba 100644 --- a/src/ros2_medkit_gateway/test/test_auth_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_auth_manager.cpp @@ -578,8 +578,8 @@ TEST_F(AuthManagerTest, CleanupExpiredTokens) { auto result = manager.authenticate("test", "test"); ASSERT_TRUE(result.has_value()); - // Wait for tokens to expire - std::this_thread::sleep_for(std::chrono::seconds(2)); + // Wait for tokens to expire (3s margin for loaded systems) + std::this_thread::sleep_for(std::chrono::seconds(3)); // Cleanup should remove expired tokens size_t cleaned = manager.cleanup_expired_tokens(); diff --git a/src/ros2_medkit_gateway/test/test_health_handlers.cpp b/src/ros2_medkit_gateway/test/test_health_handlers.cpp index f92a8d68..2dc95392 100644 --- a/src/ros2_medkit_gateway/test/test_health_handlers.cpp +++ b/src/ros2_medkit_gateway/test/test_health_handlers.cpp @@ -57,6 +57,14 @@ TEST_F(HealthHandlersTest, HandleHealthResponseContainsStatusHealthy) { EXPECT_EQ(body["status"], "healthy"); } +TEST_F(HealthHandlersTest, HandleHealthNullNodeOmitsDiscovery) { + // ctx_ uses nullptr for GatewayNode, so discovery info should not be present + handlers_.handle_health(req_, res_); + auto body = json::parse(res_.body); + EXPECT_EQ(body["status"], "healthy"); + EXPECT_FALSE(body.contains("discovery")); +} + TEST_F(HealthHandlersTest, HandleHealthResponseContainsTimestamp) { handlers_.handle_health(req_, res_); auto body = json::parse(res_.body); diff --git a/src/ros2_medkit_gateway/test/test_merge_pipeline.cpp b/src/ros2_medkit_gateway/test/test_merge_pipeline.cpp new file mode 100644 index 00000000..b9ea5f75 --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_merge_pipeline.cpp @@ -0,0 +1,1117 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_medkit_gateway/discovery/discovery_layer.hpp" +#include "ros2_medkit_gateway/discovery/layers/manifest_layer.hpp" +#include "ros2_medkit_gateway/discovery/layers/plugin_layer.hpp" +#include "ros2_medkit_gateway/discovery/layers/runtime_layer.hpp" +#include "ros2_medkit_gateway/discovery/manifest/runtime_linker.hpp" +#include "ros2_medkit_gateway/discovery/merge_pipeline.hpp" +#include "ros2_medkit_gateway/discovery/merge_types.hpp" + +#include + +#include + +using namespace ros2_medkit_gateway::discovery; +using namespace ros2_medkit_gateway; + +// Concrete test layer for unit testing +class TestLayer : public DiscoveryLayer { + public: + TestLayer(std::string name, LayerOutput output, std::unordered_map policies = {}) + : name_(std::move(name)), output_(std::move(output)), policies_(std::move(policies)) { + } + + std::string name() const override { + return name_; + } + bool provides_runtime_apps() const override { + return name_ == "runtime"; + } + LayerOutput discover() override { + return output_; + } + + MergePolicy policy_for(FieldGroup group) const override { + auto it = policies_.find(group); + if (it != policies_.end()) { + return it->second; + } + return MergePolicy::ENRICHMENT; // default + } + + private: + std::string name_; + LayerOutput output_; + std::unordered_map policies_; +}; + +// @verifies REQ_INTEROP_003 +TEST(MergeTypesTest, MergePolicyValues) { + // Verify enum values exist and are distinct + EXPECT_NE(static_cast(MergePolicy::AUTHORITATIVE), static_cast(MergePolicy::ENRICHMENT)); + EXPECT_NE(static_cast(MergePolicy::ENRICHMENT), static_cast(MergePolicy::FALLBACK)); +} + +TEST(MergeTypesTest, FieldGroupValues) { + // Verify all 5 field groups exist + EXPECT_NE(static_cast(FieldGroup::IDENTITY), static_cast(FieldGroup::HIERARCHY)); + EXPECT_NE(static_cast(FieldGroup::LIVE_DATA), static_cast(FieldGroup::STATUS)); + auto metadata = FieldGroup::METADATA; + (void)metadata; +} + +TEST(MergeTypesTest, MergeReportDefaultEmpty) { + MergeReport report; + EXPECT_TRUE(report.conflicts.empty()); + EXPECT_TRUE(report.entity_source.empty()); + EXPECT_EQ(report.total_entities, 0u); + EXPECT_EQ(report.enriched_count, 0u); + EXPECT_EQ(report.conflict_count, 0u); + EXPECT_EQ(report.id_collision_count, 0u); +} + +TEST(MergeTypesTest, MergeReportToJson) { + MergeReport report; + report.layers = {"manifest", "runtime"}; + report.total_entities = 10; + report.enriched_count = 7; + report.conflict_count = 2; + report.id_collision_count = 0; + + auto j = report.to_json(); + EXPECT_EQ(j["total_entities"], 10); + EXPECT_EQ(j["enriched_count"], 7); + EXPECT_EQ(j["conflict_count"], 2); + EXPECT_EQ(j["id_collisions"], 0); + EXPECT_EQ(j["layers"].size(), 2u); +} + +TEST(DiscoveryLayerTest, TestLayerReturnsConfiguredOutput) { + LayerOutput output; + Area area; + area.id = "powertrain"; + area.name = "Powertrain"; + output.areas.push_back(area); + + TestLayer layer("test", output); + EXPECT_EQ(layer.name(), "test"); + + auto result = layer.discover(); + ASSERT_EQ(result.areas.size(), 1u); + EXPECT_EQ(result.areas[0].id, "powertrain"); +} + +TEST(DiscoveryLayerTest, PolicyForReturnsConfiguredPolicy) { + TestLayer layer("test", {}, + {{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}, {FieldGroup::LIVE_DATA, MergePolicy::FALLBACK}}); + + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::LIVE_DATA), MergePolicy::FALLBACK); + EXPECT_EQ(layer.policy_for(FieldGroup::STATUS), MergePolicy::ENRICHMENT); // default +} + +// Helper to create test entities +namespace { + +Area make_area(const std::string & id, const std::string & name = "") { + Area a; + a.id = id; + a.name = name.empty() ? id : name; + a.namespace_path = "/" + id; + return a; +} + +Component make_component(const std::string & id, const std::string & area = "", const std::string & ns = "/") { + Component c; + c.id = id; + c.name = id; + c.area = area; + c.namespace_path = ns; + c.fqn = ns == "/" ? "/" + id : ns + "/" + id; + c.source = "test"; + return c; +} + +App make_app(const std::string & id, const std::string & component_id = "") { + App a; + a.id = id; + a.name = id; + a.component_id = component_id; + a.source = "test"; + return a; +} + +Function make_function(const std::string & id, const std::string & name = "") { + Function f; + f.id = id; + f.name = name.empty() ? id : name; + return f; +} + +} // namespace + +// @verifies REQ_INTEROP_003 +class MergePipelineTest : public ::testing::Test { + protected: + MergePipeline pipeline_; +}; + +TEST_F(MergePipelineTest, EmptyPipelineReturnsEmptyResult) { + auto result = pipeline_.execute(); + EXPECT_TRUE(result.areas.empty()); + EXPECT_TRUE(result.components.empty()); + EXPECT_TRUE(result.apps.empty()); + EXPECT_TRUE(result.functions.empty()); + EXPECT_EQ(result.report.total_entities, 0u); +} + +TEST_F(MergePipelineTest, SingleLayerPassthrough) { + LayerOutput output; + output.areas.push_back(make_area("powertrain")); + output.components.push_back(make_component("engine", "powertrain", "/powertrain")); + output.apps.push_back(make_app("engine_app", "engine")); + + pipeline_.add_layer(std::make_unique("manifest", output)); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.areas.size(), 1u); + EXPECT_EQ(result.areas[0].id, "powertrain"); + ASSERT_EQ(result.components.size(), 1u); + EXPECT_EQ(result.components[0].id, "engine"); + ASSERT_EQ(result.apps.size(), 1u); + EXPECT_EQ(result.apps[0].id, "engine_app"); + EXPECT_EQ(result.report.total_entities, 3u); + EXPECT_EQ(result.report.conflict_count, 0u); +} + +TEST_F(MergePipelineTest, MultipleLayersDisjointEntities) { + LayerOutput manifest_output; + manifest_output.areas.push_back(make_area("powertrain")); + + LayerOutput runtime_output; + runtime_output.areas.push_back(make_area("chassis")); + + pipeline_.add_layer(std::make_unique("manifest", manifest_output)); + pipeline_.add_layer(std::make_unique("runtime", runtime_output)); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.areas.size(), 2u); + EXPECT_EQ(result.report.total_entities, 2u); + EXPECT_EQ(result.report.conflict_count, 0u); +} + +TEST_F(MergePipelineTest, AuthoritativeWinsOverEnrichment) { + // Manifest: IDENTITY=AUTH, LIVE_DATA=ENRICH + // Runtime: IDENTITY=FALLBACK, LIVE_DATA=AUTH + // Same component in both - manifest name wins, runtime topics win + + Component manifest_comp = make_component("engine", "powertrain", "/powertrain"); + manifest_comp.name = "Engine ECU"; + manifest_comp.source = "manifest"; + + Component runtime_comp = make_component("engine", "powertrain", "/powertrain"); + runtime_comp.name = "engine"; + runtime_comp.source = "node"; + runtime_comp.topics.publishes = {"/powertrain/engine/rpm"}; + + LayerOutput manifest_out; + manifest_out.components.push_back(manifest_comp); + + LayerOutput runtime_out; + runtime_out.components.push_back(runtime_comp); + + pipeline_.add_layer(std::make_unique( + "manifest", manifest_out, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}, + {FieldGroup::LIVE_DATA, MergePolicy::ENRICHMENT}})); + + pipeline_.add_layer(std::make_unique( + "runtime", runtime_out, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::FALLBACK}, + {FieldGroup::LIVE_DATA, MergePolicy::AUTHORITATIVE}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.components.size(), 1u); + EXPECT_EQ(result.components[0].name, "Engine ECU"); // manifest IDENTITY wins + EXPECT_EQ(result.components[0].topics.publishes.size(), 1u); // runtime LIVE_DATA wins + EXPECT_EQ(result.components[0].source, "manifest"); // higher priority source +} + +TEST_F(MergePipelineTest, EnrichmentFillsEmptyFields) { + // Manifest has name but no topics + // Runtime has topics + // Both declare ENRICHMENT for LIVE_DATA + // Result: topics filled from runtime + + Component manifest_comp = make_component("engine", "powertrain", "/powertrain"); + manifest_comp.source = "manifest"; + + Component runtime_comp = make_component("engine", "powertrain", "/powertrain"); + runtime_comp.topics.publishes = {"/powertrain/engine/rpm"}; + runtime_comp.topics.subscribes = {"/powertrain/engine/throttle"}; + + LayerOutput manifest_out; + manifest_out.components.push_back(manifest_comp); + + LayerOutput runtime_out; + runtime_out.components.push_back(runtime_comp); + + pipeline_.add_layer(std::make_unique( + "manifest", manifest_out, + std::unordered_map{{FieldGroup::LIVE_DATA, MergePolicy::ENRICHMENT}})); + pipeline_.add_layer(std::make_unique( + "runtime", runtime_out, + std::unordered_map{{FieldGroup::LIVE_DATA, MergePolicy::ENRICHMENT}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.components.size(), 1u); + EXPECT_FALSE(result.components[0].topics.publishes.empty()); +} + +TEST_F(MergePipelineTest, AuthoritativeVsAuthoritativeHigherPriorityWins) { + // Both layers claim AUTHORITATIVE for IDENTITY + // Higher priority (first added) wins, conflict logged + + Component manifest_comp = make_component("engine", "powertrain", "/powertrain"); + manifest_comp.name = "Manifest Engine"; + + Component runtime_comp = make_component("engine", "powertrain", "/powertrain"); + runtime_comp.name = "Runtime Engine"; + + LayerOutput manifest_out; + manifest_out.components.push_back(manifest_comp); + LayerOutput runtime_out; + runtime_out.components.push_back(runtime_comp); + + pipeline_.add_layer(std::make_unique( + "manifest", manifest_out, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}})); + pipeline_.add_layer(std::make_unique( + "runtime", runtime_out, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.components.size(), 1u); + EXPECT_EQ(result.components[0].name, "Manifest Engine"); // higher priority wins + EXPECT_GE(result.report.conflict_count, 1u); // conflict recorded +} + +TEST_F(MergePipelineTest, CollectionFieldsUnionOnEnrichment) { + // Both layers provide services for the same component with ENRICHMENT + // Result: union of services (deduped by full_path) + + Component layer1_comp = make_component("engine", "", "/powertrain"); + layer1_comp.services.push_back( + ServiceInfo{"calibrate", "/powertrain/engine/calibrate", "std_srvs/srv/Trigger", std::nullopt}); + + Component layer2_comp = make_component("engine", "", "/powertrain"); + layer2_comp.services.push_back( + ServiceInfo{"calibrate", "/powertrain/engine/calibrate", "std_srvs/srv/Trigger", std::nullopt}); + layer2_comp.services.push_back( + ServiceInfo{"reset", "/powertrain/engine/reset", "std_srvs/srv/Trigger", std::nullopt}); + + LayerOutput out1, out2; + out1.components.push_back(layer1_comp); + out2.components.push_back(layer2_comp); + + pipeline_.add_layer(std::make_unique( + "layer1", out1, std::unordered_map{{FieldGroup::LIVE_DATA, MergePolicy::ENRICHMENT}})); + pipeline_.add_layer(std::make_unique( + "layer2", out2, std::unordered_map{{FieldGroup::LIVE_DATA, MergePolicy::ENRICHMENT}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.components.size(), 1u); + // Union: calibrate (deduped) + reset = 2 services + EXPECT_EQ(result.components[0].services.size(), 2u); +} + +// --- ManifestLayer and RuntimeLayer tests --- + +// @verifies REQ_INTEROP_003 +TEST(ManifestLayerTest, DefaultPolicies) { + ManifestLayer layer(nullptr); + EXPECT_EQ(layer.name(), "manifest"); + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::HIERARCHY), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::LIVE_DATA), MergePolicy::ENRICHMENT); + EXPECT_EQ(layer.policy_for(FieldGroup::STATUS), MergePolicy::FALLBACK); + EXPECT_EQ(layer.policy_for(FieldGroup::METADATA), MergePolicy::AUTHORITATIVE); +} + +TEST(ManifestLayerTest, PolicyOverride) { + ManifestLayer layer(nullptr); + layer.set_policy(FieldGroup::LIVE_DATA, MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::LIVE_DATA), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::AUTHORITATIVE); +} + +TEST(ManifestLayerTest, DiscoverReturnsEmptyWhenNoManifest) { + ManifestLayer layer(nullptr); + auto output = layer.discover(); + EXPECT_TRUE(output.areas.empty()); + EXPECT_TRUE(output.components.empty()); + EXPECT_TRUE(output.apps.empty()); + EXPECT_TRUE(output.functions.empty()); +} + +// @verifies REQ_INTEROP_003 +TEST(RuntimeLayerTest, DefaultPolicies) { + RuntimeLayer layer(nullptr); + EXPECT_EQ(layer.name(), "runtime"); + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::FALLBACK); + EXPECT_EQ(layer.policy_for(FieldGroup::HIERARCHY), MergePolicy::FALLBACK); + EXPECT_EQ(layer.policy_for(FieldGroup::LIVE_DATA), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::STATUS), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::METADATA), MergePolicy::ENRICHMENT); +} + +TEST(RuntimeLayerTest, DiscoverReturnsEmptyWhenNoStrategy) { + RuntimeLayer layer(nullptr); + auto output = layer.discover(); + EXPECT_TRUE(output.areas.empty()); + EXPECT_TRUE(output.components.empty()); +} + +// --- PluginLayer tests --- + +class MockIntrospectionProvider : public IntrospectionProvider { + public: + IntrospectionResult introspect(const IntrospectionInput & input) override { + last_input_ = input; + introspect_called_ = true; + return result_; + } + + IntrospectionResult result_; + IntrospectionInput last_input_; + bool introspect_called_{false}; +}; + +// @verifies REQ_INTEROP_003 +TEST(PluginLayerTest, DefaultPolicies) { + auto provider = std::make_shared(); + PluginLayer layer("lidar_mapper", provider.get()); + EXPECT_EQ(layer.name(), "lidar_mapper"); + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::ENRICHMENT); + EXPECT_EQ(layer.policy_for(FieldGroup::METADATA), MergePolicy::ENRICHMENT); +} + +TEST(PluginLayerTest, MapsNewEntitiesToLayerOutput) { + auto provider = std::make_shared(); + + Component new_comp; + new_comp.id = "lidar_unit"; + new_comp.name = "LiDAR Processing Unit"; + new_comp.source = "plugin"; + provider->result_.new_entities.components.push_back(new_comp); + + PluginLayer layer("lidar_mapper", provider.get()); + auto output = layer.discover(); + ASSERT_EQ(output.components.size(), 1u); + EXPECT_EQ(output.components[0].id, "lidar_unit"); +} + +TEST(PluginLayerTest, MetadataPassedThrough) { + auto provider = std::make_shared(); + provider->result_.metadata["engine"] = {{"x-medkit-temperature", 85}}; + + PluginLayer layer("sensor_plugin", provider.get()); + auto output = layer.discover(); + ASSERT_EQ(output.entity_metadata.size(), 1u); + EXPECT_TRUE(output.entity_metadata.count("engine")); + EXPECT_EQ(layer.get_metadata().at("engine")["x-medkit-temperature"], 85); +} + +TEST(PluginLayerTest, DiscoverReturnsEmptyWhenNoProvider) { + PluginLayer layer("broken_plugin", nullptr); + auto output = layer.discover(); + EXPECT_TRUE(output.areas.empty()); + EXPECT_TRUE(output.components.empty()); + EXPECT_TRUE(output.apps.empty()); + EXPECT_TRUE(output.entity_metadata.empty()); +} + +// @verifies REQ_INTEROP_003 +TEST_F(MergePipelineTest, PluginReceivesDiscoveryContext) { + // Manifest layer provides a component + LayerOutput manifest_out; + manifest_out.components.push_back(make_component("engine", "powertrain", "/powertrain")); + manifest_out.apps.push_back(make_app("engine_ecu", "engine")); + pipeline_.add_layer(std::make_unique("manifest", manifest_out)); + + // Runtime layer provides an area + LayerOutput runtime_out; + runtime_out.areas.push_back(make_area("powertrain")); + pipeline_.add_layer(std::make_unique("runtime", runtime_out)); + + // Plugin layer should see entities from manifest + runtime + auto provider = std::make_shared(); + auto plugin = std::make_unique("test_plugin", provider.get()); + pipeline_.add_layer(std::move(plugin)); + + pipeline_.execute(); + + // Plugin's introspect() should have received the context from previous layers + ASSERT_TRUE(provider->introspect_called_); + EXPECT_EQ(provider->last_input_.components.size(), 1u); + EXPECT_EQ(provider->last_input_.components[0].id, "engine"); + EXPECT_EQ(provider->last_input_.apps.size(), 1u); + EXPECT_EQ(provider->last_input_.apps[0].id, "engine_ecu"); + EXPECT_EQ(provider->last_input_.areas.size(), 1u); + EXPECT_EQ(provider->last_input_.areas[0].id, "powertrain"); +} + +// --- GapFillConfig tests --- + +// @verifies REQ_INTEROP_003 +TEST(GapFillConfigTest, DefaultAllowsAll) { + GapFillConfig config; + EXPECT_TRUE(config.allow_heuristic_areas); + EXPECT_TRUE(config.allow_heuristic_components); + EXPECT_TRUE(config.allow_heuristic_apps); + EXPECT_FALSE(config.allow_heuristic_functions); +} + +TEST(GapFillConfigTest, NamespaceBlacklist) { + GapFillConfig config; + config.namespace_blacklist = {"/rosout", "/parameter_events"}; + EXPECT_EQ(config.namespace_blacklist.size(), 2u); + EXPECT_EQ(config.namespace_blacklist[0], "/rosout"); +} + +TEST(RuntimeLayerTest, GapFillFilterBlocksApps) { + GapFillConfig gap_fill; + gap_fill.allow_heuristic_apps = false; + + RuntimeLayer layer(nullptr); + layer.set_gap_fill_config(gap_fill); + auto output = layer.discover(); + EXPECT_TRUE(output.apps.empty()); +} + +TEST(MergeReportTest, FilteredByGapFillInJson) { + MergeReport report; + report.layers = {"manifest", "runtime"}; + report.total_entities = 5; + report.filtered_by_gap_fill = 3; + + auto j = report.to_json(); + EXPECT_EQ(j["filtered_by_gap_fill"], 3); +} + +TEST(RuntimeLayerTest, FilteredCountTracked) { + // RuntimeLayer with no strategy returns 0 filtered + RuntimeLayer layer(nullptr); + auto output = layer.discover(); + EXPECT_EQ(layer.last_filtered_count(), 0u); +} + +// --- Post-merge linking tests --- + +TEST_F(MergePipelineTest, PostMergeLinkingSetsAppOnlineStatus) { + // Manifest provides app with ros_binding + App manifest_app = make_app("controller_app", "nav_comp"); + manifest_app.source = "manifest"; + App::RosBinding binding; + binding.node_name = "controller"; + binding.namespace_pattern = "/nav"; + manifest_app.ros_binding = binding; + + // Runtime provides matching app (node) + App runtime_node; + runtime_node.id = "controller"; + runtime_node.name = "controller"; + runtime_node.source = "heuristic"; + runtime_node.is_online = true; + runtime_node.bound_fqn = "/nav/controller"; + + LayerOutput manifest_out; + manifest_out.apps.push_back(manifest_app); + + LayerOutput runtime_out; + runtime_out.apps.push_back(runtime_node); + + pipeline_.add_layer(std::make_unique( + "manifest", manifest_out, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}, + {FieldGroup::STATUS, MergePolicy::FALLBACK}})); + pipeline_.add_layer(std::make_unique( + "runtime", runtime_out, + std::unordered_map{{FieldGroup::STATUS, MergePolicy::AUTHORITATIVE}})); + + // Enable linking + ManifestConfig manifest_config; + pipeline_.set_linker(std::make_unique(nullptr), manifest_config); + + auto result = pipeline_.execute(); + // Both apps in result (different IDs). Linker matches controller_app's binding + // to runtime controller's bound_fqn. + auto it = std::find_if(result.apps.begin(), result.apps.end(), [](const App & a) { + return a.id == "controller_app"; + }); + ASSERT_NE(it, result.apps.end()); + EXPECT_TRUE(it->is_online); + EXPECT_EQ(it->bound_fqn, "/nav/controller"); +} + +TEST_F(MergePipelineTest, PostMergeLinkingReportsOrphanNodes) { + // Runtime provides an app (node) with no matching manifest app + App orphan_node; + orphan_node.id = "orphan_node"; + orphan_node.name = "orphan_node"; + orphan_node.source = "heuristic"; + orphan_node.is_online = true; + orphan_node.bound_fqn = "/test/orphan_node"; + + LayerOutput runtime_out; + runtime_out.apps.push_back(orphan_node); + + pipeline_.add_layer(std::make_unique("runtime", runtime_out)); + + ManifestConfig manifest_config; + pipeline_.set_linker(std::make_unique(nullptr), manifest_config); + + auto result = pipeline_.execute(); + auto linking = pipeline_.get_linking_result(); + EXPECT_FALSE(linking.orphan_nodes.empty()); +} + +// --- M4: Cross-type entity ID collision detection --- + +TEST_F(MergePipelineTest, CrossTypeIdCollisionDetected) { + // Same ID used for both an Area and a Component - should detect collision + LayerOutput output; + output.areas.push_back(make_area("shared_id")); + output.components.push_back(make_component("shared_id")); + + pipeline_.add_layer(std::make_unique("layer1", output)); + + auto result = pipeline_.execute(); + EXPECT_EQ(result.report.id_collision_count, 1u); +} + +TEST_F(MergePipelineTest, SameTypeIdIsNotCollision) { + // Same ID in two layers of the same type - normal merge, not a collision + LayerOutput out1; + out1.areas.push_back(make_area("powertrain")); + LayerOutput out2; + out2.areas.push_back(make_area("powertrain")); + + pipeline_.add_layer(std::make_unique("layer1", out1)); + pipeline_.add_layer(std::make_unique("layer2", out2)); + + auto result = pipeline_.execute(); + EXPECT_EQ(result.report.id_collision_count, 0u); + EXPECT_EQ(result.areas.size(), 1u); // merged, not duplicated +} + +// --- M5: FALLBACK policy behavior --- + +TEST_F(MergePipelineTest, FallbackVsFallbackKeepsTargetWhenBothHaveData) { + // Both layers declare FALLBACK for IDENTITY, both have name set + // Higher priority target keeps its value (first non-empty) + Component comp1 = make_component("engine", "powertrain", "/powertrain"); + comp1.name = "First Layer Engine"; + + Component comp2 = make_component("engine", "powertrain", "/powertrain"); + comp2.name = "Second Layer Engine"; + + LayerOutput out1, out2; + out1.components.push_back(comp1); + out2.components.push_back(comp2); + + pipeline_.add_layer(std::make_unique( + "layer1", out1, std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::FALLBACK}})); + pipeline_.add_layer(std::make_unique( + "layer2", out2, std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::FALLBACK}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.components.size(), 1u); + EXPECT_EQ(result.components[0].name, "First Layer Engine"); + EXPECT_EQ(result.report.conflict_count, 0u); +} + +TEST_F(MergePipelineTest, FallbackVsFallbackFillsEmptyFields) { + // Target has empty description, source has it - FALLBACK should fill the gap + Component comp1 = make_component("engine", "powertrain", "/powertrain"); + comp1.description = ""; + + Component comp2 = make_component("engine", "powertrain", "/powertrain"); + comp2.description = "Engine control unit"; + + LayerOutput out1, out2; + out1.components.push_back(comp1); + out2.components.push_back(comp2); + + pipeline_.add_layer(std::make_unique( + "layer1", out1, std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::FALLBACK}})); + pipeline_.add_layer(std::make_unique( + "layer2", out2, std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::FALLBACK}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.components.size(), 1u); + EXPECT_EQ(result.components[0].description, "Engine control unit"); +} + +TEST_F(MergePipelineTest, FallbackVsEnrichmentEnrichmentWins) { + // ENRICHMENT > FALLBACK + Component comp1 = make_component("engine", "powertrain", "/powertrain"); + comp1.name = "Fallback Name"; + + Component comp2 = make_component("engine", "powertrain", "/powertrain"); + comp2.name = "Enrichment Name"; + + LayerOutput out1, out2; + out1.components.push_back(comp1); + out2.components.push_back(comp2); + + pipeline_.add_layer(std::make_unique( + "fallback", out1, std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::FALLBACK}})); + pipeline_.add_layer(std::make_unique( + "enrich", out2, std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::ENRICHMENT}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.components.size(), 1u); + EXPECT_EQ(result.components[0].name, "Enrichment Name"); // ENRICHMENT wins over FALLBACK +} + +// --- M6: MergeConflict struct fields --- + +TEST_F(MergePipelineTest, MergeConflictStructPopulated) { + // AUTH vs AUTH on same entity - verify conflict fields + Component comp1 = make_component("engine", "powertrain", "/powertrain"); + comp1.name = "Manifest Engine"; + + Component comp2 = make_component("engine", "powertrain", "/powertrain"); + comp2.name = "Runtime Engine"; + + LayerOutput out1, out2; + out1.components.push_back(comp1); + out2.components.push_back(comp2); + + pipeline_.add_layer(std::make_unique( + "manifest", out1, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}})); + pipeline_.add_layer(std::make_unique( + "runtime", out2, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}})); + + auto result = pipeline_.execute(); + ASSERT_GE(result.report.conflicts.size(), 1u); + + // Find the IDENTITY conflict for "engine" + bool found = false; + for (const auto & c : result.report.conflicts) { + if (c.entity_id == "engine" && c.field_group == FieldGroup::IDENTITY) { + EXPECT_EQ(c.winning_layer, "manifest"); + EXPECT_EQ(c.losing_layer, "runtime"); + found = true; + break; + } + } + EXPECT_TRUE(found) << "Expected IDENTITY conflict for entity 'engine'"; +} + +// --- m9: MergeReport::to_json() includes conflicts --- + +TEST(MergeTypesTest, MergeReportToJsonIncludesConflicts) { + MergeReport report; + report.layers = {"manifest", "runtime"}; + report.conflict_count = 1; + report.conflicts.push_back({"engine", FieldGroup::IDENTITY, "manifest", "runtime"}); + + auto j = report.to_json(); + ASSERT_TRUE(j.contains("conflicts")); + ASSERT_EQ(j["conflicts"].size(), 1u); + EXPECT_EQ(j["conflicts"][0]["entity_id"], "engine"); + EXPECT_EQ(j["conflicts"][0]["field_group"], "IDENTITY"); + EXPECT_EQ(j["conflicts"][0]["winning_layer"], "manifest"); + EXPECT_EQ(j["conflicts"][0]["losing_layer"], "runtime"); +} + +// --- Three-layer merge (manifest + runtime + plugin) --- + +// @verifies REQ_INTEROP_003 +TEST_F(MergePipelineTest, ThreeLayerMerge_PluginEnrichesManifestEntity) { + // Manifest: component "engine" with AUTHORITATIVE identity + Component manifest_comp = make_component("engine", "powertrain", "/powertrain"); + manifest_comp.name = "Engine ECU"; + manifest_comp.description = "Main engine controller"; + + // Runtime: component "engine" with FALLBACK identity, AUTHORITATIVE live_data (topics) + Component runtime_comp = make_component("engine", "powertrain", "/powertrain"); + runtime_comp.name = "engine"; // heuristic name, should not override manifest + ServiceInfo svc; + svc.name = "get_status"; + svc.full_path = "/powertrain/get_status"; + svc.type = "std_srvs/srv/Trigger"; + runtime_comp.services.push_back(svc); + + // Plugin: component "engine" with ENRICHMENT metadata - adds vendor extension + Component plugin_comp = make_component("engine", "powertrain", "/powertrain"); + plugin_comp.description = "Plugin-enriched description"; // IDENTITY: won't override manifest AUTH + plugin_comp.source = "vendor-plugin"; // METADATA: will fill empty field + + LayerOutput manifest_out, runtime_out, plugin_out; + manifest_out.components.push_back(manifest_comp); + runtime_out.components.push_back(runtime_comp); + plugin_out.components.push_back(plugin_comp); + + pipeline_.add_layer(std::make_unique( + "manifest", manifest_out, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}, + {FieldGroup::LIVE_DATA, MergePolicy::ENRICHMENT}, + {FieldGroup::METADATA, MergePolicy::AUTHORITATIVE}})); + pipeline_.add_layer(std::make_unique( + "runtime", runtime_out, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::FALLBACK}, + {FieldGroup::LIVE_DATA, MergePolicy::AUTHORITATIVE}})); + pipeline_.add_layer(std::make_unique( + "plugin", plugin_out, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::ENRICHMENT}, + {FieldGroup::LIVE_DATA, MergePolicy::ENRICHMENT}, + {FieldGroup::METADATA, MergePolicy::ENRICHMENT}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.components.size(), 1u); + + const auto & merged = result.components[0]; + // Name from manifest (AUTHORITATIVE IDENTITY wins over plugin ENRICHMENT) + EXPECT_EQ(merged.name, "Engine ECU"); + // Description from manifest (AUTHORITATIVE beats ENRICHMENT) + EXPECT_EQ(merged.description, "Main engine controller"); + // Services from runtime (AUTHORITATIVE LIVE_DATA) + EXPECT_EQ(merged.services.size(), 1u); + // entity_source tracks the first layer that introduced this entity + EXPECT_EQ(result.report.entity_source["engine"], "manifest"); +} + +// --- App STATUS field group merge --- + +// @verifies REQ_INTEROP_003 +TEST_F(MergePipelineTest, AppStatusMerge_BoolOrSemantics) { + // Two layers provide the same App with different is_online values + App app1 = make_app("controller", "nav_comp"); + app1.is_online = false; + + App app2 = make_app("controller", "nav_comp"); + app2.is_online = true; + app2.bound_fqn = "/nav/controller"; + + LayerOutput out1, out2; + out1.apps.push_back(app1); + out2.apps.push_back(app2); + + pipeline_.add_layer(std::make_unique( + "manifest", out1, std::unordered_map{{FieldGroup::STATUS, MergePolicy::FALLBACK}})); + pipeline_.add_layer(std::make_unique( + "runtime", out2, std::unordered_map{{FieldGroup::STATUS, MergePolicy::AUTHORITATIVE}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.apps.size(), 1u); + // Runtime STATUS is AUTHORITATIVE, so is_online=true and bound_fqn set + EXPECT_TRUE(result.apps[0].is_online); + EXPECT_EQ(result.apps[0].bound_fqn, "/nav/controller"); +} + +// @verifies REQ_INTEROP_003 +TEST_F(MergePipelineTest, ThreeLayerMerge_PerFieldGroupOwnerTracking) { + // Regression test: verify that when Runtime wins STATUS (AUTH) over Manifest (FALLBACK), + // a Plugin (ENRICH) cannot override Runtime's authoritative STATUS. + // Previously, owner_layer_idx was fixed to first layer, so Plugin compared against + // Manifest's FALLBACK policy instead of Runtime's AUTH, incorrectly winning STATUS. + App manifest_app = make_app("controller", "nav_comp"); + manifest_app.is_online = false; + + App runtime_app = make_app("controller", "nav_comp"); + runtime_app.is_online = true; + runtime_app.bound_fqn = "/nav/controller"; + + App plugin_app = make_app("controller", "nav_comp"); + plugin_app.is_online = false; // Plugin says offline - should NOT override Runtime's AUTH + plugin_app.bound_fqn = std::nullopt; + + LayerOutput manifest_out, runtime_out, plugin_out; + manifest_out.apps.push_back(manifest_app); + runtime_out.apps.push_back(runtime_app); + plugin_out.apps.push_back(plugin_app); + + pipeline_.add_layer(std::make_unique( + "manifest", manifest_out, + std::unordered_map{{FieldGroup::STATUS, MergePolicy::FALLBACK}, + {FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}})); + pipeline_.add_layer(std::make_unique( + "runtime", runtime_out, + std::unordered_map{{FieldGroup::STATUS, MergePolicy::AUTHORITATIVE}, + {FieldGroup::IDENTITY, MergePolicy::FALLBACK}})); + pipeline_.add_layer(std::make_unique( + "plugin", plugin_out, + std::unordered_map{{FieldGroup::STATUS, MergePolicy::ENRICHMENT}, + {FieldGroup::IDENTITY, MergePolicy::ENRICHMENT}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.apps.size(), 1u); + // Runtime's AUTH should win STATUS - plugin's ENRICH cannot override + EXPECT_TRUE(result.apps[0].is_online); + EXPECT_EQ(result.apps[0].bound_fqn, "/nav/controller"); +} + +// --- GapFillConfig namespace filtering --- +// These tests verify the namespace matching semantics used by RuntimeLayer. +// Since filter_by_namespace is internal to runtime_layer.cpp, we replicate the +// matching logic here to test the *semantics* of GapFillConfig lists. + +namespace { +// Mirrors the is_namespace_allowed() semantics in runtime_layer.cpp: +// path-segment boundary matching (ns == w || ns starts with w + "/"). +bool is_namespace_allowed(const std::string & ns, const GapFillConfig & config) { + if (!config.namespace_whitelist.empty()) { + bool found = + std::any_of(config.namespace_whitelist.begin(), config.namespace_whitelist.end(), [&ns](const std::string & w) { + return ns == w || ns.find(w + "/") == 0; + }); + if (!found) { + return false; + } + } + for (const auto & b : config.namespace_blacklist) { + if (ns == b || ns.find(b + "/") == 0) { + return false; + } + } + return true; +} +} // namespace + +// @verifies REQ_INTEROP_003 +TEST(GapFillConfigTest, NamespaceWhitelistFiltersAreas) { + GapFillConfig config; + config.namespace_whitelist = {"/robot"}; + + // Exact match passes + EXPECT_TRUE(is_namespace_allowed("/robot", config)); + // Child namespace passes (path-segment boundary) + EXPECT_TRUE(is_namespace_allowed("/robot/nav", config)); + // Different namespace blocked + EXPECT_FALSE(is_namespace_allowed("/sensor", config)); + // Prefix that is NOT a path segment boundary should be blocked + EXPECT_FALSE(is_namespace_allowed("/robotics", config)); +} + +// @verifies REQ_INTEROP_003 +TEST(GapFillConfigTest, NamespaceBlacklistFiltersAreas) { + GapFillConfig config; + config.namespace_blacklist = {"/rosout"}; + + // Not blacklisted + EXPECT_TRUE(is_namespace_allowed("/robot", config)); + // Exact match blocked + EXPECT_FALSE(is_namespace_allowed("/rosout", config)); + // Child namespace blocked (path-segment boundary) + EXPECT_FALSE(is_namespace_allowed("/rosout/sub", config)); + // Prefix that is NOT a path segment boundary should pass + EXPECT_TRUE(is_namespace_allowed("/rosoutput", config)); +} + +// --- Pipeline exception safety --- + +namespace { +class ThrowingLayer : public DiscoveryLayer { + public: + std::string name() const override { + return "throwing"; + } + LayerOutput discover() override { + throw std::runtime_error("plugin crash"); + } + MergePolicy policy_for(FieldGroup /*group*/) const override { + return MergePolicy::ENRICHMENT; + } +}; +} // namespace + +// @verifies REQ_INTEROP_003 +TEST_F(MergePipelineTest, LayerExceptionDoesNotCrashPipeline) { + // A good layer followed by a throwing layer - good layer's data should survive + LayerOutput good_output; + good_output.areas.push_back(make_area("powertrain")); + pipeline_.add_layer(std::make_unique("good", good_output)); + pipeline_.add_layer(std::make_unique()); + + auto result = pipeline_.execute(); + // Good layer's data should be present despite the throwing layer + ASSERT_EQ(result.areas.size(), 1u); + EXPECT_EQ(result.areas[0].id, "powertrain"); +} + +TEST_F(MergePipelineTest, FunctionMerge_HostsAndIdentity) { + // Layer 1 (AUTH for IDENTITY): function with name, no hosts + Function auth_func = make_function("diagnostics", "Diagnostics Suite"); + auth_func.source = "manifest"; + + LayerOutput auth_output; + auth_output.functions.push_back(auth_func); + + // Layer 2 (ENRICHMENT): function with hosts, different name + Function enrich_func = make_function("diagnostics", "diag_runtime"); + enrich_func.hosts = {"engine_ecu", "brake_controller"}; + enrich_func.source = "runtime"; + + LayerOutput enrich_output; + enrich_output.functions.push_back(enrich_func); + + pipeline_.add_layer(std::make_unique( + "manifest", auth_output, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}, + {FieldGroup::HIERARCHY, MergePolicy::ENRICHMENT}})); + + pipeline_.add_layer(std::make_unique( + "runtime", enrich_output, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::FALLBACK}, + {FieldGroup::HIERARCHY, MergePolicy::ENRICHMENT}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.functions.size(), 1u); + EXPECT_EQ(result.functions[0].name, "Diagnostics Suite"); // AUTH identity wins + EXPECT_EQ(result.functions[0].hosts.size(), 2u); // ENRICHMENT fills hosts + EXPECT_EQ(result.functions[0].source, "manifest"); // higher priority source +} + +// --- field_group_from_string / merge_policy_from_string parsing --- + +TEST(MergeTypesParsingTest, FieldGroupFromStringValid) { + EXPECT_EQ(field_group_from_string("identity"), FieldGroup::IDENTITY); + EXPECT_EQ(field_group_from_string("hierarchy"), FieldGroup::HIERARCHY); + EXPECT_EQ(field_group_from_string("live_data"), FieldGroup::LIVE_DATA); + EXPECT_EQ(field_group_from_string("status"), FieldGroup::STATUS); + EXPECT_EQ(field_group_from_string("metadata"), FieldGroup::METADATA); +} + +TEST(MergeTypesParsingTest, FieldGroupFromStringInvalid) { + EXPECT_EQ(field_group_from_string(""), std::nullopt); + EXPECT_EQ(field_group_from_string("IDENTITY"), std::nullopt); + EXPECT_EQ(field_group_from_string("Identity"), std::nullopt); + EXPECT_EQ(field_group_from_string("unknown"), std::nullopt); + EXPECT_EQ(field_group_from_string("live-data"), std::nullopt); +} + +TEST(MergeTypesParsingTest, MergePolicyFromStringValid) { + EXPECT_EQ(merge_policy_from_string("authoritative"), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(merge_policy_from_string("enrichment"), MergePolicy::ENRICHMENT); + EXPECT_EQ(merge_policy_from_string("fallback"), MergePolicy::FALLBACK); +} + +TEST(MergeTypesParsingTest, MergePolicyFromStringInvalid) { + EXPECT_EQ(merge_policy_from_string(""), std::nullopt); + EXPECT_EQ(merge_policy_from_string("AUTHORITATIVE"), std::nullopt); + EXPECT_EQ(merge_policy_from_string("auth"), std::nullopt); + EXPECT_EQ(merge_policy_from_string("unknown"), std::nullopt); +} + +// --- ManifestLayer / RuntimeLayer set_policy override --- + +TEST(LayerPolicyOverrideTest, ManifestLayerSetPolicyOverridesDefault) { + ManifestLayer layer(nullptr); + + // Default: LIVE_DATA = ENRICHMENT + EXPECT_EQ(layer.policy_for(FieldGroup::LIVE_DATA), MergePolicy::ENRICHMENT); + + // Override to AUTHORITATIVE + layer.set_policy(FieldGroup::LIVE_DATA, MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::LIVE_DATA), MergePolicy::AUTHORITATIVE); + + // Other policies unchanged + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::STATUS), MergePolicy::FALLBACK); +} + +TEST(LayerPolicyOverrideTest, RuntimeLayerSetPolicyOverridesDefault) { + RuntimeLayer layer(nullptr); + + // Default: IDENTITY = FALLBACK + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::FALLBACK); + + // Override to AUTHORITATIVE + layer.set_policy(FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::AUTHORITATIVE); + + // Other policies unchanged + EXPECT_EQ(layer.policy_for(FieldGroup::LIVE_DATA), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::METADATA), MergePolicy::ENRICHMENT); +} + +TEST(LayerPolicyOverrideTest, PluginLayerSetPolicyOverridesDefault) { + PluginLayer layer("test_plugin", nullptr); + + // Default: all ENRICHMENT + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::ENRICHMENT); + + // Override IDENTITY to AUTHORITATIVE + layer.set_policy(FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::IDENTITY), MergePolicy::AUTHORITATIVE); + EXPECT_EQ(layer.policy_for(FieldGroup::HIERARCHY), MergePolicy::ENRICHMENT); +} + +// --- Policy override affects merge behavior end-to-end --- + +TEST_F(MergePipelineTest, PolicyOverrideChangedMergeBehavior) { + // Manifest layer with LIVE_DATA overridden from ENRICHMENT to AUTHORITATIVE + // This means manifest topics should win over runtime topics + LayerOutput manifest_output; + manifest_output.areas.push_back(make_area("powertrain")); + + Component manifest_comp; + manifest_comp.id = "engine"; + manifest_comp.area = "powertrain"; + manifest_comp.source = "manifest"; + manifest_comp.topics.publishes = {"manifest_topic"}; + manifest_output.components.push_back(manifest_comp); + + // Runtime layer provides same component with different topics + Component runtime_comp; + runtime_comp.id = "engine"; + runtime_comp.area = "powertrain"; + runtime_comp.source = "runtime"; + runtime_comp.topics.publishes = {"runtime_topic"}; + + LayerOutput runtime_output; + runtime_output.components.push_back(runtime_comp); + + // Use TestLayers with actual ManifestLayer/RuntimeLayer default policies + override + pipeline_.add_layer(std::make_unique( + "manifest", manifest_output, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::AUTHORITATIVE}, + {FieldGroup::HIERARCHY, MergePolicy::AUTHORITATIVE}, + {FieldGroup::LIVE_DATA, MergePolicy::AUTHORITATIVE}, // overridden! + {FieldGroup::STATUS, MergePolicy::FALLBACK}, + {FieldGroup::METADATA, MergePolicy::AUTHORITATIVE}})); + + pipeline_.add_layer(std::make_unique( + "runtime", runtime_output, + std::unordered_map{{FieldGroup::IDENTITY, MergePolicy::FALLBACK}, + {FieldGroup::HIERARCHY, MergePolicy::FALLBACK}, + {FieldGroup::LIVE_DATA, MergePolicy::AUTHORITATIVE}, + {FieldGroup::STATUS, MergePolicy::AUTHORITATIVE}, + {FieldGroup::METADATA, MergePolicy::ENRICHMENT}})); + + auto result = pipeline_.execute(); + ASSERT_EQ(result.components.size(), 1u); + + // With both layers AUTH for LIVE_DATA, manifest (higher priority) wins + // This means we get a conflict but manifest topics survive + EXPECT_EQ(result.components[0].source, "manifest"); + EXPECT_GE(result.report.conflict_count, 1u); +} diff --git a/src/ros2_medkit_gateway/test/test_runtime_linker.cpp b/src/ros2_medkit_gateway/test/test_runtime_linker.cpp index 7a0a1d18..2e60eae9 100644 --- a/src/ros2_medkit_gateway/test/test_runtime_linker.cpp +++ b/src/ros2_medkit_gateway/test/test_runtime_linker.cpp @@ -40,15 +40,29 @@ class RuntimeLinkerTest : public ::testing::Test { return app; } - // Helper to create a test Component (representing a runtime node) - Component create_component(const std::string & id, const std::string & ns = "/") { - Component comp; - comp.id = id; - comp.name = id; - comp.namespace_path = ns; - comp.fqn = ns == "/" ? "/" + id : ns + "/" + id; - comp.source = "node"; - return comp; + // Helper to create a test App with topic namespace binding + App create_topic_app(const std::string & id, const std::string & topic_ns) { + App app; + app.id = id; + app.name = id; + app.source = "manifest"; + + App::RosBinding binding; + binding.topic_namespace = topic_ns; + app.ros_binding = binding; + + return app; + } + + // Helper to create a runtime App (representing a discovered ROS 2 node) + App create_runtime_app(const std::string & id, const std::string & ns = "/") { + App app; + app.id = id; + app.name = id; + app.source = "heuristic"; + app.is_online = true; + app.bound_fqn = ns == "/" ? "/" + id : ns + "/" + id; + return app; } std::unique_ptr linker_; @@ -59,11 +73,12 @@ class RuntimeLinkerTest : public ::testing::Test { // Exact Match Tests // ============================================================================= +// @verifies REQ_INTEROP_003 TEST_F(RuntimeLinkerTest, ExactMatch_NodeNameAndNamespace) { std::vector apps = {create_app("controller_app", "controller", "/nav")}; - std::vector components = {create_component("controller", "/nav")}; + std::vector runtime_apps = {create_runtime_app("controller", "/nav")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_EQ(result.linked_apps.size(), 1); EXPECT_TRUE(result.linked_apps[0].is_online); @@ -76,9 +91,9 @@ TEST_F(RuntimeLinkerTest, ExactMatch_NodeNameAndNamespace) { TEST_F(RuntimeLinkerTest, ExactMatch_RootNamespace) { std::vector apps = {create_app("my_app", "my_node", "/")}; - std::vector components = {create_component("my_node", "/")}; + std::vector runtime_apps = {create_runtime_app("my_node", "/")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_EQ(result.linked_apps.size(), 1); EXPECT_TRUE(result.linked_apps[0].is_online); @@ -87,9 +102,9 @@ TEST_F(RuntimeLinkerTest, ExactMatch_RootNamespace) { TEST_F(RuntimeLinkerTest, NoMatch_DifferentNodeName) { std::vector apps = {create_app("app1", "controller", "/nav")}; - std::vector components = {create_component("planner", "/nav")}; + std::vector runtime_apps = {create_runtime_app("planner", "/nav")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_EQ(result.linked_apps.size(), 1); EXPECT_FALSE(result.linked_apps[0].is_online); @@ -100,9 +115,9 @@ TEST_F(RuntimeLinkerTest, NoMatch_DifferentNodeName) { TEST_F(RuntimeLinkerTest, NoMatch_DifferentNamespace) { std::vector apps = {create_app("app1", "controller", "/navigation")}; - std::vector components = {create_component("controller", "/planning")}; + std::vector runtime_apps = {create_runtime_app("controller", "/planning")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_FALSE(result.linked_apps[0].is_online); EXPECT_EQ(result.unlinked_app_ids.size(), 1); @@ -114,12 +129,12 @@ TEST_F(RuntimeLinkerTest, NoMatch_DifferentNamespace) { TEST_F(RuntimeLinkerTest, WildcardNamespace_MatchesAny) { std::vector apps = {create_app("app1", "controller", "*")}; - std::vector components = { - create_component("controller", "/ns1"), - create_component("controller", "/ns2"), + std::vector runtime_apps = { + create_runtime_app("controller", "/ns1"), + create_runtime_app("controller", "/ns2"), }; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.linked_apps[0].is_online); // Should match the first one found @@ -130,9 +145,9 @@ TEST_F(RuntimeLinkerTest, WildcardNamespace_MatchesAny) { TEST_F(RuntimeLinkerTest, WildcardNamespace_MatchesRootNamespace) { std::vector apps = {create_app("app1", "my_node", "*")}; - std::vector components = {create_component("my_node", "/")}; + std::vector runtime_apps = {create_runtime_app("my_node", "/")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.linked_apps[0].is_online); EXPECT_EQ(result.linked_apps[0].bound_fqn, "/my_node"); @@ -140,9 +155,9 @@ TEST_F(RuntimeLinkerTest, WildcardNamespace_MatchesRootNamespace) { TEST_F(RuntimeLinkerTest, WildcardNamespace_MatchesNestedNamespace) { std::vector apps = {create_app("app1", "controller", "*")}; - std::vector components = {create_component("controller", "/robot/nav/local")}; + std::vector runtime_apps = {create_runtime_app("controller", "/robot/nav/local")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.linked_apps[0].is_online); EXPECT_EQ(result.linked_apps[0].bound_fqn, "/robot/nav/local/controller"); @@ -160,13 +175,13 @@ TEST_F(RuntimeLinkerTest, TopicNamespace_MatchesByPublisher) { binding.topic_namespace = "/sensor_data"; app.ros_binding = binding; - Component comp = create_component("sensor_driver", "/"); - comp.topics.publishes = {"/sensor_data/imu", "/sensor_data/gps"}; + App rt_app = create_runtime_app("sensor_driver", "/"); + rt_app.topics.publishes = {"/sensor_data/imu", "/sensor_data/gps"}; std::vector apps = {app}; - std::vector components = {comp}; + std::vector runtime_apps = {rt_app}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.linked_apps[0].is_online); EXPECT_EQ(result.linked_apps[0].bound_fqn, "/sensor_driver"); @@ -180,13 +195,13 @@ TEST_F(RuntimeLinkerTest, TopicNamespace_MatchesBySubscriber) { binding.topic_namespace = "/cmd"; app.ros_binding = binding; - Component comp = create_component("motor_driver", "/"); - comp.topics.subscribes = {"/cmd/velocity", "/cmd/position"}; + App rt_app = create_runtime_app("motor_driver", "/"); + rt_app.topics.subscribes = {"/cmd/velocity", "/cmd/position"}; std::vector apps = {app}; - std::vector components = {comp}; + std::vector runtime_apps = {rt_app}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.linked_apps[0].is_online); } @@ -199,13 +214,13 @@ TEST_F(RuntimeLinkerTest, TopicNamespace_NoMatch) { binding.topic_namespace = "/navigation"; app.ros_binding = binding; - Component comp = create_component("sensor_driver", "/"); - comp.topics.publishes = {"/sensor/imu"}; + App rt_app = create_runtime_app("sensor_driver", "/"); + rt_app.topics.publishes = {"/sensor/imu"}; std::vector apps = {app}; - std::vector components = {comp}; + std::vector runtime_apps = {rt_app}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_FALSE(result.linked_apps[0].is_online); } @@ -216,13 +231,13 @@ TEST_F(RuntimeLinkerTest, TopicNamespace_NoMatch) { TEST_F(RuntimeLinkerTest, OrphanNodes_DetectedCorrectly) { std::vector apps = {create_app("app1", "controller", "/nav")}; - std::vector components = { - create_component("controller", "/nav"), - create_component("planner", "/nav"), - create_component("mapper", "/map"), + std::vector runtime_apps = { + create_runtime_app("controller", "/nav"), + create_runtime_app("planner", "/nav"), + create_runtime_app("mapper", "/map"), }; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_EQ(result.orphan_nodes.size(), 2); EXPECT_TRUE(std::find(result.orphan_nodes.begin(), result.orphan_nodes.end(), "/nav/planner") != @@ -236,12 +251,12 @@ TEST_F(RuntimeLinkerTest, OrphanNodes_AllMatched) { create_app("app1", "node1", "*"), create_app("app2", "node2", "*"), }; - std::vector components = { - create_component("node1", "/"), - create_component("node2", "/"), + std::vector runtime_apps = { + create_runtime_app("node1", "/"), + create_runtime_app("node2", "/"), }; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.orphan_nodes.empty()); EXPECT_EQ(result.app_to_node.size(), 2); @@ -255,9 +270,9 @@ TEST_F(RuntimeLinkerTest, OrphanPolicy_Error_ReportsError) { config_.unmanifested_nodes = ManifestConfig::UnmanifestedNodePolicy::ERROR; std::vector apps = {}; - std::vector components = {create_component("orphan_node", "/")}; + std::vector runtime_apps = {create_runtime_app("orphan_node", "/")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.has_errors(config_.unmanifested_nodes)); EXPECT_EQ(result.orphan_nodes.size(), 1); @@ -267,11 +282,39 @@ TEST_F(RuntimeLinkerTest, OrphanPolicy_Ignore_NoError) { config_.unmanifested_nodes = ManifestConfig::UnmanifestedNodePolicy::IGNORE; std::vector apps = {}; - std::vector components = {create_component("orphan_node", "/")}; + std::vector runtime_apps = {create_runtime_app("orphan_node", "/")}; + + auto result = linker_->link(apps, runtime_apps, config_); + + EXPECT_FALSE(result.has_errors(config_.unmanifested_nodes)); +} + +// @verifies REQ_INTEROP_003 +TEST_F(RuntimeLinkerTest, OrphanPolicy_Warn_NoError) { + config_.unmanifested_nodes = ManifestConfig::UnmanifestedNodePolicy::WARN; + + std::vector apps = {}; + std::vector runtime_apps = {create_runtime_app("orphan_node", "/")}; + + auto result = linker_->link(apps, runtime_apps, config_); + + // WARN logs warnings but does not fail + EXPECT_FALSE(result.has_errors(config_.unmanifested_nodes)); + EXPECT_EQ(result.orphan_nodes.size(), 1u); +} + +// @verifies REQ_INTEROP_003 +TEST_F(RuntimeLinkerTest, OrphanPolicy_IncludeAsOrphan_NoError) { + config_.unmanifested_nodes = ManifestConfig::UnmanifestedNodePolicy::INCLUDE_AS_ORPHAN; + + std::vector apps = {}; + std::vector runtime_apps = {create_runtime_app("orphan_node", "/")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); + // INCLUDE_AS_ORPHAN includes orphans but does not fail EXPECT_FALSE(result.has_errors(config_.unmanifested_nodes)); + EXPECT_EQ(result.orphan_nodes.size(), 1u); } // ============================================================================= @@ -281,13 +324,13 @@ TEST_F(RuntimeLinkerTest, OrphanPolicy_Ignore_NoError) { TEST_F(RuntimeLinkerTest, EnrichApp_CopiesTopics) { std::vector apps = {create_app("app1", "sensor", "*")}; - Component comp = create_component("sensor", "/"); - comp.topics.publishes = {"/sensor/data", "/sensor/status"}; - comp.topics.subscribes = {"/sensor/config"}; + App rt_app = create_runtime_app("sensor", "/"); + rt_app.topics.publishes = {"/sensor/data", "/sensor/status"}; + rt_app.topics.subscribes = {"/sensor/config"}; - std::vector components = {comp}; + std::vector runtime_apps = {rt_app}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.linked_apps[0].is_online); EXPECT_EQ(result.linked_apps[0].topics.publishes.size(), 2); @@ -297,13 +340,13 @@ TEST_F(RuntimeLinkerTest, EnrichApp_CopiesTopics) { TEST_F(RuntimeLinkerTest, EnrichApp_CopiesServices) { std::vector apps = {create_app("app1", "server", "*")}; - Component comp = create_component("server", "/"); - comp.services = {{"srv1", "/srv1", "std_srvs/srv/Trigger", std::nullopt}, - {"srv2", "/srv2", "std_srvs/srv/Empty", std::nullopt}}; + App rt_app = create_runtime_app("server", "/"); + rt_app.services = {{"srv1", "/srv1", "std_srvs/srv/Trigger", std::nullopt}, + {"srv2", "/srv2", "std_srvs/srv/Empty", std::nullopt}}; - std::vector components = {comp}; + std::vector runtime_apps = {rt_app}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.linked_apps[0].is_online); EXPECT_EQ(result.linked_apps[0].services.size(), 2); @@ -312,12 +355,12 @@ TEST_F(RuntimeLinkerTest, EnrichApp_CopiesServices) { TEST_F(RuntimeLinkerTest, EnrichApp_CopiesActions) { std::vector apps = {create_app("app1", "action_server", "*")}; - Component comp = create_component("action_server", "/"); - comp.actions = {{"nav", "/nav", "nav2_msgs/action/NavigateToPose", std::nullopt}}; + App rt_app = create_runtime_app("action_server", "/"); + rt_app.actions = {{"nav", "/nav", "nav2_msgs/action/NavigateToPose", std::nullopt}}; - std::vector components = {comp}; + std::vector runtime_apps = {rt_app}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(result.linked_apps[0].is_online); EXPECT_EQ(result.linked_apps[0].actions.size(), 1); @@ -334,9 +377,9 @@ TEST_F(RuntimeLinkerTest, ExternalApp_NotLinked) { app.external = true; std::vector apps = {app}; - std::vector components = {create_component("some_node", "/")}; + std::vector runtime_apps = {create_runtime_app("some_node", "/")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_EQ(result.linked_apps.size(), 1); EXPECT_FALSE(result.linked_apps[0].is_online); @@ -355,9 +398,9 @@ TEST_F(RuntimeLinkerTest, NoBinding_GoesToUnlinked) { // No ros_binding set std::vector apps = {app}; - std::vector components = {create_component("some_node", "/")}; + std::vector runtime_apps = {create_runtime_app("some_node", "/")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_FALSE(result.linked_apps[0].is_online); EXPECT_EQ(result.unlinked_app_ids.size(), 1); @@ -371,9 +414,9 @@ TEST_F(RuntimeLinkerTest, EmptyBinding_GoesToUnlinked) { app.ros_binding = App::RosBinding{}; // Empty binding std::vector apps = {app}; - std::vector components = {create_component("some_node", "/")}; + std::vector runtime_apps = {create_runtime_app("some_node", "/")}; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_FALSE(result.linked_apps[0].is_online); EXPECT_EQ(result.unlinked_app_ids.size(), 1); @@ -388,9 +431,9 @@ TEST_F(RuntimeLinkerTest, IsAppOnline_AfterLinking) { create_app("online_app", "online_node", "*"), create_app("offline_app", "missing_node", "*"), }; - std::vector components = {create_component("online_node", "/")}; + std::vector runtime_apps = {create_runtime_app("online_node", "/")}; - linker_->link(apps, components, config_); + linker_->link(apps, runtime_apps, config_); EXPECT_TRUE(linker_->is_app_online("online_app")); EXPECT_FALSE(linker_->is_app_online("offline_app")); @@ -399,9 +442,9 @@ TEST_F(RuntimeLinkerTest, IsAppOnline_AfterLinking) { TEST_F(RuntimeLinkerTest, GetBoundNode_ReturnsCorrectFqn) { std::vector apps = {create_app("app1", "my_node", "/ns")}; - std::vector components = {create_component("my_node", "/ns")}; + std::vector runtime_apps = {create_runtime_app("my_node", "/ns")}; - linker_->link(apps, components, config_); + linker_->link(apps, runtime_apps, config_); auto bound = linker_->get_bound_node("app1"); ASSERT_TRUE(bound.has_value()); @@ -412,9 +455,9 @@ TEST_F(RuntimeLinkerTest, GetBoundNode_ReturnsCorrectFqn) { TEST_F(RuntimeLinkerTest, GetAppForNode_ReturnsCorrectId) { std::vector apps = {create_app("app1", "my_node", "/ns")}; - std::vector components = {create_component("my_node", "/ns")}; + std::vector runtime_apps = {create_runtime_app("my_node", "/ns")}; - linker_->link(apps, components, config_); + linker_->link(apps, runtime_apps, config_); auto app_id = linker_->get_app_for_node("/ns/my_node"); ASSERT_TRUE(app_id.has_value()); @@ -433,13 +476,13 @@ TEST_F(RuntimeLinkerTest, MultipleApps_AllLinked) { create_app("app2", "node2", "*"), create_app("app3", "node3", "*"), }; - std::vector components = { - create_component("node1", "/"), - create_component("node2", "/"), - create_component("node3", "/"), + std::vector runtime_apps = { + create_runtime_app("node1", "/"), + create_runtime_app("node2", "/"), + create_runtime_app("node3", "/"), }; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_EQ(result.linked_apps.size(), 3); for (const auto & app : result.linked_apps) { @@ -456,12 +499,12 @@ TEST_F(RuntimeLinkerTest, MultipleApps_SomeUnlinked) { create_app("app3", "node3", "*"), create_app("app4", "missing2", "*"), }; - std::vector components = { - create_component("node1", "/"), - create_component("node3", "/"), + std::vector runtime_apps = { + create_runtime_app("node1", "/"), + create_runtime_app("node3", "/"), }; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); EXPECT_EQ(result.linked_apps.size(), 4); EXPECT_EQ(result.app_to_node.size(), 2); @@ -477,12 +520,12 @@ TEST_F(RuntimeLinkerTest, ResultSummary_FormatsCorrectly) { create_app("app1", "node1", "*"), create_app("app2", "missing", "*"), }; - std::vector components = { - create_component("node1", "/"), - create_component("orphan", "/"), + std::vector runtime_apps = { + create_runtime_app("node1", "/"), + create_runtime_app("orphan", "/"), }; - auto result = linker_->link(apps, components, config_); + auto result = linker_->link(apps, runtime_apps, config_); std::string summary = result.summary(); EXPECT_TRUE(summary.find("1 linked") != std::string::npos); @@ -492,14 +535,128 @@ TEST_F(RuntimeLinkerTest, ResultSummary_FormatsCorrectly) { TEST_F(RuntimeLinkerTest, GetLastResult_ReturnsLatest) { std::vector apps = {create_app("app1", "node1", "*")}; - std::vector components = {create_component("node1", "/")}; + std::vector runtime_apps = {create_runtime_app("node1", "/")}; - linker_->link(apps, components, config_); + linker_->link(apps, runtime_apps, config_); const auto & last = linker_->get_last_result(); EXPECT_EQ(last.app_to_node.size(), 1); } +// ============================================================================= +// Namespace Matching Determinism Tests (Task 16) +// ============================================================================= + +TEST_F(RuntimeLinkerTest, NamespaceMatch_RejectsStringPrefix) { + // "/nav" should NOT match node in "/navigation" namespace + std::vector apps = {create_app("nav_app", "navigator", "/nav")}; + std::vector runtime_apps = {create_runtime_app("navigator", "/navigation")}; + + auto result = linker_->link(apps, runtime_apps, config_); + EXPECT_FALSE(result.linked_apps[0].is_online); +} + +TEST_F(RuntimeLinkerTest, NamespaceMatch_AcceptsPathPrefix) { + // "/nav" SHOULD match node in "/nav/sub" namespace + std::vector apps = {create_app("nav_app", "planner", "/nav")}; + std::vector runtime_apps = {create_runtime_app("planner", "/nav/sub")}; + + auto result = linker_->link(apps, runtime_apps, config_); + EXPECT_TRUE(result.linked_apps[0].is_online); +} + +TEST_F(RuntimeLinkerTest, NodeName_ExactLastSegmentOnly) { + // Binding for "map" should NOT match node "map_server" (FQN contains "/map") + std::vector apps = {create_app("mapper", "map", "/")}; + std::vector runtime_apps = {create_runtime_app("map_server", "/")}; + + auto result = linker_->link(apps, runtime_apps, config_); + EXPECT_FALSE(result.linked_apps[0].is_online); +} + +TEST_F(RuntimeLinkerTest, Wildcard_DeterministicMultiMatch) { + // Wildcard: two nodes match by name, deterministic winner (alphabetical FQN) + std::vector apps = {create_app("ctrl_app", "controller", "*")}; + std::vector runtime_apps = { + create_runtime_app("controller", "/beta"), + create_runtime_app("controller", "/alpha"), + }; + + auto result = linker_->link(apps, runtime_apps, config_); + EXPECT_TRUE(result.linked_apps[0].is_online); + // Deterministic: alphabetically first FQN wins + EXPECT_EQ(result.linked_apps[0].bound_fqn, "/alpha/controller"); +} + +TEST_F(RuntimeLinkerTest, TopicNamespace_RejectsStringPrefix) { + // Topic namespace "/state" should NOT match topic "/statement/data" + auto app = create_topic_app("state_app", "/state"); + App rt_app = create_runtime_app("some_node", "/"); + rt_app.topics.publishes = {"/statement/data"}; + + auto result = linker_->link({app}, {rt_app}, config_); + EXPECT_FALSE(result.linked_apps[0].is_online); +} + +TEST_F(RuntimeLinkerTest, TopicNamespace_AcceptsPathPrefix) { + // Topic namespace "/state" SHOULD match topic "/state/machine" + auto app = create_topic_app("state_app", "/state"); + App rt_app = create_runtime_app("some_node", "/"); + rt_app.topics.publishes = {"/state/machine"}; + + auto result = linker_->link({app}, {rt_app}, config_); + EXPECT_TRUE(result.linked_apps[0].is_online); +} + +// ============================================================================= +// Multi-match and Binding Conflict Tests (Task 17) +// ============================================================================= + +TEST_F(RuntimeLinkerTest, TwoAppsCompeteForSameNode) { + // Two manifest apps bind to the same runtime node + std::vector apps = { + create_app("app1", "controller", "/nav"), + create_app("app2", "controller", "/nav"), + }; + std::vector runtime_apps = {create_runtime_app("controller", "/nav")}; + + auto result = linker_->link(apps, runtime_apps, config_); + + // First app wins (insertion order = priority) + EXPECT_TRUE(result.linked_apps[0].is_online); + EXPECT_EQ(result.linked_apps[0].bound_fqn, "/nav/controller"); + + // Second app is unlinked (node already taken) + EXPECT_FALSE(result.linked_apps[1].is_online); + EXPECT_EQ(result.unlinked_app_ids.size(), 1u); + + // Conflict reported + EXPECT_GE(result.binding_conflicts, 1u); +} + +TEST_F(RuntimeLinkerTest, LinkingReportSummaryIncludesConflicts) { + std::vector apps = { + create_app("app1", "controller", "/nav"), + create_app("app2", "controller", "/nav"), + }; + std::vector runtime_apps = {create_runtime_app("controller", "/nav")}; + + auto result = linker_->link(apps, runtime_apps, config_); + auto summary = result.summary(); + EXPECT_TRUE(summary.find("conflict") != std::string::npos); +} + +TEST_F(RuntimeLinkerTest, WildcardMultiMatchCounted) { + std::vector apps = {create_app("ctrl_app", "controller", "*")}; + std::vector runtime_apps = { + create_runtime_app("controller", "/alpha"), + create_runtime_app("controller", "/beta"), + }; + + auto result = linker_->link(apps, runtime_apps, config_); + EXPECT_EQ(result.wildcard_multi_match, 1u); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/src/ros2_medkit_integration_tests/CMakeLists.txt b/src/ros2_medkit_integration_tests/CMakeLists.txt index a6d80497..715652f0 100644 --- a/src/ros2_medkit_integration_tests/CMakeLists.txt +++ b/src/ros2_medkit_integration_tests/CMakeLists.txt @@ -79,6 +79,11 @@ install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) if(BUILD_TESTING) find_package(launch_testing_ament_cmake REQUIRED) + # Each integration test gets a unique HTTP port via GATEWAY_TEST_PORT env var, + # allowing parallel CTest execution without port conflicts. + # Stride of 10 per test so multi-gateway tests can use get_test_port(offset). + set(_INTEG_PORT 9100) + # Feature tests (atomic, independent, 120s timeout) file(GLOB FEATURE_TESTS test/features/*.test.py) foreach(test_file ${FEATURE_TESTS}) @@ -87,7 +92,11 @@ if(BUILD_TESTING) TARGET ${test_name} TIMEOUT 120 ) - set_tests_properties(${test_name} PROPERTIES LABELS "integration;feature") + set_tests_properties(${test_name} PROPERTIES + LABELS "integration;feature" + ENVIRONMENT "GATEWAY_TEST_PORT=${_INTEG_PORT}" + ) + math(EXPR _INTEG_PORT "${_INTEG_PORT} + 10") endforeach() # Scenario tests (E2E stories, 300s timeout) @@ -98,7 +107,11 @@ if(BUILD_TESTING) TARGET ${test_name} TIMEOUT 300 ) - set_tests_properties(${test_name} PROPERTIES LABELS "integration;scenario") + set_tests_properties(${test_name} PROPERTIES + LABELS "integration;scenario" + ENVIRONMENT "GATEWAY_TEST_PORT=${_INTEG_PORT}" + ) + math(EXPR _INTEG_PORT "${_INTEG_PORT} + 10") endforeach() endif() diff --git a/src/ros2_medkit_integration_tests/ros2_medkit_test_utils/constants.py b/src/ros2_medkit_integration_tests/ros2_medkit_test_utils/constants.py index 51a4fd80..b42ec735 100644 --- a/src/ros2_medkit_integration_tests/ros2_medkit_test_utils/constants.py +++ b/src/ros2_medkit_integration_tests/ros2_medkit_test_utils/constants.py @@ -14,10 +14,23 @@ """Shared constants for ros2_medkit integration tests.""" +import os + API_BASE_PATH = '/api/v1' -DEFAULT_PORT = 8080 +DEFAULT_PORT = int(os.environ.get('GATEWAY_TEST_PORT', '8080')) DEFAULT_BASE_URL = f'http://localhost:{DEFAULT_PORT}{API_BASE_PATH}' + +def get_test_port(offset=0): + """Return the assigned test port plus an optional offset. + + Each integration test gets a unique ``GATEWAY_TEST_PORT`` from CMake. + Tests that launch multiple gateway instances use *offset* to get + additional non-colliding ports (e.g. ``get_test_port(1)``). + """ + return DEFAULT_PORT + offset + + # Gateway startup GATEWAY_STARTUP_TIMEOUT = 30.0 GATEWAY_STARTUP_INTERVAL = 0.5 diff --git a/src/ros2_medkit_integration_tests/ros2_medkit_test_utils/launch_helpers.py b/src/ros2_medkit_integration_tests/ros2_medkit_test_utils/launch_helpers.py index 4ab89528..da9caf88 100644 --- a/src/ros2_medkit_integration_tests/ros2_medkit_test_utils/launch_helpers.py +++ b/src/ros2_medkit_integration_tests/ros2_medkit_test_utils/launch_helpers.py @@ -95,9 +95,7 @@ def create_gateway_node(*, port=DEFAULT_PORT, extra_params=None, coverage=True): Ready-to-use gateway node launch action. """ - params = {'refresh_interval_ms': 1000} - if port != DEFAULT_PORT: - params['server_port'] = port + params = {'refresh_interval_ms': 1000, 'server.port': port} if extra_params: params.update(extra_params) diff --git a/src/ros2_medkit_integration_tests/test/features/test_auth.test.py b/src/ros2_medkit_integration_tests/test/features/test_auth.test.py index 3638a95d..d16abd93 100644 --- a/src/ros2_medkit_integration_tests/test/features/test_auth.test.py +++ b/src/ros2_medkit_integration_tests/test/features/test_auth.test.py @@ -31,11 +31,15 @@ import pytest import requests -from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES, API_BASE_PATH +from ros2_medkit_test_utils.constants import ( + ALLOWED_EXIT_CODES, + API_BASE_PATH, + get_test_port, +) from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase from ros2_medkit_test_utils.launch_helpers import create_gateway_node -AUTH_PORT = 8085 +AUTH_PORT = get_test_port() AUTH_BASE_URL = f'http://127.0.0.1:{AUTH_PORT}{API_BASE_PATH}' @@ -46,7 +50,6 @@ def generate_test_description(): port=AUTH_PORT, extra_params={ 'server.host': '127.0.0.1', - 'server.port': AUTH_PORT, 'auth.enabled': True, 'auth.jwt_secret': 'test_secret_key_for_jwt_signing_integration_test_12345', 'auth.jwt_algorithm': 'HS256', diff --git a/src/ros2_medkit_integration_tests/test/features/test_cors.test.py b/src/ros2_medkit_integration_tests/test/features/test_cors.test.py index 5370099c..7d434dc3 100644 --- a/src/ros2_medkit_integration_tests/test/features/test_cors.test.py +++ b/src/ros2_medkit_integration_tests/test/features/test_cors.test.py @@ -39,14 +39,14 @@ import launch_testing.actions import requests -from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES, API_BASE_PATH +from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES, API_BASE_PATH, get_test_port from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase from ros2_medkit_test_utils.launch_helpers import create_gateway_node # Test configuration - two gateway instances with different CORS settings GATEWAY_HOST = '127.0.0.1' -GATEWAY_PORT_NO_CREDS = 8085 # CORS without credentials -GATEWAY_PORT_WITH_CREDS = 8086 # CORS with credentials enabled +GATEWAY_PORT_NO_CREDS = get_test_port(0) +GATEWAY_PORT_WITH_CREDS = get_test_port(1) ALLOWED_ORIGIN = 'http://localhost:5173' ALLOWED_ORIGIN_2 = 'http://localhost:3000' DISALLOWED_ORIGIN = 'http://evil.com' @@ -59,7 +59,6 @@ def generate_test_description(): port=GATEWAY_PORT_NO_CREDS, extra_params={ 'server.host': GATEWAY_HOST, - 'server.port': GATEWAY_PORT_NO_CREDS, 'cors.allowed_origins': [ALLOWED_ORIGIN, ALLOWED_ORIGIN_2], 'cors.allowed_methods': ['GET', 'PUT', 'OPTIONS'], 'cors.allowed_headers': ['Content-Type', 'Accept', 'Authorization'], @@ -73,7 +72,6 @@ def generate_test_description(): port=GATEWAY_PORT_WITH_CREDS, extra_params={ 'server.host': GATEWAY_HOST, - 'server.port': GATEWAY_PORT_WITH_CREDS, 'cors.allowed_origins': [ALLOWED_ORIGIN], 'cors.allowed_methods': ['GET', 'PUT', 'OPTIONS'], 'cors.allowed_headers': ['Content-Type', 'Accept', 'Authorization'], diff --git a/src/ros2_medkit_integration_tests/test/features/test_discovery_gap_fill.test.py b/src/ros2_medkit_integration_tests/test/features/test_discovery_gap_fill.test.py new file mode 100644 index 00000000..ebaa11e4 --- /dev/null +++ b/src/ros2_medkit_integration_tests/test/features/test_discovery_gap_fill.test.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python3 +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Integration tests for hybrid mode gap-fill configuration. + +Tests that gap-fill controls restrict which heuristic entities the +runtime layer can create when a manifest is present. Also tests +namespace blacklist/whitelist filtering. +""" + +import os +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import TimerAction +import launch_testing +import launch_testing.actions + +from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES +from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase +from ros2_medkit_test_utils.launch_helpers import create_demo_nodes, create_gateway_node + + +def generate_test_description(): + pkg_share = get_package_share_directory('ros2_medkit_gateway') + manifest_path = os.path.join( + pkg_share, 'config', 'examples', 'demo_nodes_manifest.yaml' + ) + + # Hybrid mode with restrictive gap-fill: + # - No heuristic areas (only manifest areas) + # - No heuristic components (only manifest components) + # - Apps still allowed (for linking) + gateway_node = create_gateway_node(extra_params={ + 'discovery.mode': 'hybrid', + 'discovery.manifest_path': manifest_path, + 'discovery.manifest_strict_validation': False, + 'discovery.merge_pipeline.gap_fill.allow_heuristic_areas': False, + 'discovery.merge_pipeline.gap_fill.allow_heuristic_components': False, + 'discovery.merge_pipeline.gap_fill.allow_heuristic_apps': True, + }) + + # Launch a subset of demo nodes (some in manifest, some not) + demo_nodes = create_demo_nodes( + ['temp_sensor', 'rpm_sensor', 'pressure_sensor', 'calibration'], + ) + delayed = TimerAction(period=2.0, actions=demo_nodes) + + return ( + LaunchDescription([ + gateway_node, + delayed, + launch_testing.actions.ReadyToTest(), + ]), + {'gateway_node': gateway_node}, + ) + + +# @verifies REQ_INTEROP_003 +class TestGapFillConfig(GatewayTestCase): + """Test gap-fill restrictions in hybrid mode.""" + + def test_only_manifest_areas_present(self): + """With allow_heuristic_areas=false, only manifest areas should exist.""" + data = self.poll_endpoint_until( + '/areas', + lambda d: d if len(d.get('items', [])) >= 1 else None, + timeout=30.0, + ) + area_ids = [a['id'] for a in data['items']] + + # Manifest defines: powertrain, chassis, body, perception + # No heuristic areas from runtime namespaces should appear + for area_id in area_ids: + self.assertIn(area_id, [ + 'powertrain', 'chassis', 'body', 'perception', + # Subareas defined in manifest + 'engine', 'brakes', 'lidar', 'door', 'lights', + 'front-left-door', + ], f"Unexpected heuristic area found: {area_id}") + + def test_only_manifest_components_present(self): + """With allow_heuristic_components=false, only manifest components exist.""" + data = self.poll_endpoint_until( + '/components', + lambda d: d if len(d.get('items', [])) >= 1 else None, + timeout=30.0, + ) + component_ids = [c['id'] for c in data['items']] + + # Only manifest-defined components should be present + manifest_components = [ + 'engine-ecu', 'temp-sensor-hw', 'rpm-sensor-hw', + 'brake-ecu', 'brake-pressure-sensor-hw', 'brake-actuator-hw', + 'door-sensor-hw', 'light-module', + 'lidar-unit', + ] + for comp_id in component_ids: + self.assertIn( + comp_id, manifest_components, + f"Unexpected heuristic component found: {comp_id}", + ) + + def test_health_shows_gap_fill_filtering(self): + """Health endpoint should show filtered_by_gap_fill count.""" + health = self.poll_endpoint_until( + '/health', + lambda data: data if 'discovery' in data else None, + timeout=30.0, + ) + discovery = health.get('discovery', {}) + pipeline = discovery.get('pipeline', {}) + + # Should have filtered some entities + self.assertIn('filtered_by_gap_fill', pipeline) + + +@launch_testing.post_shutdown_test() +class TestShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes( + proc_info, allowable_exit_codes=ALLOWED_EXIT_CODES + ) diff --git a/src/ros2_medkit_integration_tests/test/features/test_discovery_layer_policies.test.py b/src/ros2_medkit_integration_tests/test/features/test_discovery_layer_policies.test.py new file mode 100644 index 00000000..3dab04f2 --- /dev/null +++ b/src/ros2_medkit_integration_tests/test/features/test_discovery_layer_policies.test.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python3 +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Integration tests for per-layer merge policy overrides in hybrid mode. + +Tests that users can override default merge policies per layer per field group +via discovery.merge_pipeline.layers.. parameters. +""" + +import os +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import TimerAction +import launch_testing +import launch_testing.actions + +from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES +from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase +from ros2_medkit_test_utils.launch_helpers import create_demo_nodes, create_gateway_node + + +def generate_test_description(): + pkg_share = get_package_share_directory('ros2_medkit_gateway') + manifest_path = os.path.join( + pkg_share, 'config', 'examples', 'demo_nodes_manifest.yaml' + ) + + # Hybrid mode with custom layer policy overrides: + # - Manifest LIVE_DATA overridden from "enrichment" to "authoritative" + # (manifest topics should take precedence over runtime topics) + # - Runtime IDENTITY overridden from "fallback" to "enrichment" + # (runtime names can fill empty manifest names) + gateway_node = create_gateway_node(extra_params={ + 'discovery.mode': 'hybrid', + 'discovery.manifest_path': manifest_path, + 'discovery.manifest_strict_validation': False, + 'discovery.merge_pipeline.layers.manifest.live_data': 'authoritative', + 'discovery.merge_pipeline.layers.runtime.identity': 'enrichment', + }) + + # Launch demo nodes that match manifest apps + demo_nodes = create_demo_nodes( + ['temp_sensor', 'rpm_sensor', 'calibration', 'pressure_sensor'], + ) + delayed = TimerAction(period=2.0, actions=demo_nodes) + + return ( + LaunchDescription([ + gateway_node, + delayed, + launch_testing.actions.ReadyToTest(), + ]), + {'gateway_node': gateway_node}, + ) + + +# @verifies REQ_INTEROP_003 +class TestLayerPolicyOverrides(GatewayTestCase): + """Test per-layer merge policy overrides in hybrid mode.""" + + def test_gateway_starts_with_policy_overrides(self): + """Gateway should start successfully with custom layer policies.""" + health = self.poll_endpoint_until( + '/health', + lambda data: data if data.get('status') == 'healthy' else None, + timeout=30.0, + ) + self.assertEqual(health['status'], 'healthy') + + def test_discovery_mode_is_hybrid(self): + """Discovery mode should be hybrid.""" + health = self.poll_endpoint_until( + '/health', + lambda data: data if 'discovery' in data else None, + timeout=30.0, + ) + discovery = health.get('discovery', {}) + self.assertEqual(discovery.get('mode'), 'hybrid') + + def test_manifest_entities_present(self): + """Manifest-defined entities should be discoverable.""" + data = self.poll_endpoint_until( + '/areas', + lambda d: d if any(a['id'] == 'powertrain' for a in d.get('items', [])) else None, + timeout=30.0, + ) + area_ids = [a['id'] for a in data['items']] + self.assertIn('powertrain', area_ids) + self.assertIn('chassis', area_ids) + + def test_manifest_apps_present(self): + """Manifest-defined apps should be discoverable.""" + data = self.poll_endpoint_until( + '/apps', + lambda d: d if len(d.get('items', [])) >= 1 else None, + timeout=30.0, + ) + app_ids = [a['id'] for a in data['items']] + # Manifest defines engine-temp-sensor, engine-rpm-sensor, etc. + self.assertTrue( + any('engine' in aid for aid in app_ids), + f"No engine apps found: {app_ids}", + ) + + def test_merge_pipeline_has_layers(self): + """Health endpoint should report merge pipeline with layer names.""" + health = self.poll_endpoint_until( + '/health', + lambda data: data if 'discovery' in data + and 'pipeline' in data.get('discovery', {}) else None, + timeout=30.0, + ) + pipeline = health['discovery']['pipeline'] + layers = pipeline.get('layers', []) + self.assertIn('manifest', layers) + self.assertIn('runtime', layers) + + def test_manifest_authoritative_live_data(self): + """Manifest LIVE_DATA=authoritative should override runtime topics. + + The manifest defines engine-temp-sensor with ros_binding for temp_sensor + in /powertrain/engine. With manifest LIVE_DATA set to authoritative, + manifest-defined topic info should take precedence. Verify that the + component's topics field is not empty (manifest provides topic data + from inherit_runtime_resources) and that the merge report shows no + LIVE_DATA conflicts (authoritative wins cleanly). + """ + health = self.poll_endpoint_until( + '/health', + lambda data: data if data.get('discovery', {}).get('pipeline', {}).get( + 'conflict_count', -1) >= 0 else None, + timeout=30.0, + ) + pipeline = health['discovery']['pipeline'] + # With authoritative vs enrichment at different levels, there should + # be no conflict (authoritative simply wins) + self.assertEqual( + pipeline.get('conflict_count', -1), 0, + f"Expected no merge conflicts with authoritative policy, got: {pipeline}", + ) + + +@launch_testing.post_shutdown_test() +class TestShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes( + proc_info, allowable_exit_codes=ALLOWED_EXIT_CODES + ) diff --git a/src/ros2_medkit_integration_tests/test/features/test_discovery_legacy_mode.test.py b/src/ros2_medkit_integration_tests/test/features/test_discovery_legacy_mode.test.py new file mode 100644 index 00000000..12b640d4 --- /dev/null +++ b/src/ros2_medkit_integration_tests/test/features/test_discovery_legacy_mode.test.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Integration tests for legacy discovery mode (create_synthetic_components: false). + +When synthetic components are disabled, each node becomes its own Component +in a 1:1 mapping (no namespace-based grouping). +""" + +import unittest + +from launch import LaunchDescription +from launch.actions import TimerAction +import launch_testing +import launch_testing.actions + +from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES +from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase +from ros2_medkit_test_utils.launch_helpers import create_demo_nodes, create_gateway_node + + +def generate_test_description(): + gateway_node = create_gateway_node( + extra_params={ + 'discovery.runtime.create_synthetic_components': False, + }, + ) + + demo_nodes = create_demo_nodes( + ['temp_sensor', 'rpm_sensor', 'pressure_sensor'], + ) + delayed = TimerAction(period=2.0, actions=demo_nodes) + + return ( + LaunchDescription([ + gateway_node, + delayed, + launch_testing.actions.ReadyToTest(), + ]), + {'gateway_node': gateway_node}, + ) + + +# @verifies REQ_INTEROP_003 +class TestLegacyDiscoveryMode(GatewayTestCase): + """Test create_synthetic_components=false (legacy 1:1 node-to-component mode).""" + + def test_each_node_has_own_component(self): + """Each node should become its own Component (no synthetic grouping).""" + data = self.poll_endpoint_until( + '/components', + lambda d: d if any( + 'temp_sensor' in c['id'] for c in d.get('items', []) + ) else None, + timeout=60.0, + ) + component_ids = [c['id'] for c in data['items']] + + # Each demo node should appear as a component + # Node names: temp_sensor, rpm_sensor, pressure_sensor + self.assertTrue( + any('temp_sensor' in cid for cid in component_ids), + f"temp_sensor not found in components: {component_ids}", + ) + self.assertTrue( + any('rpm_sensor' in cid for cid in component_ids), + f"rpm_sensor not found in components: {component_ids}", + ) + + def test_no_synthetic_namespace_components(self): + """No synthetic components from namespace grouping should exist.""" + data = self.poll_endpoint_until( + '/components', + lambda d: d if any( + 'temp_sensor' in c['id'] for c in d.get('items', []) + ) else None, + timeout=60.0, + ) + + # With synthetic off, components should NOT have source="synthetic" + for comp in data['items']: + x_medkit = comp.get('x-medkit', {}) + source = x_medkit.get('source', '') + self.assertNotEqual( + source, 'synthetic', + f"Component {comp['id']} has source=synthetic in legacy mode", + ) + + +@launch_testing.post_shutdown_test() +class TestShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes( + proc_info, allowable_exit_codes=ALLOWED_EXIT_CODES + ) diff --git a/src/ros2_medkit_integration_tests/test/features/test_discovery_namespace_filter.test.py b/src/ros2_medkit_integration_tests/test/features/test_discovery_namespace_filter.test.py new file mode 100644 index 00000000..880e0c5e --- /dev/null +++ b/src/ros2_medkit_integration_tests/test/features/test_discovery_namespace_filter.test.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Integration tests for namespace blacklist/whitelist in hybrid gap-fill. + +Tests that namespace_blacklist and namespace_whitelist parameters +correctly filter which heuristic entities the runtime layer creates. + +Strategy: launch two unmanifested nodes - one in a blacklisted namespace +(/chassis/brakes) and one in a non-blacklisted namespace (/powertrain/engine). +The blacklisted one should be filtered from gap-fill; the other should appear. +""" + +import os +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import TimerAction +import launch_ros.actions +import launch_testing +import launch_testing.actions + +from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES +from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase +from ros2_medkit_test_utils.launch_helpers import create_demo_nodes, create_gateway_node + + +def generate_test_description(): + pkg_share = get_package_share_directory('ros2_medkit_gateway') + manifest_path = os.path.join( + pkg_share, 'config', 'examples', 'demo_nodes_manifest.yaml' + ) + + # Hybrid mode with namespace blacklist: + # Block /chassis namespace from gap-fill (only manifest chassis entities) + gateway_node = create_gateway_node(extra_params={ + 'discovery.mode': 'hybrid', + 'discovery.manifest_path': manifest_path, + 'discovery.manifest_strict_validation': False, + 'discovery.merge_pipeline.gap_fill.namespace_blacklist': ['/chassis'], + }) + + # Launch demo nodes including chassis nodes + demo_nodes = create_demo_nodes( + ['temp_sensor', 'rpm_sensor', 'pressure_sensor', 'calibration'], + ) + + # Launch an extra unmanifested node in /chassis/brakes (blacklisted) + # This should be FILTERED by the namespace blacklist. + unmanifested_chassis_node = launch_ros.actions.Node( + package='ros2_medkit_integration_tests', + executable='demo_engine_temp_sensor', + name='unmanifested_chassis_sensor', + namespace='/chassis/brakes', + output='screen', + ) + + # Launch an extra unmanifested node in /powertrain/engine (not blacklisted) + # This should NOT be filtered and appear as a heuristic gap-fill app. + unmanifested_engine_node = launch_ros.actions.Node( + package='ros2_medkit_integration_tests', + executable='demo_engine_temp_sensor', + name='unmanifested_engine_sensor', + namespace='/powertrain/engine', + output='screen', + ) + + delayed = TimerAction( + period=2.0, + actions=demo_nodes + [unmanifested_chassis_node, unmanifested_engine_node], + ) + + return ( + LaunchDescription([ + gateway_node, + delayed, + launch_testing.actions.ReadyToTest(), + ]), + {'gateway_node': gateway_node}, + ) + + +# @verifies REQ_INTEROP_003 +class TestNamespaceFilter(GatewayTestCase): + """Test namespace blacklist filtering in hybrid gap-fill.""" + + def test_gateway_starts_with_namespace_filter(self): + """Gateway should start successfully with namespace blacklist.""" + health = self.poll_endpoint_until( + '/health', + lambda data: data if data.get('status') == 'healthy' else None, + timeout=30.0, + ) + self.assertEqual(health['status'], 'healthy') + discovery = health.get('discovery', {}) + self.assertEqual(discovery.get('mode'), 'hybrid') + + def test_manifest_entities_always_present(self): + """Manifest entities from blacklisted namespaces should still be present.""" + # Manifest defines chassis area and its components - + # blacklist only affects gap-fill (heuristic entities), not manifest entities + data = self.poll_endpoint_until( + '/areas', + lambda d: d if any(a['id'] == 'powertrain' for a in d.get('items', [])) else None, + timeout=30.0, + ) + area_ids = [a['id'] for a in data['items']] + # Manifest-defined areas should still exist regardless of blacklist + self.assertIn('powertrain', area_ids) + self.assertIn('chassis', area_ids) + + def test_blacklist_filters_unmanifested_chassis_node(self): + """Unmanifested node in blacklisted /chassis should NOT appear as app.""" + # Wait for the non-blacklisted unmanifested node to appear first, + # proving that gap-fill is active and enough time has passed + self.poll_endpoint_until( + '/apps', + lambda d: d if any( + 'unmanifested_engine_sensor' in a.get('id', '') + for a in d.get('items', []) + ) else None, + timeout=30.0, + ) + + # Now verify the blacklisted one is absent + data = self.get_json('/apps') + app_ids = [a['id'] for a in data.get('items', [])] + for app_id in app_ids: + self.assertNotIn( + 'unmanifested_chassis_sensor', app_id, + f"Blacklisted namespace node should not appear: {app_id}", + ) + + def test_health_shows_gap_fill_filtering(self): + """Health endpoint should show pipeline stats including filtered count.""" + health = self.poll_endpoint_until( + '/health', + lambda data: data if 'discovery' in data + and 'pipeline' in data.get('discovery', {}) else None, + timeout=30.0, + ) + pipeline = health['discovery']['pipeline'] + self.assertIn('total_entities', pipeline) + + +@launch_testing.post_shutdown_test() +class TestShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + launch_testing.asserts.assertExitCodes( + proc_info, allowable_exit_codes=ALLOWED_EXIT_CODES + ) diff --git a/src/ros2_medkit_integration_tests/test/features/test_plugin_vendor_extensions.test.py b/src/ros2_medkit_integration_tests/test/features/test_plugin_vendor_extensions.test.py new file mode 100644 index 00000000..17372882 --- /dev/null +++ b/src/ros2_medkit_integration_tests/test/features/test_plugin_vendor_extensions.test.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Integration tests for plugin vendor extension endpoints. + +Validates that plugins can register per-entity-type capabilities via +register_capability() and serve entity-scoped vendor extension resources +via register_routes(). + +Uses test_gateway_plugin which registers an "x-medkit-diagnostics" +capability for all Components and serves diagnostic data at +GET /components/{id}/x-medkit-diagnostics. +""" + +import os +import unittest + +import launch_testing +import requests + +from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES +from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase +from ros2_medkit_test_utils.launch_helpers import create_test_launch + + +def _get_test_plugin_path(): + """Get path to test_gateway_plugin.so.""" + from ament_index_python.packages import get_package_prefix + + pkg_prefix = get_package_prefix('ros2_medkit_gateway') + return os.path.join( + pkg_prefix, 'lib', 'ros2_medkit_gateway', 'libtest_gateway_plugin.so' + ) + + +def generate_test_description(): + """Launch gateway with test_gateway_plugin and a few demo nodes.""" + plugin_path = _get_test_plugin_path() + return create_test_launch( + demo_nodes=['temp_sensor', 'rpm_sensor'], + fault_manager=False, + gateway_params={ + 'plugins': ['test_plugin'], + 'plugins.test_plugin.path': plugin_path, + }, + ) + + +class TestPluginVendorExtensions(GatewayTestCase): + """Vendor extension endpoint tests via test_gateway_plugin.""" + + MIN_EXPECTED_APPS = 2 + REQUIRED_APPS = {'temp_sensor', 'rpm_sensor'} + + def _get_any_component_id(self): + """Get the first discovered component ID.""" + data = self.get_json('/components') + items = data.get('items', []) + self.assertGreater(len(items), 0, 'No components discovered') + return items[0]['id'] + + def test_01_vendor_extension_endpoint_returns_data(self): + """GET /components/{id}/x-medkit-diagnostics returns plugin data.""" + comp_id = self._get_any_component_id() + data = self.get_json(f'/components/{comp_id}/x-medkit-diagnostics') + self.assertEqual(data['entity_id'], comp_id) + self.assertEqual(data['plugin'], 'test_plugin') + self.assertIn('cpu_usage', data) + self.assertIn('memory_mb', data) + self.assertIn('uptime_seconds', data) + + def test_02_capabilities_include_vendor_extension(self): + """Entity capabilities include the plugin-registered capability.""" + comp_id = self._get_any_component_id() + data = self.get_json(f'/components/{comp_id}') + capabilities = data.get('capabilities', []) + cap_names = [c['name'] for c in capabilities] + self.assertIn( + 'x-medkit-diagnostics', + cap_names, + f'x-medkit-diagnostics not in capabilities: {cap_names}', + ) + # Verify href points to the correct path + diag_cap = next( + c for c in capabilities if c['name'] == 'x-medkit-diagnostics' + ) + self.assertIn(f'/components/{comp_id}/x-medkit-diagnostics', diag_cap['href']) + + def test_03_vendor_extension_nonexistent_entity_returns_404(self): + """GET /components/nonexistent/x-medkit-diagnostics returns 404.""" + r = requests.get( + f'{self.BASE_URL}/components/nonexistent-entity/x-medkit-diagnostics', + timeout=5, + ) + self.assertEqual(r.status_code, 404) + + def test_04_global_vendor_endpoint_still_works(self): + """GET /x-test/ping global endpoint still responds.""" + r = requests.get(f'{self.BASE_URL}/x-test/ping', timeout=5) + self.assertEqual(r.status_code, 200) + self.assertEqual(r.text, 'pong') + + +@launch_testing.post_shutdown_test() +class TestShutdown(unittest.TestCase): + """Verify gateway exits cleanly.""" + + def test_exit_codes(self, proc_info): + for info in proc_info: + self.assertIn( + info.returncode, + ALLOWED_EXIT_CODES, + f'Process {info.process_name} exited with {info.returncode}', + ) diff --git a/src/ros2_medkit_integration_tests/test/features/test_rate_limiting.test.py b/src/ros2_medkit_integration_tests/test/features/test_rate_limiting.test.py index 394b5477..d97fc934 100644 --- a/src/ros2_medkit_integration_tests/test/features/test_rate_limiting.test.py +++ b/src/ros2_medkit_integration_tests/test/features/test_rate_limiting.test.py @@ -35,13 +35,13 @@ import launch_testing.actions import requests -from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES, API_BASE_PATH +from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES, API_BASE_PATH, get_test_port from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase from ros2_medkit_test_utils.launch_helpers import create_gateway_node # Test configuration GATEWAY_HOST = '127.0.0.1' -GATEWAY_PORT = 8087 +GATEWAY_PORT = get_test_port() ALLOWED_ORIGIN = 'http://localhost:5173' @@ -51,7 +51,6 @@ def generate_test_description(): port=GATEWAY_PORT, extra_params={ 'server.host': GATEWAY_HOST, - 'server.port': GATEWAY_PORT, 'rate_limiting.enabled': True, 'rate_limiting.global_requests_per_minute': 100, 'rate_limiting.client_requests_per_minute': 2, diff --git a/src/ros2_medkit_integration_tests/test/features/test_tls.test.py b/src/ros2_medkit_integration_tests/test/features/test_tls.test.py index e34c04f3..2461f981 100644 --- a/src/ros2_medkit_integration_tests/test/features/test_tls.test.py +++ b/src/ros2_medkit_integration_tests/test/features/test_tls.test.py @@ -36,11 +36,12 @@ import launch_ros.actions import launch_testing import launch_testing.actions +from ros2_medkit_test_utils.constants import get_test_port from ros2_medkit_test_utils.coverage import get_coverage_env import urllib3 -# Port for HTTPS testing (different from default HTTP port) -HTTPS_PORT = 8443 +# Port for HTTPS testing +HTTPS_PORT = get_test_port() HTTPS_BASE_URL = f'https://localhost:{HTTPS_PORT}' diff --git a/src/ros2_medkit_integration_tests/test/features/test_updates.test.py b/src/ros2_medkit_integration_tests/test/features/test_updates.test.py index 2cf0b19f..caa5f90e 100644 --- a/src/ros2_medkit_integration_tests/test/features/test_updates.test.py +++ b/src/ros2_medkit_integration_tests/test/features/test_updates.test.py @@ -26,13 +26,13 @@ import launch_testing.actions import requests -from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES, API_BASE_PATH +from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES, API_BASE_PATH, get_test_port from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase from ros2_medkit_test_utils.launch_helpers import get_coverage_env -PORT_NO_PLUGIN = 8088 -PORT_WITH_PLUGIN = 8089 +PORT_NO_PLUGIN = get_test_port(0) +PORT_WITH_PLUGIN = get_test_port(1) def _get_test_plugin_path(): diff --git a/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_bulk_data_upload.test.py b/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_bulk_data_upload.test.py index 2648c900..b0a5d4fb 100644 --- a/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_bulk_data_upload.test.py +++ b/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_bulk_data_upload.test.py @@ -31,12 +31,11 @@ import launch_testing.actions import requests -from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES, API_BASE_PATH +from ros2_medkit_test_utils.constants import ALLOWED_EXIT_CODES, API_BASE_PATH, get_test_port from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase from ros2_medkit_test_utils.launch_helpers import create_demo_nodes, create_gateway_node -# Use a unique port to avoid conflicts with other integration tests -TEST_PORT = 8765 +TEST_PORT = get_test_port() BULK_DATA_BASE_URL = f'http://localhost:{TEST_PORT}{API_BASE_PATH}' @@ -48,7 +47,6 @@ def generate_test_description(): port=TEST_PORT, extra_params={ 'server.host': '127.0.0.1', - 'server.port': TEST_PORT, 'bulk_data.storage_dir': tmpdir, 'bulk_data.max_upload_size': 104857600, 'bulk_data.categories': ['calibration', 'firmware'], diff --git a/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_discovery_hybrid.test.py b/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_discovery_hybrid.test.py index 843962bf..b9b040d0 100644 --- a/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_discovery_hybrid.test.py +++ b/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_discovery_hybrid.test.py @@ -51,9 +51,9 @@ def generate_test_description(): ) gateway = create_gateway_node(extra_params={ - 'discovery_mode': 'hybrid', - 'manifest_path': manifest_path, - 'manifest_strict_validation': False, + 'discovery.mode': 'hybrid', + 'discovery.manifest_path': manifest_path, + 'discovery.manifest_strict_validation': False, 'unmanifested_nodes': 'warn', }) @@ -435,6 +435,17 @@ def test_30_nonexistent_function(self): """404 for non-existent function.""" self.assert_entity_not_found('functions', 'nonexistent') + def test_31_health_includes_discovery_diagnostics(self): + """GET /health in hybrid mode includes discovery pipeline info.""" + data = self.get_json('/health') + self.assertIn('discovery', data) + self.assertEqual(data['discovery']['mode'], 'hybrid') + self.assertIn('pipeline', data['discovery']) + pipeline = data['discovery']['pipeline'] + self.assertIn('layers', pipeline) + self.assertIn('total_entities', pipeline) + self.assertGreater(pipeline['total_entities'], 0) + @launch_testing.post_shutdown_test() class TestShutdown(unittest.TestCase): diff --git a/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_discovery_manifest.test.py b/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_discovery_manifest.test.py index a7051143..b10a5cb8 100644 --- a/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_discovery_manifest.test.py +++ b/src/ros2_medkit_integration_tests/test/scenarios/test_scenario_discovery_manifest.test.py @@ -53,9 +53,9 @@ def generate_test_description(): ) gateway = create_gateway_node(extra_params={ - 'discovery_mode': 'manifest_only', - 'manifest_path': manifest_path, - 'manifest_strict_validation': False, + 'discovery.mode': 'manifest_only', + 'discovery.manifest_path': manifest_path, + 'discovery.manifest_strict_validation': False, }) # Launch a subset of demo nodes to verify apps become online diff --git a/src/ros2_medkit_serialization/CMakeLists.txt b/src/ros2_medkit_serialization/CMakeLists.txt index 47f4b5c2..8720ffd8 100644 --- a/src/ros2_medkit_serialization/CMakeLists.txt +++ b/src/ros2_medkit_serialization/CMakeLists.txt @@ -51,6 +51,10 @@ add_library(${PROJECT_NAME} src/vendored/dynmsg/yaml_utils.cpp ) +# Enable PIC so this static library can be linked into shared objects +# (e.g. test_gateway_plugin.so via gateway_lib.a) +set_target_properties(${PROJECT_NAME} PROPERTIES POSITION_INDEPENDENT_CODE ON) + target_include_directories(${PROJECT_NAME} PUBLIC $ $