diff --git a/scripts/check_no_naked_subscriptions.sh b/scripts/check_no_naked_subscriptions.sh index e9514aab5..38f0309a4 100755 --- a/scripts/check_no_naked_subscriptions.sh +++ b/scripts/check_no_naked_subscriptions.sh @@ -46,9 +46,9 @@ ALLOWED_DIRS_PATTERN="(${GATEWAY_ROOT}/src/ros2_common/|${GATEWAY_ROOT}/include/ ALLOWED_LEGACY_FILES=( "${GATEWAY_ROOT}/src/http/handlers/sse_fault_handler.cpp" # faults provider follow-up "${GATEWAY_ROOT}/src/trigger_fault_subscriber.cpp" # faults provider follow-up - "${GATEWAY_ROOT}/src/trigger_topic_subscriber.cpp" # data_stream provider follow-up - "${GATEWAY_ROOT}/src/operation_manager.cpp" # operations provider follow-up - "${GATEWAY_ROOT}/src/log_manager.cpp" # logs provider follow-up + "${GATEWAY_ROOT}/src/ros2/trigger_topic_subscriber.cpp" # adapter for TopicSubscriptionTransport (rclcpp boundary) + "${GATEWAY_ROOT}/src/ros2/transports/ros2_action_transport.cpp" # operations provider follow-up + "${GATEWAY_ROOT}/src/ros2/transports/ros2_log_source.cpp" # /rosout adapter (LogSource port) "${FAULT_MANAGER_ROOT}/src/snapshot_capture.cpp" # uses LockedSubscriptionGuard (in-place serialisation) "${FAULT_MANAGER_ROOT}/include/ros2_medkit_fault_manager/snapshot_capture.hpp" # comment references the guarded API "${FAULT_MANAGER_ROOT}/src/rosbag_capture.cpp" # bag-recorder spawns its own node + executor, no shared rcl hash map diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index 40fc7ef60..017974871 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -149,12 +149,19 @@ target_precompile_headers(gateway_core PRIVATE # transitively get both layers from a single dependency. add_library(gateway_ros2 STATIC src/aggregation/aggregation_manager.cpp - src/configuration_manager.cpp - src/data_access_manager.cpp src/data/ros2_topic_data_provider.cpp + src/ros2/conversions/fault_msg_conversions.cpp + src/ros2/providers/ros2_runtime_introspection.cpp + src/ros2/transports/ros2_action_transport.cpp + src/ros2/transports/ros2_fault_service_transport.cpp + src/ros2/transports/ros2_log_source.cpp + src/ros2/transports/ros2_parameter_transport.cpp + src/ros2/transports/ros2_service_transport.cpp + src/ros2/transports/ros2_topic_subscription_transport.cpp + src/ros2/transports/ros2_topic_transport.cpp + src/ros2/trigger_topic_subscriber.cpp src/default_script_provider.cpp src/discovery/discovery_manager.cpp - src/discovery/hybrid_discovery.cpp src/discovery/layers/manifest_layer.cpp src/discovery/layers/plugin_layer.cpp src/discovery/layers/runtime_layer.cpp @@ -162,8 +169,6 @@ add_library(gateway_ros2 STATIC src/discovery/manifest/manifest_parser.cpp src/discovery/manifest/runtime_linker.cpp src/discovery/merge_pipeline.cpp - src/discovery/runtime_discovery.cpp - src/fault_manager.cpp src/fault_manager_paths.cpp src/gateway_node.cpp src/http/handlers/auth_handlers.cpp @@ -186,8 +191,6 @@ add_library(gateway_ros2 STATIC src/http/handlers/update_handlers.cpp src/http/http_server.cpp src/http/rest_server.cpp - src/log_manager.cpp - src/operation_manager.cpp src/openapi/capability_generator.cpp src/openapi/openapi_spec_builder.cpp src/openapi/path_builder.cpp @@ -200,9 +203,6 @@ add_library(gateway_ros2 STATIC src/ros2_common/ros2_subscription_slot.cpp src/script_manager.cpp src/trigger_fault_subscriber.cpp - src/trigger_manager.cpp - src/trigger_topic_subscriber.cpp - src/type_introspection.cpp ) # Private implementation headers for openapi module (shared by both layers) @@ -366,6 +366,112 @@ if(BUILD_TESTING) ) set_tests_properties(gateway_core_smoke PROPERTIES LABELS "unit") + # ─── DataAccessManager routing test (gateway_core only) ──────────────────── + # Mock-transport coverage proving the manager body compiles + links against + # gateway_core alone, with no rclcpp on the link line. + add_executable(test_data_access_manager_routing test/test_data_access_manager_routing.cpp) + target_link_libraries(test_data_access_manager_routing + PRIVATE + gateway_core + GTest::gtest + GTest::gtest_main + ) + add_test( + NAME data_access_manager_routing + COMMAND $ + ) + set_tests_properties(data_access_manager_routing PROPERTIES LABELS "unit") + + # ─── OperationManager routing test (gateway_core only) ───────────────────── + # Mock-transport coverage proving the manager body (service / action call + # routing, goal tracking, status callback wiring, cleanup) compiles and + # links against gateway_core alone, with no rclcpp on the link line. + add_executable(test_operation_manager_routing test/test_operation_manager_routing.cpp) + target_link_libraries(test_operation_manager_routing + PRIVATE + gateway_core + GTest::gtest + GTest::gtest_main + ) + add_test( + NAME operation_manager_routing + COMMAND $ + ) + set_tests_properties(operation_manager_routing PROPERTIES LABELS "unit") + + # ─── ConfigurationManager routing test (gateway_core only) ───────────────── + # Mock-transport coverage proving the manager body (CRUD delegation, + # self-node short circuit, reset orchestration via cached defaults, + # shutdown idempotency, error code propagation) compiles and links against + # gateway_core alone, with no rclcpp on the link line. + add_executable(test_configuration_manager_routing test/test_configuration_manager_routing.cpp) + target_link_libraries(test_configuration_manager_routing + PRIVATE + gateway_core + GTest::gtest + GTest::gtest_main + ) + add_test( + NAME configuration_manager_routing + COMMAND $ + ) + set_tests_properties(configuration_manager_routing PROPERTIES LABELS "unit") + + # ─── FaultManager routing test (gateway_core only) ───────────────────────── + # Mock-transport coverage proving the manager body (eight SOVD operations, + # error_message propagation, JSON-shape contract for get_fault_with_env) + # compiles and links against gateway_core alone, with no rclcpp on the link + # line. + add_executable(test_fault_manager_routing test/test_fault_manager_routing.cpp) + target_link_libraries(test_fault_manager_routing + PRIVATE + gateway_core + GTest::gtest + GTest::gtest_main + ) + add_test( + NAME fault_manager_routing + COMMAND $ + ) + set_tests_properties(fault_manager_routing PROPERTIES LABELS "unit") + + # ─── LogManager routing test (gateway_core only) ─────────────────────────── + # Mock-source coverage proving the manager body (ring-buffer storage, + # plugin observer pattern, manages_ingestion short-circuit, severity + # filter, source teardown) compiles and links against gateway_core alone, + # with no rclcpp on the link line. + add_executable(test_log_manager_routing test/test_log_manager_routing.cpp) + target_link_libraries(test_log_manager_routing + PRIVATE + gateway_core + GTest::gtest + GTest::gtest_main + ) + add_test( + NAME log_manager_routing + COMMAND $ + ) + set_tests_properties(log_manager_routing PROPERTIES LABELS "unit") + + # ─── TriggerManager routing test (gateway_core only) ─────────────────────── + # Mock-transport coverage proving the manager body (data-trigger subscribe + # routing, handle-destruction unsubscribe, sample callback dispatch into + # the resource-change notifier, orphan sweep, multi-trigger isolation, + # shutdown teardown) compiles and links against gateway_core alone, with + # no rclcpp on the link line. + add_executable(test_trigger_manager_routing test/test_trigger_manager_routing.cpp) + target_link_libraries(test_trigger_manager_routing + PRIVATE + gateway_core + GTest::gtest + GTest::gtest_main + ) + add_test( + NAME trigger_manager_routing + COMMAND $ + ) + set_tests_properties(trigger_manager_routing PROPERTIES LABELS "unit") + # Add GTest find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp_action REQUIRED) @@ -409,7 +515,7 @@ if(BUILD_TESTING) medkit_target_dependencies(test_discovery_manager std_msgs) medkit_set_test_domain(test_discovery_manager) - # Add RuntimeDiscoveryStrategy tests + # Add Ros2RuntimeIntrospection tests ament_add_gtest(test_runtime_discovery test/test_runtime_discovery.cpp) target_link_libraries(test_runtime_discovery gateway_ros2) medkit_set_test_domain(test_runtime_discovery) diff --git a/src/ros2_medkit_gateway/README.md b/src/ros2_medkit_gateway/README.md index a90c2234b..ff478cabe 100644 --- a/src/ros2_medkit_gateway/README.md +++ b/src/ros2_medkit_gateway/README.md @@ -1156,7 +1156,7 @@ The gateway can be configured via parameters in `config/gateway_params.yaml` or | ---------------------------- | ------ | ----------- | -------------------------------------------------------------------------------------- | | `server.host` | string | `127.0.0.1` | Host to bind the REST server (`127.0.0.1` for localhost, `0.0.0.0` for all interfaces) | | `server.port` | int | `8080` | Port for the REST API (range: 1024-65535) | -| `refresh_interval_ms` | int | `10000` | Cache refresh interval in milliseconds (range: 100-60000) | +| `refresh_interval_ms` | int | `30000` | Safety-backstop refresh interval in milliseconds. Primary refresh is graph-event driven; this controls only the periodic forced refresh (range: 100-60000) | | `data_provider.max_parallel_samples` | int | `8` | Max concurrent topic samples when fetching data (range: 1-256) | | `topic_sample_timeout_sec` | float | `2.0` | Timeout for sampling topics with active publishers (range: 0.1-30.0) | | `fault_manager.namespace` | string | `""` | Optional namespace prefix for fault manager services and events (for example `robot1`) | @@ -1405,7 +1405,7 @@ ros2 launch ros2_medkit_gateway gateway_https.launch.py min_tls_version:=1.3 | `server_host` | `127.0.0.1` | Host to bind HTTPS server | | `server_port` | `8443` | Port for HTTPS API | | `min_tls_version` | `1.2` | Minimum TLS version (`1.2` or `1.3`) | -| `refresh_interval_ms` | `10000` | Cache refresh interval in milliseconds | +| `refresh_interval_ms` | `30000` | Safety-backstop refresh interval (graph events drive primary refresh) | **Usage with curl (self-signed certs):** ```bash diff --git a/src/ros2_medkit_gateway/config/gateway_params.yaml b/src/ros2_medkit_gateway/config/gateway_params.yaml index 93b04ebe1..515c6ddb0 100644 --- a/src/ros2_medkit_gateway/config/gateway_params.yaml +++ b/src/ros2_medkit_gateway/config/gateway_params.yaml @@ -47,11 +47,18 @@ ros2_medkit_gateway: # TODO: Mutual TLS (client certificate verification) is not yet implemented # See: https://github.com/selfpatch/ros2_medkit/issues/XXX - # Cache refresh interval in milliseconds - # How often to discover ROS 2 nodes and update the cache + # Safety-backstop refresh interval in milliseconds. + # + # Primary discovery is driven by rclcpp graph events (polled every + # 100 ms): a refresh runs whenever a node, topic, service or action + # is added or removed, so on a stable graph this timer rarely fires. + # This value controls only the safety backstop that forces a refresh + # in the unlikely case a graph event is missed (lost wakeup, rclcpp + # anomaly, etc.). + # # Valid range: 100-60000 (0.1s to 60s) - # Default: 10000 (10 seconds) - optimized for developer experience - refresh_interval_ms: 10000 + # Default: 30000 (30 seconds) + refresh_interval_ms: 30000 # CORS Configuration # Cross-Origin Resource Sharing settings for browser-based clients @@ -156,10 +163,14 @@ ros2_medkit_gateway: # Controls how ROS 2 graph entities are mapped to SOVD entities discovery: # Discovery mode - # Options: - # - "runtime_only": Use ROS 2 graph introspection only (default) - # - "manifest_only": Only expose manifest-declared entities - # - "hybrid": Manifest as source of truth + runtime linking + # Selects which layers the merge pipeline activates: + # - "runtime_only": Bypass the pipeline; ROS 2 graph introspection + # drives entities directly (default). + # - "manifest_only": Bypass the pipeline; manifest is the sole source + # of truth. + # - "hybrid": Run the merge pipeline with manifest + runtime + plugin + # layers. Manifest is the source of truth; runtime data links to + # manifest apps. mode: "runtime_only" # Path to manifest file (required for manifest_only and hybrid modes) diff --git a/src/ros2_medkit_gateway/design/aggregation.rst b/src/ros2_medkit_gateway/design/aggregation.rst index 681512753..388a1229a 100644 --- a/src/ros2_medkit_gateway/design/aggregation.rst +++ b/src/ros2_medkit_gateway/design/aggregation.rst @@ -504,10 +504,11 @@ Health Monitoring ~~~~~~~~~~~~~~~~~ ``AggregationManager`` calls ``check_all_health()`` during each entity cache -refresh cycle (controlled by ``refresh_interval_ms``, default: 10000 ms). Each -``PeerClient`` GETs ``/api/v1/health`` on its peer. If the health check fails, -the peer is marked unhealthy and excluded from fan-out queries and entity -fetching. +refresh cycle. Refresh is primarily driven by rclcpp graph events (polled +every 100 ms), with ``refresh_interval_ms`` (default: 30000 ms) providing a +safety backstop for the case a graph event is missed. Each ``PeerClient`` +GETs ``/api/v1/health`` on its peer. If the health check fails, the peer is +marked unhealthy and excluded from fan-out queries and entity fetching. When a peer recovers (health check succeeds again), it is automatically re-included. diff --git a/src/ros2_medkit_gateway/design/index.rst b/src/ros2_medkit_gateway/design/index.rst index b9c6e95fb..2c0d5ae49 100644 --- a/src/ros2_medkit_gateway/design/index.rst +++ b/src/ros2_medkit_gateway/design/index.rst @@ -70,12 +70,16 @@ The following diagram shows the relationships between the main components of the note as N_layer_split This package compiles into two layered static libraries: * gateway_core - middleware-neutral business logic (handlers, - auth, fault model, aggregation, neutral managers, providers, - entity model). Does not link rclcpp. - * gateway_ros2 - ROS adapter classes (GatewayNode, + auth, fault model, aggregation, all SOVD managers including DataAccessManager, OperationManager, ConfigurationManager, - FaultManager, LogManager, TriggerManager, RuntimeDiscovery, - NativeTopicSampler, TypeIntrospection). Publicly links + FaultManager, LogManager, TriggerManager, providers, entity + model). Does not link rclcpp. Managers consume neutral + Transport interfaces (TopicTransport, ServiceTransport, + ActionTransport, ParameterTransport, FaultServiceTransport, + LogSource, TopicSubscriptionTransport). + * gateway_ros2 - ROS-binding adapters (GatewayNode plus the + Ros2*Transport implementations, TriggerTopicSubscriber, + Ros2RuntimeIntrospection, NativeTopicSampler). Publicly links gateway_core. Class associations on this diagram remain valid; the boundary affects build-time only. end note @@ -109,7 +113,8 @@ The following diagram shows the relationships between the main components of the + get_name(): string } - class RuntimeDiscoveryStrategy { + class Ros2RuntimeIntrospection { + + introspect(IntrospectionInput): IntrospectionResult + discover_apps(): vector + discover_functions(): vector - config_: RuntimeConfig @@ -120,13 +125,6 @@ The following diagram shows the relationships between the main components of the - manifest_: Manifest } - class HybridDiscoveryStrategy { - - pipeline_: MergePipeline - + refresh(): void - + add_layer(): void - + get_merge_report(): MergeReport - } - class MergePipeline { + add_layer(): void + execute(): MergeResult @@ -146,7 +144,7 @@ The following diagram shows the relationships between the main components of the } class RuntimeLayer { - - strategy_: RuntimeDiscoveryStrategy* + - runtime_introspection_: Ros2RuntimeIntrospection* - gap_fill_config_: GapFillConfig } @@ -323,8 +321,9 @@ The following diagram shows the relationships between the main components of the } class TriggerTopicSubscriber { - + subscribe_topic(): void - + unsubscribe_topic(): void + + subscribe(topic, type, handle_key, cb): void + + unsubscribe(handle_key): void + + set_retry_callback(cb): void } class EntityCache { @@ -398,11 +397,9 @@ The following diagram shows the relationships between the main components of the DiscoveryManager ..> ActionInfo : creates ' Discovery strategy hierarchy - DiscoveryManager --> DiscoveryStrategy : uses - RuntimeDiscoveryStrategy .up.|> DiscoveryStrategy : implements - ManifestDiscoveryStrategy .up.|> DiscoveryStrategy : implements - HybridDiscoveryStrategy .up.|> DiscoveryStrategy : implements - HybridDiscoveryStrategy *--> MergePipeline : owns + DiscoveryManager *--> Ros2RuntimeIntrospection : owns + DiscoveryManager *--> MergePipeline : owns (hybrid mode) + Ros2RuntimeIntrospection .up.|> IntrospectionProvider : implements ' MergePipeline layer architecture MergePipeline o--> DiscoveryLayer : ordered layers @@ -410,7 +407,7 @@ The following diagram shows the relationships between the main components of the ManifestLayer .up.|> DiscoveryLayer : implements RuntimeLayer .up.|> DiscoveryLayer : implements PluginLayer .up.|> DiscoveryLayer : implements - RuntimeLayer --> RuntimeDiscoveryStrategy : delegates + RuntimeLayer --> Ros2RuntimeIntrospection : delegates ' REST Server uses HTTP library RESTServer *--> HTTPLibServer : owns @@ -428,7 +425,7 @@ The following diagram shows the relationships between the main components of the TriggerManager --> ResourceChangeNotifier : subscribes to TriggerManager --> ConditionRegistry : evaluates with TriggerManager --> TriggerStore : persists via - TriggerManager --> TriggerTopicSubscriber : manages data subscriptions + TriggerManager --> "TopicSubscriptionTransport" : data subscriptions via transport SqliteTriggerStore .up.|> TriggerStore : implements ConditionRegistry o--> ConditionEvaluator : contains many RESTServer --> TriggerManager : uses @@ -461,17 +458,19 @@ Each entry below is tagged with the static library it compiles into: **Discovery Strategies:** - - **RuntimeDiscoveryStrategy** - Heuristic discovery via ROS 2 graph introspection + - **Ros2RuntimeIntrospection** - IntrospectionProvider wrapping ROS 2 graph queries - Maps nodes to Apps with ``source: "heuristic"`` - Creates Functions from namespace grouping - Never creates Areas or Components (those come from manifest/HostInfoProvider) + - Same interface as plugin-provided IntrospectionProviders, so the merge pipeline + treats built-in graph queries identically to plugin contributions - **ManifestDiscoveryStrategy** - Static discovery from YAML manifest - Provides stable, semantic entity IDs - Supports offline detection of failed components - - **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 + - **Hybrid mode (DiscoveryManager + MergePipeline)** - Combines manifest + runtime + plugins + - DiscoveryManager constructs a ``MergePipeline`` with the configured layers + - Supports dynamic plugin layers added at runtime via ``add_plugin_layer()`` + - Thread-safe: a mutex protects the cached merged result, all reads return by value **Merge Pipeline:** @@ -489,7 +488,7 @@ Each entry below is tagged with the static library it compiles into: - ``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, + - ``RuntimeLayer`` - Wraps Ros2RuntimeIntrospection; 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). @@ -612,12 +611,16 @@ It consists of five main components: - Stores trigger metadata, condition parameters, and evaluator state (previous values) - Supports partial updates for status changes and lifetime extensions -5. **TriggerTopicSubscriber** ``[gateway_ros2]`` - Manages ROS 2 topic subscriptions for data triggers. +5. **TriggerTopicSubscriber** ``[gateway_ros2]`` - Generic per-handle subscription executor for data triggers. - - Creates ``rclcpp::GenericSubscription`` instances for monitored data topics - - Reference-counted: multiple triggers on the same topic share one subscription - - Publishes data changes to ``ResourceChangeNotifier`` for condition evaluation - - Automatically unsubscribes when the last trigger for a topic is removed + - Creates one ``rclcpp::GenericSubscription`` per ``handle_key`` provided by the caller + - Each trigger owns its own subscription handle - no reference-counting across triggers + - Wrapped by ``Ros2TopicSubscriptionTransport``, which exposes the neutral + ``TopicSubscriptionTransport`` interface to ``TriggerManager`` + - Subscription lifetime is tied to the trigger entry: removing the trigger drops the + handle and tears down the underlying subscription + - Per-handle callbacks publish samples back to ``ResourceChangeNotifier`` for + condition evaluation Additional Design Documents ---------------------------- diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/configuration_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/configuration_manager.hpp index b83d4b96b..7c072593a 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/configuration_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/configuration_manager.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 mfaferek93 +// 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. @@ -14,185 +14,5 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ros2_medkit_gateway { - -using json = nlohmann::json; - -/// Error codes for parameter operations -enum class ParameterErrorCode { - NONE = 0, ///< No error (success) - NOT_FOUND, ///< Parameter does not exist - READ_ONLY, ///< Parameter is read-only and cannot be modified - SERVICE_UNAVAILABLE, ///< Parameter service not available (node unreachable) - TIMEOUT, ///< Operation timed out - TYPE_MISMATCH, ///< Value type doesn't match parameter type - INVALID_VALUE, ///< Invalid value for parameter - NO_DEFAULTS_CACHED, ///< No default values cached for reset operation - SHUT_DOWN, ///< ConfigurationManager has been shut down - INTERNAL_ERROR ///< Internal/unexpected error -}; - -/// Result of a parameter operation -struct ParameterResult { - bool success; - json data; - std::string error_message; - ParameterErrorCode error_code{ParameterErrorCode::NONE}; ///< Structured error code for programmatic handling -}; - -/// Manager for ROS2 node parameters -/// Provides CRUD operations on node parameters via native rclcpp APIs -/// Also caches initial parameter values as "defaults" for reset operations -class ConfigurationManager { - public: - explicit ConfigurationManager(rclcpp::Node * node); - ~ConfigurationManager(); - - ConfigurationManager(const ConfigurationManager &) = delete; - ConfigurationManager & operator=(const ConfigurationManager &) = delete; - ConfigurationManager(ConfigurationManager &&) = delete; - ConfigurationManager & operator=(ConfigurationManager &&) = delete; - - /// Clean up shared param node and cached clients before ROS 2 context shutdown. - /// Must be called before rclcpp::shutdown() to prevent use-after-free. - /// Idempotent - safe to call multiple times. - void shutdown(); - - /// List all parameters for a node - /// @param node_name Fully qualified node name (e.g., "/powertrain/engine/engine_temp_sensor") - /// @return ParameterResult with array of {name, value, type} objects - ParameterResult list_parameters(const std::string & node_name); - - /// Get a specific parameter value - /// @param node_name Fully qualified node name - /// @param param_name Parameter name - /// @return ParameterResult with {name, value, type, description, read_only} - ParameterResult get_parameter(const std::string & node_name, const std::string & param_name); - - /// Set a parameter value - /// @param node_name Fully qualified node name - /// @param param_name Parameter name - /// @param value New value (JSON type will be converted to appropriate ROS2 type) - /// @return ParameterResult with {name, value, type} - ParameterResult set_parameter(const std::string & node_name, const std::string & param_name, const json & value); - - /// Reset a specific parameter to its default (initial) value - /// @param node_name Fully qualified node name - /// @param param_name Parameter name - /// @return ParameterResult with reset parameter info - ParameterResult reset_parameter(const std::string & node_name, const std::string & param_name); - - /// Reset all parameters of a node to their default (initial) values - /// @param node_name Fully qualified node name - /// @return ParameterResult with count of reset parameters - ParameterResult reset_all_parameters(const std::string & node_name); - - private: - /// Get or create a cached SyncParametersClient for the given node. - std::shared_ptr get_param_client(const std::string & node_name); - - /// Cache default values for a node (called on first access) - /// @pre spin_mutex_ must be held by the caller - void cache_default_values(const std::string & node_name); - - /// Check if a node is in the negative cache (recently unavailable) - bool is_node_unavailable(const std::string & node_name) const; - - /// Mark a node as unavailable in the negative cache - void mark_node_unavailable(const std::string & node_name); - - /// Check if node_name is the gateway's own node (self-query guard) - bool is_self_node(const std::string & node_name) const; - - /// List parameters for the gateway's own node (direct access, no IPC) - ParameterResult list_own_parameters(); - - /// Get a specific parameter from the gateway's own node - ParameterResult get_own_parameter(const std::string & param_name); - - /// Return SHUT_DOWN error result - static ParameterResult shut_down_result(); - - /// Convert ROS2 parameter type to string - static std::string parameter_type_to_string(rclcpp::ParameterType type); - - /// Convert ROS2 ParameterValue to JSON - static json parameter_value_to_json(const rclcpp::ParameterValue & value); - - /// Convert JSON value to ROS2 ParameterValue - static rclcpp::ParameterValue json_to_parameter_value(const json & value, rclcpp::ParameterType hint_type); - - /// Get the service timeout as a chrono duration - std::chrono::duration get_service_timeout() const; - - rclcpp::Node * node_; - - /// Timeout for waiting for parameter services (configurable via parameter_service_timeout_sec parameter) - double service_timeout_sec_{2.0}; - - /// Negative cache TTL (configurable via parameter_service_negative_cache_sec parameter) - double negative_cache_ttl_sec_{60.0}; - - /// Gateway's own fully qualified node name (for self-query detection) - std::string own_node_fqn_; - - /// Shutdown flag - prevents use-after-free on param_node_ after shutdown - std::atomic shutdown_{false}; - - /// Negative cache: nodes whose parameter service was recently unavailable. - /// Avoids repeated blocking waits on nodes that don't have parameter services. - mutable std::shared_mutex negative_cache_mutex_; - std::unordered_map unavailable_nodes_; - - /// Cache of default parameter values per node - /// Key: node_name, Value: map of param_name -> Parameter - mutable std::mutex defaults_mutex_; - std::map> default_values_; - - /// Mutex to serialize spin operations on param_node_. - /// SyncParametersClient::wait_for_service/get_parameters/etc spin param_node_ - /// internally via spin_node_until_future_complete which is NOT thread-safe. - /// This mutex is ONLY held during the actual ROS 2 IPC call, not during - /// cache lookups or JSON building. With negative cache + self-guard, - /// most requests never touch this mutex. - /// Declared BEFORE param_node_ so it outlives the node during destruction. - mutable std::timed_mutex spin_mutex_; - - /// Internal node for parameter client operations. - /// Created once in constructor - must be in DDS graph early for fast service discovery. - /// SyncParametersClient requires a node NOT in an executor to spin internally. - std::shared_ptr param_node_; - - /// Cached SyncParametersClient per target node (avoids recreating clients) - mutable std::mutex clients_mutex_; - std::map> param_clients_; - - /// Maximum entries in negative cache before hard eviction - static constexpr size_t kMaxNegativeCacheSize = 500; - - /// Margin added to service_timeout_sec_ for spin_mutex acquisition timeout. - /// Must exceed parameter_service_timeout_sec to avoid spurious TIMEOUT errors. - static constexpr double kSpinLockMarginSec{1.0}; - - /// Timeout for shutdown to wait for in-flight IPC - static constexpr std::chrono::seconds kShutdownTimeout{10}; - - /// Try to acquire spin_mutex_ with timeout. - /// Returns unique_lock on success, nullopt on timeout (result populated with error). - std::optional> try_acquire_spin_lock(ParameterResult & result); -}; - -} // namespace ros2_medkit_gateway +// Backwards-compatibility shim - header relocated to core/managers/. +#include "ros2_medkit_gateway/core/managers/configuration_manager.hpp" diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/configuration/parameter_types.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/configuration/parameter_types.hpp new file mode 100644 index 000000000..82c664324 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/configuration/parameter_types.hpp @@ -0,0 +1,46 @@ +// 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 + +namespace ros2_medkit_gateway { + +using json = nlohmann::json; + +/// Error codes for parameter operations. +enum class ParameterErrorCode { + NONE = 0, ///< No error (success) + NOT_FOUND, ///< Parameter does not exist + READ_ONLY, ///< Parameter is read-only and cannot be modified + SERVICE_UNAVAILABLE, ///< Parameter service not available (node unreachable) + TIMEOUT, ///< Operation timed out + TYPE_MISMATCH, ///< Value type doesn't match parameter type + INVALID_VALUE, ///< Invalid value for parameter + NO_DEFAULTS_CACHED, ///< No default values cached for reset operation + SHUT_DOWN, ///< ConfigurationManager has been shut down + INTERNAL_ERROR ///< Internal/unexpected error +}; + +/// Result of a parameter operation. +struct ParameterResult { + bool success; + json data; + std::string error_message; + ParameterErrorCode error_code{ParameterErrorCode::NONE}; ///< Structured error code for programmatic handling +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/discovery_strategy.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/discovery_strategy.hpp index 2a80ac1db..e1ab7ecf4 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/discovery_strategy.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/discovery_strategy.hpp @@ -28,14 +28,11 @@ struct Function; namespace discovery { /** - * @brief Interface for discovery strategies (Strategy Pattern) + * @brief Legacy interface for discovery strategies. * - * Allows different discovery implementations: - * - RuntimeDiscoveryStrategy: Discovers from ROS 2 graph (current behavior) - * - ManifestDiscoveryStrategy: Discovers from YAML manifest (future) - * - HybridDiscoveryStrategy: Combines both (future) - * - * @see RuntimeDiscoveryStrategy + * Discovery is now driven by IntrospectionProvider implementations composed + * inside DiscoveryManager's MergePipeline. This abstract base is kept for + * test doubles that still want a strategy-shaped seam. */ class DiscoveryStrategy { public: diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/layers/runtime_layer.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/layers/runtime_layer.hpp index 6aa896032..79b9228d3 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/layers/runtime_layer.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/layers/runtime_layer.hpp @@ -16,7 +16,7 @@ #include "ros2_medkit_gateway/core/discovery/discovery_layer.hpp" #include "ros2_medkit_gateway/core/discovery/merge_types.hpp" -#include "ros2_medkit_gateway/discovery/runtime_discovery.hpp" +#include "ros2_medkit_gateway/ros2/providers/ros2_runtime_introspection.hpp" #include @@ -24,14 +24,14 @@ namespace ros2_medkit_gateway { namespace discovery { /** - * @brief Discovery layer wrapping RuntimeDiscoveryStrategy + * @brief Discovery layer wrapping the ROS 2 runtime IntrospectionProvider. * * Default policies: IDENTITY=FALLBACK, HIERARCHY=FALLBACK, LIVE_DATA=AUTH, * STATUS=AUTH, METADATA=ENRICH */ class RuntimeLayer : public DiscoveryLayer { public: - explicit RuntimeLayer(RuntimeDiscoveryStrategy * runtime_strategy); + explicit RuntimeLayer(ros2::Ros2RuntimeIntrospection * runtime_introspection); std::string name() const override { return "runtime"; @@ -62,7 +62,7 @@ class RuntimeLayer : public DiscoveryLayer { std::vector discover_actions(); private: - RuntimeDiscoveryStrategy * runtime_strategy_; + ros2::Ros2RuntimeIntrospection * runtime_introspection_; std::unordered_map policies_; GapFillConfig gap_fill_config_; size_t last_filtered_count_{0}; diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/service_action_resolver.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/service_action_resolver.hpp new file mode 100644 index 000000000..d895db87f --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/service_action_resolver.hpp @@ -0,0 +1,57 @@ +// 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 "ros2_medkit_gateway/core/discovery/models/common.hpp" + +namespace ros2_medkit_gateway { + +/** + * @brief Lookup interface for resolving services / actions by component + * namespace + operation name. + * + * Decouples OperationManager from DiscoveryManager so the manager body + * compiles inside the neutral core layer without pulling rclcpp through + * the discovery_manager.hpp transitive include chain. DiscoveryManager + * implements this interface, but mock resolvers in unit tests can also. + */ +class ServiceActionResolver { + public: + ServiceActionResolver() = default; + ServiceActionResolver(const ServiceActionResolver &) = delete; + ServiceActionResolver & operator=(const ServiceActionResolver &) = delete; + ServiceActionResolver(ServiceActionResolver &&) = delete; + ServiceActionResolver & operator=(ServiceActionResolver &&) = delete; + virtual ~ServiceActionResolver() = default; + + /** + * @brief Resolve a service by component namespace + operation name. + * @return ServiceInfo if a match exists, std::nullopt otherwise. + */ + virtual std::optional find_service(const std::string & component_ns, + const std::string & operation_name) const = 0; + + /** + * @brief Resolve an action by component namespace + operation name. + * @return ActionInfo if a match exists, std::nullopt otherwise. + */ + virtual std::optional find_action(const std::string & component_ns, + const std::string & operation_name) const = 0; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/faults/fault_types.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/faults/fault_types.hpp new file mode 100644 index 000000000..820800766 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/faults/fault_types.hpp @@ -0,0 +1,44 @@ +// 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 + +namespace ros2_medkit_gateway { + +using json = nlohmann::json; + +/// Outcome of a fault-management operation that returns JSON. `data` carries +/// the response body the handler will serve on success; remains empty on +/// errors. +struct FaultResult { + bool success; + json data; + std::string error_message; +}; + +/// Neutral outcome of `get_fault_with_env`. `data` carries +/// `{ "fault": {...}, "environment_data": {...} }` already converted to JSON +/// by the transport. The handler post-processes rosbag snapshots to add the +/// per-request `bulk_data_uri` (which depends on entity_path) and the +/// freeze_frame snapshots to extract their primary value. +struct FaultWithEnvJsonResult { + bool success; + std::string error_message; + json data; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/configuration_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/configuration_manager.hpp new file mode 100644 index 000000000..6d0a9323c --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/configuration_manager.hpp @@ -0,0 +1,107 @@ +// 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 "ros2_medkit_gateway/core/configuration/parameter_types.hpp" +#include "ros2_medkit_gateway/core/transports/parameter_transport.hpp" + +namespace ros2_medkit_gateway { + +using json = nlohmann::json; + +/** + * @brief Application service for ROS 2 node parameters. + * + * Pure C++; ROS-side I/O is performed by the injected ParameterTransport + * adapter (typically Ros2ParameterTransport). All rclcpp::SyncParametersClient + * usage, the rclcpp::Parameter defaults cache, the negative-cache for + * unreachable nodes, the spin_mutex serialising parameter-client spins, and + * the JSON <-> rclcpp::ParameterValue conversion helpers live in the adapter. + * + * The manager retains the high-level orchestration: + * - shutdown ordering contract (idempotent shutdown forwards to transport) + * - reset orchestration (compose set_parameter with cached default values) + * - the public CRUD surface consumed by handler_context handlers + * + * Self-node short-circuiting (avoiding IPC for the gateway's own parameters) + * is delegated to the transport because identifying the gateway's own FQN + * requires the rclcpp node handle. + */ +class ConfigurationManager { + public: + /** + * @param transport Concrete ParameterTransport adapter. Manager takes + * shared ownership. + */ + explicit ConfigurationManager(std::shared_ptr transport); + + ~ConfigurationManager(); + + ConfigurationManager(const ConfigurationManager &) = delete; + ConfigurationManager & operator=(const ConfigurationManager &) = delete; + ConfigurationManager(ConfigurationManager &&) = delete; + ConfigurationManager & operator=(ConfigurationManager &&) = delete; + + /// Idempotent teardown. Forwards to the transport's shutdown so cached + /// parameter clients drop their references before rclcpp::shutdown() runs. + /// Must be called before rclcpp::shutdown() to prevent use-after-free of + /// the parameter-client internal node held by the transport. + void shutdown(); + + /// List all parameters for a node. + /// @param node_name Fully qualified node name (e.g., "/powertrain/engine/engine_temp_sensor"). + /// @return ParameterResult with array of {name, value, type} objects. + ParameterResult list_parameters(const std::string & node_name); + + /// Get a specific parameter value. + /// @param node_name Fully qualified node name. + /// @param param_name Parameter name. + /// @return ParameterResult with {name, value, type, description, read_only}. + ParameterResult get_parameter(const std::string & node_name, const std::string & param_name); + + /// Set a parameter value. + /// @param node_name Fully qualified node name. + /// @param param_name Parameter name. + /// @param value New value (JSON type will be converted to the appropriate ROS 2 type by the transport). + /// @return ParameterResult with {name, value, type}. + ParameterResult set_parameter(const std::string & node_name, const std::string & param_name, const json & value); + + /// Reset a specific parameter to its default (initial) value. + /// @param node_name Fully qualified node name. + /// @param param_name Parameter name. + /// @return ParameterResult with reset parameter info. + ParameterResult reset_parameter(const std::string & node_name, const std::string & param_name); + + /// Reset all parameters of a node to their default (initial) values. + /// @param node_name Fully qualified node name. + /// @return ParameterResult with count of reset parameters. + ParameterResult reset_all_parameters(const std::string & node_name); + + private: + /// Return SHUT_DOWN error result. + static ParameterResult shut_down_result(); + + std::shared_ptr transport_; + + /// Shutdown flag - prevents delegation to the transport after teardown. + std::atomic shutdown_{false}; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/data_access_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/data_access_manager.hpp new file mode 100644 index 000000000..a2bf7722b --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/data_access_manager.hpp @@ -0,0 +1,132 @@ +// 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 "ros2_medkit_gateway/core/transports/topic_transport.hpp" + +namespace ros2_medkit_serialization { +class TypeIntrospection; // forward decl, defined in ros2_medkit_serialization/type_introspection.hpp +} // namespace ros2_medkit_serialization + +namespace ros2_medkit_gateway { + +using json = nlohmann::json; + +class TopicDataProvider; // forward decl, see core/data/topic_data_provider.hpp + +/** + * @brief Application service for topic publish + sample. + * + * Pure C++; ROS-side I/O is performed by the injected TopicTransport adapter + * (typically Ros2TopicTransport). Type introspection and the per-entity + * TopicDataProvider pointer remain on the manager because they are consumed + * directly by handlers and the discovery manager. + */ +class DataAccessManager { + public: + /** + * @param transport Concrete TopicTransport adapter. Manager takes shared + * ownership. + * @param topic_sample_timeout_sec Default sample timeout in seconds. Used + * when callers pass a negative timeout. + */ + explicit DataAccessManager(std::shared_ptr transport, double topic_sample_timeout_sec = 1.0); + + ~DataAccessManager() = default; + + DataAccessManager(const DataAccessManager &) = delete; + DataAccessManager & operator=(const DataAccessManager &) = delete; + DataAccessManager(DataAccessManager &&) = delete; + DataAccessManager & operator=(DataAccessManager &&) = delete; + + /** + * @brief Publish data to a specific topic. + * + * @param topic_path Full topic path (e.g., /chassis/brakes/command). + * @param msg_type ROS 2 message type (e.g., std_msgs/msg/Float32). + * @param data JSON data to publish. + * @param timeout_sec Timeout for the publish operation. + * @return JSON with publish status. + */ + json publish_to_topic(const std::string & topic_path, const std::string & msg_type, const json & data, + double timeout_sec = 5.0); + + /** + * @brief Get topic sample with fallback to metadata on timeout. + * + * If the topic is publishing, returns actual data with type info. If the + * topic times out, returns metadata (type, schema, pub/sub counts) instead + * of an error. + * + * @param topic_name Full topic path. + * @param timeout_sec Timeout for data retrieval. Use -1.0 to use the + * configured default. + */ + json get_topic_sample_with_fallback(const std::string & topic_name, double timeout_sec = -1.0); + + /** + * @brief Get single topic sample using the native fast path. + * + * Fast path for single-topic sampling. When no publishers are present, + * returns metadata-only without calling into the transport. + * + * @param topic_name Full topic path. + * @param timeout_sec Timeout for sampling (only used if topic has publishers). + */ + json get_topic_sample_native(const std::string & topic_name, double timeout_sec = 1.0); + + /** + * @brief Get the type introspection instance (used by handlers and discovery). + * + * Forwarded to the transport adapter, which owns the rclcpp-coupled + * implementation. May return nullptr for transports that do not support + * introspection. + */ + ros2_medkit_serialization::TypeIntrospection * get_type_introspection() const { + return transport_ ? transport_->get_type_introspection() : nullptr; + } + + /** + * @brief Attach a TopicDataProvider for sampling. + * + * The provider owns the pool-backed subscription path. Non-owning pointer; + * caller retains ownership. Safe to call once at wiring time. + */ + void set_topic_data_provider(TopicDataProvider * provider) { + topic_data_provider_ = provider; + } + + TopicDataProvider * get_topic_data_provider() const { + return topic_data_provider_; + } + + /** + * @brief Get the configured topic sample timeout in seconds. + */ + double get_topic_sample_timeout() const { + return topic_sample_timeout_sec_; + } + + private: + std::shared_ptr transport_; + TopicDataProvider * topic_data_provider_{nullptr}; ///< Non-owning; set at wiring time. + double topic_sample_timeout_sec_; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/fault_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/fault_manager.hpp new file mode 100644 index 000000000..85798df2a --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/fault_manager.hpp @@ -0,0 +1,104 @@ +// 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 "ros2_medkit_gateway/core/faults/fault_types.hpp" +#include "ros2_medkit_gateway/core/transports/fault_service_transport.hpp" + +namespace ros2_medkit_gateway { + +using json = nlohmann::json; + +/** + * @brief Application service for fault-management operations. + * + * Pure C++; ROS-side I/O is performed by the injected FaultServiceTransport + * adapter (typically Ros2FaultServiceTransport). All + * `rclcpp::Client` instances, the seven per-client + * mutexes, and the message-to-JSON conversion helpers live in the adapter. + * + * The manager body delegates each of the eight SOVD operations to the + * transport and returns its neutral FaultResult / FaultWithEnvJsonResult + * structures. The handler-facing public API is preserved verbatim across + * the refactor; the only behaviour change is that get_fault_with_env now + * returns the response body as JSON ("fault" + "environment_data") rather + * than as raw message types - the transport performs the conversion. + */ +class FaultManager { + public: + /** + * @param transport Concrete FaultServiceTransport adapter. Manager takes + * shared ownership. + */ + explicit FaultManager(std::shared_ptr transport); + + ~FaultManager() = default; + + FaultManager(const FaultManager &) = delete; + FaultManager & operator=(const FaultManager &) = delete; + FaultManager(FaultManager &&) = delete; + FaultManager & operator=(FaultManager &&) = delete; + + /// Report a fault from a component. + /// @param fault_code Unique fault identifier + /// @param severity Fault severity (0=INFO, 1=WARN, 2=ERROR, 3=CRITICAL) + /// @param description Human-readable description + /// @param source_id Component identifier (namespace path) + FaultResult report_fault(const std::string & fault_code, uint8_t severity, const std::string & description, + const std::string & source_id); + + /// Get all faults, optionally filtered by component (prefix match on source_id). + FaultResult list_faults(const std::string & source_id = "", bool include_prefailed = true, + bool include_confirmed = true, bool include_cleared = false, bool include_healed = false, + bool include_muted = false, bool include_clusters = false); + + /// Get a specific fault by code with environment data, returned as JSON. + /// `data` carries `{ "fault": {...}, "environment_data": {...} }`. The + /// rosbag-snapshot bulk_data_uri is intentionally NOT included; per-request + /// URL building belongs to the handler that knows the entity path. + FaultWithEnvJsonResult get_fault_with_env(const std::string & fault_code, const std::string & source_id = ""); + + /// Get a specific fault by code (JSON result - "fault" body only). + FaultResult get_fault(const std::string & fault_code, const std::string & source_id = ""); + + /// Clear a fault. + FaultResult clear_fault(const std::string & fault_code); + + /// Get snapshots for a fault (optional topic filter). + FaultResult get_snapshots(const std::string & fault_code, const std::string & topic = ""); + + /// Get rosbag file info for a fault. + FaultResult get_rosbag(const std::string & fault_code); + + /// Get all rosbag files for an entity (batch operation). + FaultResult list_rosbags(const std::string & entity_fqn); + + /// Check if fault manager services are available. + bool is_available() const; + + /// Wait for fault manager services to become available (forwards to transport). + bool wait_for_services(std::chrono::duration timeout); + + private: + std::shared_ptr transport_; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/log_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/log_manager.hpp new file mode 100644 index 000000000..226691404 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/log_manager.hpp @@ -0,0 +1,208 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/core/log_types.hpp" +#include "ros2_medkit_gateway/core/providers/log_provider.hpp" +#include "ros2_medkit_gateway/core/providers/log_provider_registry.hpp" +#include "ros2_medkit_gateway/core/transports/log_source.hpp" + +namespace ros2_medkit_gateway { + +using json = nlohmann::json; + +class ResourceChangeNotifier; // forward declaration + +/** + * @brief Manages log collection via the default ring-buffer backend. + * + * Pure C++ application service. Middleware-side ingestion is performed by an + * injected LogSource adapter (typically Ros2LogSource) that subscribes to + * /rosout and converts each message into a neutral LogEntry. The manager body + * keeps its ring-buffer storage, per-entity config map, monotonic id counter, + * the plugin observer pattern, and the manages_ingestion short-circuit. None + * of those depend on ROS types. + * + * Plugin integration (two modes): + * - **Observer mode** (default): If a LogProvider plugin is registered, get_logs() + * and get_config() delegate to it. on_log_entry() is called on ALL LogProvider + * observers for every LogEntry produced by the source. Observers returning true + * suppress ring-buffer storage. + * - **Full ingestion** (manages_ingestion() == true): The primary LogProvider owns + * the entire log pipeline. LogManager never calls source->start(), so no + * subscription is created. All queries and config operations delegate to the plugin. + * + * FQN normalization: + * - entity.fqn from the entity cache has a leading '/' (e.g. "/powertrain/engine/temp_sensor") + * - Source-emitted entries DO NOT have a leading '/' (rcl_node_get_logger_name convention) + * - Callers pass raw FQNs from entity.fqn; LogManager strips leading '/' internally. + */ +class LogManager { + public: + /// Default maximum number of entries retained per node in the ring buffer + static constexpr size_t kDefaultBufferSize = 200; + + /** + * @brief Construct LogManager + * + * If the primary LogProvider's manages_ingestion() returns true, source->start() + * is never called, so no subscription is created. Otherwise the source is + * started with on_log_entry() as the entry-point callback. + * + * @param source LogSource adapter (typically Ros2LogSource). + * Manager takes shared ownership. + * @param provider_registry LogProviderRegistry port for LogProvider lookup + * (typically the PluginManager). May be nullptr - + * equivalent to "no plugins registered". + * @param max_buffer_size Ring buffer size per node (override for unit testing) + */ + explicit LogManager(std::shared_ptr source, LogProviderRegistry * provider_registry = nullptr, + size_t max_buffer_size = kDefaultBufferSize); + + ~LogManager(); + + LogManager(const LogManager &) = delete; + LogManager & operator=(const LogManager &) = delete; + LogManager(LogManager &&) = delete; + LogManager & operator=(LogManager &&) = delete; + + /** + * @brief Query log entries for a set of node FQNs + * + * If a LogProvider plugin is registered, delegates to it. + * Otherwise uses the local ring buffer. + * + * @param node_fqns Node FQNs from entity.fqn (WITH leading '/' - normalized internally) + * @param prefix_match If true, match all buffered nodes whose name starts with the given prefix + * (used for Component queries). If false, exact match (App queries). + * @param min_severity Additional severity filter from query parameter. Empty = no override. + * @param context_filter Substring filter applied to log entry's name (logger name). Empty = no filter. + * @param entity_id Entity ID for config lookup. Empty = use defaults. + * @return JSON array of LogEntry objects sorted by id ascending, capped at entity config max_entries. + */ + tl::expected get_logs(const std::vector & node_fqns, bool prefix_match, + const std::string & min_severity, const std::string & context_filter, + const std::string & entity_id); + + /// Get current log configuration for entity (returns defaults if unconfigured) + tl::expected get_config(const std::string & entity_id) const; + + /** + * @brief Update log configuration for an entity + * @return Empty string on success, error message on validation failure + */ + std::string update_config(const std::string & entity_id, const std::optional & severity_filter, + const std::optional & max_entries); + + /** + * @brief Programmatically add a log entry (e.g. from trigger log_settings) + * + * Creates a LogEntry and pushes it to the internal ring buffer using the + * same path as on_log_entry(). If a ResourceChangeNotifier is set, emits a + * "logs" CREATED notification so triggers can observe log changes. + * + * @param entity_id Entity to associate the log with (used as logger name) + * @param severity SOVD severity string (debug, info, warning, error, fatal) + * @param message Human-readable log message + * @param metadata Additional JSON metadata stored in the message (appended) + */ + void add_log_entry(const std::string & entity_id, const std::string & severity, const std::string & message, + const nlohmann::json & metadata); + + /// Set the ResourceChangeNotifier for emitting log change events. + /// Called by GatewayNode after both LogManager and the notifier are available. + void set_notifier(ResourceChangeNotifier * notifier); + + /// Callback that maps a logger FQN to a manifest entity ID. + /// Returns empty string if the FQN cannot be resolved. + using NodeToEntityFn = std::function; + + /// Set the node-to-entity resolver for trigger notifications. + /// When set, on_log_entry() resolves logger names to manifest entity IDs before + /// notifying the ResourceChangeNotifier. + void set_node_to_entity_resolver(NodeToEntityFn resolver); + + // ---- Static utilities (no middleware required - safe in unit tests) ---- + + /// Convert ROS 2 uint8 log level -> SOVD severity string ("debug" for unknown levels) + static std::string level_to_severity(uint8_t level); + + /// Convert SOVD severity string -> ROS 2 uint8 log level (0 for invalid/empty) + static uint8_t severity_to_level(const std::string & severity); + + /// Check if a severity string is valid (one of: debug, info, warning, error, fatal) + static bool is_valid_severity(const std::string & severity); + + /// Format a LogEntry as SOVD JSON (id, timestamp, severity, message, context) + static json entry_to_json(const LogEntry & entry); + + /// Strip leading '/' from a node FQN for ring-buffer key normalization + static std::string normalize_fqn(const std::string & fqn); + + // ---- Test injection (for unit tests - do not use in production code) ---- + + /** + * @brief Inject a log entry directly into the ring buffer (bypasses the source) + * + * Used by unit tests to populate the buffer without a live source feed. + * In production the buffer is populated exclusively by source-emitted entries. + */ + void inject_entry_for_testing(LogEntry entry); + + private: + /// Entry point invoked for every LogEntry produced by the source. Performs + /// observer notification, ring-buffer storage, and trigger-side notifier + /// dispatch. + void on_log_entry(const LogEntry & entry); + + /// Get the effective LogProvider: plugin if registered, else nullptr (use local buffer) + LogProvider * effective_provider() const; + + std::shared_ptr source_; + LogProviderRegistry * provider_registry_; + ResourceChangeNotifier * notifier_ = nullptr; + /// Write-once: must be set before the source starts delivering entries. + /// After that, only read from source callbacks (no mutex needed). + NodeToEntityFn node_to_entity_resolver_; + size_t max_buffer_size_; + + // Ring buffers keyed by normalized node name (no leading '/') + // e.g. "powertrain/engine/temp_sensor" -> deque + std::unordered_map> buffers_; + mutable std::mutex buffers_mutex_; + + // Per-entity configuration keyed by entity_id + std::unordered_map configs_; + mutable std::mutex configs_mutex_; + + // Monotonically increasing entry ID (never resets; starts at 1) + std::atomic next_id_{1}; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/operation_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/operation_manager.hpp new file mode 100644 index 000000000..51e740d73 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/operation_manager.hpp @@ -0,0 +1,189 @@ +// 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 +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/core/discovery/service_action_resolver.hpp" +#include "ros2_medkit_gateway/core/operations/operation_types.hpp" +#include "ros2_medkit_gateway/core/transports/action_transport.hpp" +#include "ros2_medkit_gateway/core/transports/service_transport.hpp" + +namespace ros2_medkit_gateway { + +class ResourceChangeNotifier; + +using json = nlohmann::json; + +/** + * @brief Application service for ROS 2 service / action operations. + * + * Pure C++; ROS-side I/O is performed by the injected ServiceTransport and + * ActionTransport adapters. Goal tracking, UUID generation, type validation + * and component-namespace resolution remain in the manager body. + */ +class OperationManager { + public: + /** + * @param service_transport Concrete ServiceTransport adapter (typically Ros2ServiceTransport). + * @param action_transport Concrete ActionTransport adapter (typically Ros2ActionTransport). + * @param resolver Service / action lookup interface (DiscoveryManager implements it). + * Must outlive this manager. May be nullptr in tests that do not exercise + * the component-name resolution paths. + * @param service_call_timeout_sec Timeout in seconds applied to every service / action call. + */ + OperationManager(std::shared_ptr service_transport, + std::shared_ptr action_transport, ServiceActionResolver * resolver, + int service_call_timeout_sec = 10); + + ~OperationManager(); + + OperationManager(const OperationManager &) = delete; + OperationManager & operator=(const OperationManager &) = delete; + OperationManager(OperationManager &&) = delete; + OperationManager & operator=(OperationManager &&) = delete; + + /// Idempotent teardown. Clears tracked goals and unsubscribes from + /// status topics. Subsequent calls are no-ops. Transport teardown is + /// owned by the transport destructors. + void shutdown(); + + /// Set optional notifier for broadcasting operation status changes to trigger subsystem. + void set_notifier(ResourceChangeNotifier * notifier); + + /// Call a ROS 2 service synchronously through the ServiceTransport. + ServiceCallResult call_service(const std::string & service_path, const std::string & service_type, + const json & request); + + /// Find and call a service by component and operation name. + /// Resolves the path / type via the ServiceActionResolver when type is unset. + ServiceCallResult call_component_service(const std::string & component_ns, const std::string & operation_name, + const std::optional & service_type, const json & request); + + /// Validate message type format (package/srv/Type or package/action/Type or package/msg/Type). + static bool is_valid_message_type(const std::string & type); + + /// Validate UUID hex string format (32 hex characters). + static bool is_valid_uuid_hex(const std::string & uuid_hex); + + /// Check if type is a service type (contains /srv/). + static bool is_service_type(const std::string & type); + + /// Check if type is an action type (contains /action/). + static bool is_action_type(const std::string & type); + + // ==================== ACTION OPERATIONS ==================== + + /// Send a goal to an action server through the ActionTransport. + ActionSendGoalResult send_action_goal(const std::string & action_path, const std::string & action_type, + const json & goal, const std::string & entity_id = ""); + + /// Send a goal using component namespace + operation name. Resolves through + /// the ServiceActionResolver when type is unset. + ActionSendGoalResult send_component_action_goal(const std::string & component_ns, const std::string & operation_name, + const std::optional & action_type, const json & goal, + const std::string & entity_id = ""); + + /// Cancel a running action goal. + ActionCancelResult cancel_action_goal(const std::string & action_path, const std::string & goal_id); + + /// Get the result of a completed action. + ActionGetResultResult get_action_result(const std::string & action_path, const std::string & action_type, + const std::string & goal_id); + + /// Get tracked goal info by goal_id. + std::optional get_tracked_goal(const std::string & goal_id) const; + + /// List all tracked goals. + std::vector list_tracked_goals() const; + + /// Get all goals for a specific action path, sorted by created_at (newest first). + std::vector get_goals_for_action(const std::string & action_path) const; + + /// Get the most recent goal for a specific action path. + std::optional get_latest_goal_for_action(const std::string & action_path) const; + + /// Update goal status in tracking. No-op if the goal is unknown. + void update_goal_status(const std::string & goal_id, ActionGoalStatus status); + + /// Update goal feedback in tracking. No-op if the goal is unknown. + void update_goal_feedback(const std::string & goal_id, const json & feedback); + + /// Remove completed goals older than max_age. Forwards the unsubscribe + /// request to the action transport for each path that becomes empty. + void cleanup_old_goals(std::chrono::seconds max_age = std::chrono::seconds(300)); + + /// Subscribe (idempotently) to action status updates. Wires the transport + /// callback into update_goal_status() so external status changes update + /// the tracking map on the transport's executor thread. + void subscribe_to_action_status(const std::string & action_path); + + /// Unsubscribe from action status updates. Idempotent. + void unsubscribe_from_action_status(const std::string & action_path); + + private: + /// Convert UUID hex string to JSON array of byte values. + static json uuid_hex_to_json_array(const std::string & uuid_hex); + + /// Generate a random version-4 UUID (16 bytes). + std::array generate_uuid(); + + /// Convert UUID bytes to JSON array. + static json uuid_bytes_to_json_array(const std::array & uuid); + + /// Convert UUID bytes to lowercase hex string (no separators). + static std::string uuid_bytes_to_hex(const std::array & uuid); + + /// Track a new goal in the local map. + void track_goal(const std::string & goal_id, const std::string & action_path, const std::string & action_type, + const std::string & entity_id); + + /// Propagate a goal status change observed via the action transport into + /// the tracking map, firing the resource-change notifier on transitions. + void on_status_callback(const std::string & action_path, const std::string & goal_id, ActionGoalStatus status); + + std::shared_ptr service_transport_; + std::shared_ptr action_transport_; + ServiceActionResolver * resolver_; + ResourceChangeNotifier * notifier_ = nullptr; + + /// RNG for UUID generation. Guarded by rng_mutex_. + std::mutex rng_mutex_; + std::mt19937 rng_; + + /// Service / action call timeout in seconds. + int service_call_timeout_sec_; + + /// Map of goal_id -> ActionGoalInfo for tracking active goals. + mutable std::mutex goals_mutex_; + std::map tracked_goals_; + + /// Set of action paths the manager has asked the transport to subscribe to. + /// Used so cleanup_old_goals can issue a single unsubscribe per path. + mutable std::mutex subscriptions_mutex_; + std::map subscribed_paths_; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/trigger_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/trigger_manager.hpp index ac45da3c3..0c0e03946 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/trigger_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/trigger_manager.hpp @@ -32,13 +32,13 @@ #include "ros2_medkit_gateway/core/condition_evaluator.hpp" #include "ros2_medkit_gateway/core/resource_change_notifier.hpp" +#include "ros2_medkit_gateway/core/transports/topic_subscription_transport.hpp" #include "ros2_medkit_gateway/core/trigger_store.hpp" namespace ros2_medkit_gateway { // Forward declarations class LogManager; -class TriggerTopicSubscriber; /// Error categories for trigger create/update operations. enum class TriggerError { ValidationError, CapacityExceeded, PersistenceError, NotFound }; @@ -81,8 +81,18 @@ struct TriggerConfig { /// matching and persistence via TriggerStore. class TriggerManager { public: + /// Construct the trigger manager. + /// + /// @param notifier Resource-change notifier the manager subscribes to. + /// @param conditions Registry of condition evaluators (OnChange, etc.). + /// @param store Persistence store for trigger metadata + state. + /// @param config Capacity + on-restart behaviour. + /// @param topic_transport Optional transport used to register data-trigger + /// topic subscriptions. When null the manager retains + /// its CRUD/dispatch behaviour but never subscribes + /// (useful in tests that don't need topic routing). TriggerManager(ResourceChangeNotifier & notifier, ConditionRegistry & conditions, TriggerStore & store, - const TriggerConfig & config); + const TriggerConfig & config, std::shared_ptr topic_transport = nullptr); ~TriggerManager(); // Non-copyable, non-movable (owns trigger state and notifier subscription) @@ -143,10 +153,6 @@ class TriggerManager { /// Set the entity hierarchy resolver. Called by GatewayNode after cache is available. void set_entity_children_fn(EntityChildrenFn fn); - /// Set the topic subscriber for data trigger subscriptions. - /// Called by GatewayNode after TriggerTopicSubscriber is created. - void set_topic_subscriber(TriggerTopicSubscriber * subscriber); - /// Set the LogManager for trigger log_settings integration. /// Called by GatewayNode after both TriggerManager and LogManager are available. void set_log_manager(LogManager * log_manager); @@ -159,7 +165,8 @@ class TriggerManager { void set_resolve_topic_fn(ResolveTopicFn fn); /// Retry resolving data triggers whose topic names were unknown at creation. - /// Called periodically by TriggerTopicSubscriber's retry timer. + /// Called periodically (today: from the rclcpp adapter's retry tick) so + /// that triggers stuck without a topic name get a chance to subscribe. void retry_unresolved_triggers(); // --- Entity existence check (for orphan sweep) ---------------------------- @@ -254,8 +261,15 @@ class TriggerManager { mutable std::mutex hierarchy_mutex_; EntityChildrenFn entity_children_fn_; - // Data trigger topic subscriber (non-owning, optional) - TriggerTopicSubscriber * topic_subscriber_{nullptr}; + // Data trigger topic transport (shared, optional). The adapter side owns + // the rclcpp resources; the manager only stores per-trigger handles. + std::shared_ptr topic_transport_; + + // Per-trigger live subscription handles. Destruction of a handle + // unsubscribes the underlying topic; the manager removes entries from + // this map on remove()/cleanup_expired_trigger() so the dtor side-effect + // ties subscription lifetime to trigger lifetime. Guarded by triggers_mutex_. + std::unordered_map> topic_handles_; // LogManager for log_settings integration (non-owning, optional) LogManager * log_manager_{nullptr}; diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/operations/operation_types.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/operations/operation_types.hpp new file mode 100644 index 000000000..e06a32407 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/operations/operation_types.hpp @@ -0,0 +1,83 @@ +// 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 + +namespace ros2_medkit_gateway { + +using json = nlohmann::json; + +/// Result of a synchronous service call. +struct ServiceCallResult { + bool success; + json response; + std::string error_message; +}; + +/// Action goal status (matches ROS 2 action_msgs/msg/GoalStatus enum values +/// but is a neutral C++ enum that does not require action_msgs include). +enum class ActionGoalStatus : int8_t { + UNKNOWN = 0, + ACCEPTED = 1, + EXECUTING = 2, + CANCELING = 3, + SUCCEEDED = 4, + CANCELED = 5, + ABORTED = 6 +}; + +/// Convert status enum to lowercase string ("succeeded", "canceled", etc.). +std::string action_status_to_string(ActionGoalStatus status); + +/// Result of sending an action goal. +struct ActionSendGoalResult { + bool success; + std::string goal_id; ///< UUID hex string + bool goal_accepted; + std::string error_message; +}; + +/// Result of canceling an action goal. +struct ActionCancelResult { + bool success; + int8_t return_code; ///< 0=accepted, 1=rejected, 2=unknown_id, 3=terminated + std::string error_message; +}; + +/// Result of getting an action result. +struct ActionGetResultResult { + bool success; + ActionGoalStatus status; + json result; + std::string error_message; +}; + +/// Tracked action goal info (stored locally). +struct ActionGoalInfo { + std::string goal_id; + std::string action_path; ///< e.g., /powertrain/engine/long_calibration + std::string action_type; ///< e.g., example_interfaces/action/Fibonacci + std::string entity_id; ///< SOVD entity ID (e.g., engine) for trigger notifications + ActionGoalStatus status; + json last_feedback; + std::chrono::system_clock::time_point created_at; + std::chrono::system_clock::time_point last_update; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/plugins/plugin_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/plugins/plugin_manager.hpp index 079dea3c1..1f1aabc85 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/plugins/plugin_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/plugins/plugin_manager.hpp @@ -23,6 +23,7 @@ #include "ros2_medkit_gateway/core/providers/fault_provider.hpp" #include "ros2_medkit_gateway/core/providers/introspection_provider.hpp" #include "ros2_medkit_gateway/core/providers/log_provider.hpp" +#include "ros2_medkit_gateway/core/providers/log_provider_registry.hpp" #include "ros2_medkit_gateway/core/providers/operation_provider.hpp" #include "ros2_medkit_gateway/core/providers/script_provider.hpp" #include "ros2_medkit_gateway/core/providers/update_provider.hpp" @@ -60,10 +61,10 @@ namespace ros2_medkit_gateway { * non-owning provider pointers (e.g. UpdateManager). In GatewayNode, declare * plugin_mgr_ BEFORE update_mgr_ so that destruction order is safe. */ -class PluginManager { +class PluginManager : public LogProviderRegistry { public: PluginManager() = default; - ~PluginManager(); + ~PluginManager() override; // Non-copyable, non-movable (owns dlopen handles) PluginManager(const PluginManager &) = delete; @@ -155,6 +156,17 @@ class PluginManager { */ std::vector get_log_observers() const; + // LogProviderRegistry implementation - thin forwards over the existing + // get_log_provider / get_log_observers accessors. Keeps the LogManager + // dependency expressed via the neutral port so the manager body links + // against gateway_core alone. + LogProvider * primary_log_provider() const override { + return get_log_provider(); + } + std::vector log_observers() const override { + return get_log_observers(); + } + /** * @brief Get all introspection providers with their plugin names * @return (plugin_name, provider) pairs for all IntrospectionProvider plugins diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/providers/log_provider_registry.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/providers/log_provider_registry.hpp new file mode 100644 index 000000000..7a68f3b35 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/providers/log_provider_registry.hpp @@ -0,0 +1,57 @@ +// 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 + +namespace ros2_medkit_gateway { + +class LogProvider; // forward declaration + +/** + * @brief Port: lookup of LogProvider plugins for the LogManager. + * + * Decouples LogManager from PluginManager so the manager body can live in the + * middleware-neutral build layer alongside the other managers. The production + * adapter is PluginManager itself (which implements this interface inline); + * unit tests can supply a lightweight mock instead. + * + * The two methods correspond to the two LogManager-internal usages: + * - primary(): consulted at construction (manages_ingestion short-circuit) + * and at every get_logs / get_config / update_config call to + * decide whether to delegate to a plugin. + * - observers(): consulted on every source-emitted entry so the observer + * pattern can fan out to all loaded plugins. + */ +class LogProviderRegistry { + public: + LogProviderRegistry() = default; + LogProviderRegistry(const LogProviderRegistry &) = delete; + LogProviderRegistry & operator=(const LogProviderRegistry &) = delete; + LogProviderRegistry(LogProviderRegistry &&) = delete; + LogProviderRegistry & operator=(LogProviderRegistry &&) = delete; + virtual ~LogProviderRegistry() = default; + + /// First-loaded LogProvider plugin (used as the primary backend for queries + /// and config) or nullptr when no such plugin is registered. + virtual LogProvider * primary_log_provider() const = 0; + + /// All loaded LogProvider plugins (observer pattern: each receives every + /// source-emitted LogEntry). Order is implementation-defined; LogManager + /// iterates the full list. + virtual std::vector log_observers() const = 0; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/action_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/action_transport.hpp new file mode 100644 index 000000000..7ac541afd --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/action_transport.hpp @@ -0,0 +1,57 @@ +// 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 "ros2_medkit_gateway/core/operations/operation_types.hpp" + +namespace ros2_medkit_gateway { + +class ActionTransport { + public: + /// Status update from a status-topic subscription. The action_path and the + /// new status for one tracked goal. Manager updates its own tracking map. + using StatusCallback = + std::function; + + ActionTransport() = default; + ActionTransport(const ActionTransport &) = delete; + ActionTransport & operator=(const ActionTransport &) = delete; + ActionTransport(ActionTransport &&) = delete; + ActionTransport & operator=(ActionTransport &&) = delete; + virtual ~ActionTransport() = default; + + virtual ActionSendGoalResult send_goal(const std::string & action_path, const std::string & action_type, + const json & goal, std::chrono::duration timeout) = 0; + + virtual ActionCancelResult cancel_goal(const std::string & action_path, const std::string & goal_id, + std::chrono::duration timeout) = 0; + + virtual ActionGetResultResult get_result(const std::string & action_path, const std::string & action_type, + const std::string & goal_id, std::chrono::duration timeout) = 0; + + /// Subscribe to status updates for an action path. The callback fires when + /// any tracked goal's status changes. Subsequent calls for the same path + /// are no-ops (idempotent). + virtual void subscribe_status(const std::string & action_path, StatusCallback callback) = 0; + + virtual void unsubscribe_status(const std::string & action_path) = 0; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/fault_service_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/fault_service_transport.hpp new file mode 100644 index 000000000..63963d085 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/fault_service_transport.hpp @@ -0,0 +1,63 @@ +// 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 "ros2_medkit_gateway/core/faults/fault_types.hpp" + +namespace ros2_medkit_gateway { + +/// Port for the seven services provided by the ros2_medkit_fault_manager +/// package. The transport wraps `rclcpp::Client`, +/// converts message types to JSON internally, and returns the neutral +/// FaultResult / FaultWithEnvJsonResult structs already filled. +class FaultServiceTransport { + public: + FaultServiceTransport() = default; + FaultServiceTransport(const FaultServiceTransport &) = delete; + FaultServiceTransport & operator=(const FaultServiceTransport &) = delete; + FaultServiceTransport(FaultServiceTransport &&) = delete; + FaultServiceTransport & operator=(FaultServiceTransport &&) = delete; + virtual ~FaultServiceTransport() = default; + + virtual FaultResult report_fault(const std::string & fault_code, uint8_t severity, const std::string & description, + const std::string & source_id) = 0; + + virtual FaultResult list_faults(const std::string & source_id, bool include_prefailed, bool include_confirmed, + bool include_cleared, bool include_healed, bool include_muted, + bool include_clusters) = 0; + + virtual FaultWithEnvJsonResult get_fault_with_env(const std::string & fault_code, const std::string & source_id) = 0; + + virtual FaultResult get_fault(const std::string & fault_code, const std::string & source_id) = 0; + + virtual FaultResult clear_fault(const std::string & fault_code) = 0; + + virtual FaultResult get_snapshots(const std::string & fault_code, const std::string & topic) = 0; + + virtual FaultResult get_rosbag(const std::string & fault_code) = 0; + + virtual FaultResult list_rosbags(const std::string & entity_fqn) = 0; + + virtual bool wait_for_services(std::chrono::duration timeout) = 0; + + virtual bool is_available() const = 0; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/log_source.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/log_source.hpp new file mode 100644 index 000000000..f65e0f7b4 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/log_source.hpp @@ -0,0 +1,48 @@ +// 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 "ros2_medkit_gateway/core/log_types.hpp" + +namespace ros2_medkit_gateway { + +/// Port: source of /rosout-like log entries. The adapter +/// (`Ros2LogSource`) subscribes to /rosout and delivers each message as a +/// neutral LogEntry, normalising logger name (no leading slash) and SOVD +/// severity. Manager keeps the ring-buffer storage; the source merely +/// produces entries. +class LogSource { + public: + using EntryCallback = std::function; + + LogSource() = default; + LogSource(const LogSource &) = delete; + LogSource & operator=(const LogSource &) = delete; + LogSource(LogSource &&) = delete; + LogSource & operator=(LogSource &&) = delete; + virtual ~LogSource() = default; + + /// Start delivering entries. Idempotent. Calling start() twice replaces the + /// previous callback. + virtual void start(EntryCallback callback) = 0; + + /// Stop delivering entries. Idempotent. After stop() returns, the callback + /// is guaranteed not to fire again. + virtual void stop() = 0; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/parameter_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/parameter_transport.hpp new file mode 100644 index 000000000..7aed276bc --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/parameter_transport.hpp @@ -0,0 +1,90 @@ +// 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 "ros2_medkit_gateway/core/configuration/parameter_types.hpp" + +namespace ros2_medkit_gateway { + +/// Single parameter descriptor returned by list_parameters. +struct ParameterDescriptor { + std::string name; + std::string type; ///< "bool", "integer", "double", "string", "byte_array", + ///< "bool_array", "integer_array", "double_array", "string_array" + json value = nullptr; + std::string description; + bool read_only = false; +}; + +class ParameterTransport { + public: + ParameterTransport() = default; + ParameterTransport(const ParameterTransport &) = delete; + ParameterTransport & operator=(const ParameterTransport &) = delete; + ParameterTransport(ParameterTransport &&) = delete; + ParameterTransport & operator=(ParameterTransport &&) = delete; + virtual ~ParameterTransport() = default; + + /// True iff `node_name` resolves to the gateway's own node (self-parameter + /// lookup short-circuit). Manager uses this to avoid IPC for own params. + virtual bool is_self_node(const std::string & node_name) const = 0; + + /// List all parameters with descriptors for `node_name`. + virtual ParameterResult list_parameters(const std::string & node_name) = 0; + + /// Get one parameter with descriptor. + virtual ParameterResult get_parameter(const std::string & node_name, const std::string & param_name) = 0; + + /// Set one parameter; result.data echoes the committed descriptor. + virtual ParameterResult set_parameter(const std::string & node_name, const std::string & param_name, + const json & value) = 0; + + /// Self-node variants. Read-fast path that does not go via IPC. + virtual ParameterResult list_own_parameters() = 0; + virtual ParameterResult get_own_parameter(const std::string & param_name) = 0; + + /// Look up the cached default (initial) value for a single parameter. + /// On success, `result.data` is the JSON value of the default. The transport + /// owns the defaults cache; manager-level reset operations compose this with + /// `set_parameter` to perform the actual reset. + /// + /// Error codes follow the same contract as the rest of the surface: + /// - NO_DEFAULTS_CACHED: nothing cached for `node_name` yet + /// - NOT_FOUND: the node has cached defaults but not for `param_name` + /// - SERVICE_UNAVAILABLE / TIMEOUT / SHUT_DOWN: as per IPC reachability + virtual ParameterResult get_default(const std::string & node_name, const std::string & param_name) = 0; + + /// List all cached default (initial) values for a node. On success, + /// `result.data` is a JSON array of `{"name", "value", "type"}` entries. + /// Used by `reset_all_parameters` to drive a chain of `set_parameter` calls. + virtual ParameterResult list_defaults(const std::string & node_name) = 0; + + /// Service availability check (same semantics as today's negative cache). + virtual bool is_node_available(const std::string & node_name) const = 0; + + /// Drop client/cached state for a given node (called by the manager when + /// reset / refresh is requested). + virtual void invalidate(const std::string & node_name) = 0; + + /// Tear down all cached clients before rclcpp::shutdown(). Idempotent. + virtual void shutdown() = 0; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/service_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/service_transport.hpp new file mode 100644 index 000000000..c208f7c95 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/service_transport.hpp @@ -0,0 +1,38 @@ +// 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 "ros2_medkit_gateway/core/operations/operation_types.hpp" + +namespace ros2_medkit_gateway { + +class ServiceTransport { + public: + ServiceTransport() = default; + ServiceTransport(const ServiceTransport &) = delete; + ServiceTransport & operator=(const ServiceTransport &) = delete; + ServiceTransport(ServiceTransport &&) = delete; + ServiceTransport & operator=(ServiceTransport &&) = delete; + virtual ~ServiceTransport() = default; + + virtual ServiceCallResult call(const std::string & service_path, const std::string & service_type, + const json & request, std::chrono::duration timeout) = 0; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_subscription_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_subscription_transport.hpp new file mode 100644 index 000000000..75ace0f4c --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_subscription_transport.hpp @@ -0,0 +1,60 @@ +// 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 + +namespace ros2_medkit_gateway { + +using json = nlohmann::json; + +/// RAII handle for a single trigger-side topic subscription. Destruction +/// unsubscribes; manager holds these in its tracking map. +class TopicSubscriptionHandle { + public: + TopicSubscriptionHandle() = default; + TopicSubscriptionHandle(const TopicSubscriptionHandle &) = delete; + TopicSubscriptionHandle & operator=(const TopicSubscriptionHandle &) = delete; + TopicSubscriptionHandle(TopicSubscriptionHandle &&) = delete; + TopicSubscriptionHandle & operator=(TopicSubscriptionHandle &&) = delete; + virtual ~TopicSubscriptionHandle() = default; +}; + +/// Port for the trigger subsystem to subscribe to a topic and receive each +/// sample as JSON. Replaces the direct `TriggerTopicSubscriber *` pointer +/// the trigger manager currently holds; the adapter wraps the existing +/// subscriber to preserve its destructor pattern. +class TopicSubscriptionTransport { + public: + using SampleCallback = std::function; + + TopicSubscriptionTransport() = default; + TopicSubscriptionTransport(const TopicSubscriptionTransport &) = delete; + TopicSubscriptionTransport & operator=(const TopicSubscriptionTransport &) = delete; + TopicSubscriptionTransport(TopicSubscriptionTransport &&) = delete; + TopicSubscriptionTransport & operator=(TopicSubscriptionTransport &&) = delete; + virtual ~TopicSubscriptionTransport() = default; + + /// Subscribe and return a handle. Returns nullptr on transport error + /// (topic not advertised, type mismatch, etc.). The handle's destructor + /// guarantees no callback fires after destruction completes. + virtual std::unique_ptr subscribe(const std::string & topic_path, + const std::string & msg_type, SampleCallback callback) = 0; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_transport.hpp new file mode 100644 index 000000000..16cfeb03c --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_transport.hpp @@ -0,0 +1,85 @@ +// 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 + +namespace ros2_medkit_serialization { +class TypeIntrospection; // forward decl - defined in ros2_medkit_serialization/type_introspection.hpp. +} // namespace ros2_medkit_serialization + +namespace ros2_medkit_gateway { + +using json = nlohmann::json; + +/// Neutral result for a topic-sample call routed through the transport port. +/// +/// The existing `TopicSampleResult` struct in +/// `core/data/data_types.hpp` belongs to the topic-data-provider plugin +/// surface and has a different shape (per-topic batch errors, endpoint QoS, +/// etc.). The transport-level result here is intentionally narrower: it +/// captures only what the manager facade needs to forward to handlers. A +/// later phase reconciles the two if it makes sense to do so. +struct TopicSample { + bool success; + std::string status; ///< "data" | "metadata_only" | "error" + std::string topic; + std::string type; + json data = json::object(); + json type_info = json::object(); + uint64_t publisher_count = 0; + uint64_t subscriber_count = 0; + std::string error_message; +}; + +/// Port: topic publish + sample. Concrete adapter +/// `Ros2TopicTransport` (src/ros2/transports/) implements it via +/// rclcpp GenericPublisher / NativeTopicSampler. +class TopicTransport { + public: + TopicTransport() = default; + TopicTransport(const TopicTransport &) = delete; + TopicTransport & operator=(const TopicTransport &) = delete; + TopicTransport(TopicTransport &&) = delete; + TopicTransport & operator=(TopicTransport &&) = delete; + virtual ~TopicTransport() = default; + + /// Publish JSON-encoded data to a topic. Implementations cache the underlying + /// publisher keyed by (topic, type). + virtual json publish(const std::string & topic_path, const std::string & msg_type, const json & data, + std::chrono::duration timeout) = 0; + + /// Sample a topic with timeout. Implementations fall back to metadata if no + /// publisher is active. `timeout < 0` means "use the implementation default". + virtual TopicSample sample(const std::string & topic_name, std::chrono::duration timeout) = 0; + + /// Publisher and subscriber count for a topic. Implementations may return + /// stale data if the underlying graph snapshot is cached. + virtual std::pair count_publishers_subscribers(const std::string & topic_name) const = 0; + + /// Type-introspection helper used by handlers to enrich SOVD payloads with + /// schema and default-value templates. The TypeIntrospection backend lives + /// in `ros2_medkit_serialization` alongside the rest of the rosidl glue; + /// transports own an instance so the manager body remains middleware-neutral. + /// May return nullptr in transports that do not support introspection (test + /// mocks, alternative middlewares). + virtual ros2_medkit_serialization::TypeIntrospection * get_type_introspection() const = 0; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/data_access_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/data_access_manager.hpp index 1c5d12e8f..524720c5f 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/data_access_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/data_access_manager.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 mfaferek93 +// 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. @@ -14,132 +14,5 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ros2_medkit_gateway/core/data/data_types.hpp" -#include "ros2_medkit_gateway/core/type_introspection.hpp" -#include "ros2_medkit_serialization/json_serializer.hpp" - -namespace ros2_medkit_gateway { - -using json = nlohmann::json; - -class TopicDataProvider; // forward decl, see data/topic_data_provider.hpp - -class DataAccessManager { - public: - explicit DataAccessManager(rclcpp::Node * node); - - /** - * @brief Publish data to a specific topic - * @param topic_path Full topic path (e.g., /chassis/brakes/command) - * @param msg_type ROS 2 message type (e.g., std_msgs/msg/Float32) - * @param data JSON data to publish - * @param timeout_sec Timeout for the publish operation - * @return JSON with publish status - */ - json publish_to_topic(const std::string & topic_path, const std::string & msg_type, const json & data, - double timeout_sec = 5.0); - - /** - * @brief Get topic sample with fallback to metadata on timeout - * - * If the topic is publishing, returns actual data with type info. - * If the topic times out, returns metadata (type, schema, pub/sub counts) instead of error. - * - * @param topic_name Full topic path (e.g., "/powertrain/engine/temperature") - * @param timeout_sec Timeout for data retrieval. Use -1.0 to use the topic_sample_timeout_sec parameter (default) - * @return JSON object with one of two structures: - * - status="data": {topic, timestamp, data, status, type, type_info, publisher_count, subscriber_count} - * - status="metadata_only": {topic, timestamp, status, type, type_info, publisher_count, subscriber_count} - * @throws TopicNotAvailableException if topic doesn't exist or metadata cannot be retrieved - */ - json get_topic_sample_with_fallback(const std::string & topic_name, double timeout_sec = -1.0); - - /** - * @brief Get the type introspection instance - */ - TypeIntrospection * get_type_introspection() const { - return type_introspection_.get(); - } - - /** - * @brief Attach a TopicDataProvider for sampling. - * - * The provider owns the pool-backed subscription path (issue #375 race fix). - * Non-owning pointer; caller retains ownership. Safe to call once at wiring - * time; no concurrent access to the setter. - */ - void set_topic_data_provider(TopicDataProvider * provider) { - topic_data_provider_ = provider; - } - - TopicDataProvider * get_topic_data_provider() const { - return topic_data_provider_; - } - - /** - * @brief Get single topic sample using native rclcpp APIs - * - * Fast path for single topic sampling with publisher count check. - * - * @param topic_name Full topic path - * @param timeout_sec Timeout for sampling (only used if topic has publishers) - * @return JSON with topic data or metadata - */ - json get_topic_sample_native(const std::string & topic_name, double timeout_sec = 1.0); - - /** - * @brief Get the configured topic sample timeout - * @return Timeout in seconds for topic sampling - */ - double get_topic_sample_timeout() const { - return topic_sample_timeout_sec_; - } - - private: - /** - * @brief Convert TopicSampleResult to JSON with type info enrichment - */ - json sample_result_to_json(const TopicSampleResult & sample); - - /** - * @brief Get or create a cached GenericPublisher for a topic - * @param topic_path Full topic path - * @param msg_type ROS 2 message type - * @return Shared pointer to GenericPublisher - */ - rclcpp::GenericPublisher::SharedPtr get_or_create_publisher(const std::string & topic_path, - const std::string & msg_type); - - rclcpp::Node * node_; - - /// JSON serializer for native message serialization - std::shared_ptr serializer_; - - /// Cached publishers (topic+type -> publisher) - std::unordered_map publishers_; - - /// Mutex for thread-safe publisher cache access - mutable std::shared_mutex publishers_mutex_; - - std::unique_ptr type_introspection_; - TopicDataProvider * topic_data_provider_{nullptr}; ///< Non-owning; set at wiring time. - double topic_sample_timeout_sec_; - - /** - * @brief Get default timeout for topic sampling (from parameter) - */ - double get_default_topic_timeout() const { - return topic_sample_timeout_sec_; - } -}; - -} // namespace ros2_medkit_gateway +// Backwards-compatibility shim - header relocated to core/managers/. +#include "ros2_medkit_gateway/core/managers/data_access_manager.hpp" 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 03cdc578f..e1c081943 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 @@ -15,30 +15,34 @@ #pragma once #include "ros2_medkit_gateway/core/discovery/discovery_enums.hpp" -#include "ros2_medkit_gateway/core/discovery/discovery_strategy.hpp" #include "ros2_medkit_gateway/core/discovery/merge_types.hpp" #include "ros2_medkit_gateway/core/discovery/models/app.hpp" #include "ros2_medkit_gateway/core/discovery/models/area.hpp" #include "ros2_medkit_gateway/core/discovery/models/common.hpp" #include "ros2_medkit_gateway/core/discovery/models/component.hpp" #include "ros2_medkit_gateway/core/discovery/models/function.hpp" +#include "ros2_medkit_gateway/core/discovery/service_action_resolver.hpp" #include "ros2_medkit_gateway/core/providers/host_info_provider.hpp" -#include "ros2_medkit_gateway/discovery/hybrid_discovery.hpp" #include "ros2_medkit_gateway/discovery/manifest/manifest_manager.hpp" -#include "ros2_medkit_gateway/discovery/runtime_discovery.hpp" +#include "ros2_medkit_gateway/discovery/merge_pipeline.hpp" +#include "ros2_medkit_gateway/ros2/providers/ros2_runtime_introspection.hpp" #include #include +#include #include #include #include #include +namespace ros2_medkit_serialization { +class TypeIntrospection; +} // namespace ros2_medkit_serialization + namespace ros2_medkit_gateway { // Forward declarations class TopicDataProvider; -class TypeIntrospection; class IntrospectionProvider; /** @@ -112,25 +116,26 @@ struct DiscoveryConfig { }; /** - * @brief Orchestrates entity discovery using pluggable strategies + * @brief Orchestrates entity discovery using a configurable merge pipeline * - * This class delegates discovery to strategy implementations based on - * the configured mode: - * - RUNTIME_ONLY: Uses RuntimeDiscoveryStrategy (traditional ROS graph) - * - MANIFEST_ONLY: Uses manifest as sole source of truth - * - HYBRID: Combines manifest definitions with runtime linking + * The discovery_mode parameter selects which layers the merge pipeline + * activates: + * - RUNTIME_ONLY: ROS 2 graph introspection only (no merge pipeline; the + * built-in Ros2RuntimeIntrospection provider is queried directly) + * - MANIFEST_ONLY: Manifest as the sole source of truth + * - HYBRID: Manifest + runtime + plugin layers merged through the pipeline * * The DiscoveryManager provides a unified interface for discovering: - * - Areas: Logical groupings (ROS 2 namespaces or manifest areas) - * - Components: Software/hardware units (ROS 2 nodes) - * - Apps: Software applications (manifest-defined) - * - Functions: Functional groupings (manifest-defined) + * - Areas: Logical groupings (manifest-only) + * - Components: Software/hardware units (HostInfoProvider, manifest, plugins) + * - Apps: Software applications (manifest, runtime nodes, plugins) + * - Functions: Functional groupings (manifest, namespaces, plugins) * - * @see discovery::RuntimeDiscoveryStrategy - * @see discovery::HybridDiscoveryStrategy + * @see ros2::Ros2RuntimeIntrospection + * @see discovery::MergePipeline * @see discovery::ManifestManager */ -class DiscoveryManager { +class DiscoveryManager : public ServiceActionResolver { public: /** * @brief Construct the discovery manager @@ -282,7 +287,8 @@ class DiscoveryManager { * @param operation_name Service name * @return ServiceInfo if found, nullopt otherwise */ - std::optional find_service(const std::string & component_ns, const std::string & operation_name) const; + std::optional find_service(const std::string & component_ns, + const std::string & operation_name) const override; /** * @brief Find an action by component namespace and operation name @@ -290,7 +296,8 @@ class DiscoveryManager { * @param operation_name Action name * @return ActionInfo if found, nullopt otherwise */ - std::optional find_action(const std::string & component_ns, const std::string & operation_name) const; + std::optional find_action(const std::string & component_ns, + const std::string & operation_name) const override; /** * @brief Set the topic sampler for component-topic mapping @@ -302,7 +309,7 @@ class DiscoveryManager { * @brief Set the type introspection for operation schema enrichment * @param introspection Pointer to TypeIntrospection (must outlive DiscoveryManager) */ - void set_type_introspection(TypeIntrospection * introspection); + void set_type_introspection(ros2_medkit_serialization::TypeIntrospection * introspection); /** * @brief Refresh the cached topic map @@ -330,6 +337,15 @@ class DiscoveryManager { */ void add_plugin_layer(const std::string & plugin_name, IntrospectionProvider * provider); + /** + * @brief Register an introspection provider with the merge pipeline + * + * Equivalent to add_plugin_layer for shared-ownership providers; the manager + * keeps the provider alive for the lifetime of the pipeline. Only meaningful + * in HYBRID mode. + */ + void register_introspection_provider(const std::string & name, std::shared_ptr provider); + /** * @brief Re-execute the merge pipeline (hybrid mode only) * @@ -368,8 +384,8 @@ class DiscoveryManager { } /** - * @brief Get the current discovery strategy name - * @return Strategy name (e.g., "runtime", "manifest", "hybrid") + * @brief Get a human-readable name for the active discovery configuration. + * @return One of "runtime", "manifest", "hybrid". */ std::string get_strategy_name() const; @@ -387,9 +403,21 @@ class DiscoveryManager { private: /** - * @brief Create and activate the appropriate strategy + * @brief Build the merge pipeline for the active discovery mode. + * + * No-op in RUNTIME_ONLY and MANIFEST_ONLY modes; in HYBRID mode wires + * ManifestLayer / RuntimeLayer based on per-layer enable flags. + */ + void build_pipeline(); + + /** + * @brief Re-execute the pipeline and cache the merged result. + * + * Only called in HYBRID mode. RUNTIME_ONLY and MANIFEST_ONLY modes serve + * entities directly from the runtime introspection provider or the + * manifest manager. */ - void create_strategy(); + void run_pipeline(); /** * @brief Apply user-configured merge policy overrides to a layer @@ -403,13 +431,18 @@ class DiscoveryManager { // Host info provider (created when default_component_enabled is true) std::unique_ptr host_info_provider_; - // Strategies - std::unique_ptr runtime_strategy_; + // ROS 2 graph adapter implementing IntrospectionProvider + std::unique_ptr runtime_introspection_; std::unique_ptr manifest_manager_; - std::unique_ptr hybrid_strategy_; - // Active strategy pointer (points to one of the above) - discovery::DiscoveryStrategy * active_strategy_{nullptr}; + // HYBRID mode pipeline + cached merged result. + std::unique_ptr pipeline_; + discovery::MergeResult cached_result_; + mutable std::mutex result_mutex_; + + // Plugin providers registered through register_introspection_provider keep + // the IntrospectionProvider alive for the lifetime of the pipeline. + std::vector> owned_providers_; }; } // namespace ros2_medkit_gateway 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 deleted file mode 100644 index 5009c1dc8..000000000 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/hybrid_discovery.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// 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/core/discovery/discovery_layer.hpp" -#include "ros2_medkit_gateway/core/discovery/discovery_strategy.hpp" -#include "ros2_medkit_gateway/discovery/merge_pipeline.hpp" - -#include - -#include -#include -#include -#include - -namespace ros2_medkit_gateway { -namespace discovery { - -/** - * @brief Hybrid discovery using a MergePipeline - * - * 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 pipeline Pre-configured merge pipeline - */ - HybridDiscoveryStrategy(rclcpp::Node * node, MergePipeline pipeline); - - std::vector discover_areas() override; - std::vector discover_components() override; - std::vector discover_apps() override; - std::vector discover_functions() override; - - std::string get_name() const override { - return "hybrid"; - } - - /** - * @brief Re-execute the pipeline and cache the result - */ - void refresh(); - - /** - * @brief Get the last merge report (returned by value for thread safety) - */ - MergeReport get_merge_report() const; - - /** - * @brief Get the last linking result (returned by value for thread safety) - */ - LinkingResult get_linking_result() const; - - /** - * @brief Get orphan nodes from last linking - */ - std::vector get_orphan_nodes() const; - - /** - * @brief Add a discovery layer to the pipeline. - * @warning Construction-time only. Must be called before the first refresh() - * (i.e., before EntityCache timer starts). Calling concurrently with refresh() - * is a data race. - */ - void add_layer(std::unique_ptr layer); - - private: - rclcpp::Node * node_; - MergePipeline pipeline_; - MergeResult cached_result_; - mutable std::mutex mutex_; -}; - -} // namespace discovery -} // namespace ros2_medkit_gateway 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 index 2a880c767..4d2db9ac9 100644 --- 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 @@ -66,8 +66,8 @@ class MergePipeline { /** * @brief Get the last merge report (returned by value for thread safety) * @warning NOT thread-safe on its own. In production, access only through - * HybridDiscoveryStrategy which holds mutex during reads. Direct calls - * from multiple threads are a data race. + * DiscoveryManager which holds a mutex around the cached pipeline result. + * Direct calls from multiple threads are a data race. */ MergeReport get_last_report() const { return last_report_; @@ -83,7 +83,7 @@ class MergePipeline { /** * @brief Get the last linking result (returned by value for thread safety) * @warning NOT thread-safe on its own. In production, access only through - * HybridDiscoveryStrategy which holds mutex during reads. + * DiscoveryManager which holds a mutex around the cached pipeline result. */ LinkingResult get_linking_result() const { return linking_result_; 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 deleted file mode 100644 index c25ce9c6b..000000000 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/runtime_discovery.hpp +++ /dev/null @@ -1,191 +0,0 @@ -// 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/core/data/topic_data_provider.hpp" -#include "ros2_medkit_gateway/core/discovery/discovery_strategy.hpp" -#include "ros2_medkit_gateway/core/discovery/models/app.hpp" -#include "ros2_medkit_gateway/core/discovery/models/area.hpp" -#include "ros2_medkit_gateway/core/discovery/models/common.hpp" -#include "ros2_medkit_gateway/core/discovery/models/component.hpp" -#include "ros2_medkit_gateway/core/discovery/models/function.hpp" -#include "ros2_medkit_gateway/core/type_introspection.hpp" - -#include -#include -#include -#include -#include - -namespace ros2_medkit_gateway { - -// Forward declaration -struct DiscoveryConfig; - -namespace discovery { - -/** - * @brief Runtime discovery strategy using ROS 2 graph introspection - * - * This is the current discovery behavior extracted into a strategy class. - * It discovers entities by querying the ROS 2 node graph at runtime. - * - * Features: - * - Discovers Functions from node namespaces (functional grouping) - * - Exposes nodes as Apps - * - Enriches apps with services, actions, and topics - * - * Note: Synthetic/heuristic Area and Component creation has been removed. - * Areas come from manifest only. Components come from HostInfoProvider - * or manifest. Namespaces create Function entities. - */ -class RuntimeDiscoveryStrategy : public DiscoveryStrategy { - public: - /** - * @brief Runtime discovery configuration options - */ - struct RuntimeConfig { - bool create_functions_from_namespaces{true}; - }; - - /** - * @brief Construct runtime discovery strategy - * @param node ROS 2 node for graph introspection (must outlive this strategy) - */ - explicit RuntimeDiscoveryStrategy(rclcpp::Node * node); - - /** - * @brief Set runtime discovery configuration - * @param config Runtime options from DiscoveryConfig - */ - void set_config(const RuntimeConfig & config); - - /// @copydoc DiscoveryStrategy::discover_areas - /// @note Always returns empty - Areas come from manifest only - std::vector discover_areas() override; - - /// @copydoc DiscoveryStrategy::discover_components - /// @note Always returns empty - Components come from HostInfoProvider or manifest - std::vector discover_components() override; - - /// @copydoc DiscoveryStrategy::discover_apps - /// @note Returns nodes as Apps in runtime discovery - std::vector discover_apps() override; - - /// @copydoc DiscoveryStrategy::discover_functions - /// @note Creates Function entities from namespace grouping when enabled - std::vector discover_functions() override; - - /** - * @brief Create Function entities from pre-discovered apps - * - * Avoids redundant ROS 2 graph introspection when apps have already been - * discovered in the same refresh cycle (e.g., refresh_cache() calls - * discover_apps() before discover_functions()). - * - * @param apps Pre-discovered apps to group by namespace - * @return Vector of Function entities - */ - std::vector discover_functions(const std::vector & apps); - - /// @copydoc DiscoveryStrategy::get_name - std::string get_name() const override { - return "runtime"; - } - - // ========================================================================= - // Runtime-specific methods (from current DiscoveryManager) - // ========================================================================= - - /** - * @brief Discover all services in the system with their types - * @return Vector of ServiceInfo with schema information - */ - std::vector discover_services(); - - /** - * @brief Discover all actions in the system with their types - * @return Vector of ActionInfo with schema information - */ - std::vector discover_actions(); - - /** - * @brief Find a service by component namespace and operation name - * @param component_ns Component namespace - * @param operation_name Service name - * @return ServiceInfo if found, nullopt otherwise - */ - std::optional find_service(const std::string & component_ns, const std::string & operation_name) const; - - /** - * @brief Find an action by component namespace and operation name - * @param component_ns Component namespace - * @param operation_name Action name - * @return ActionInfo if found, nullopt otherwise - */ - std::optional find_action(const std::string & component_ns, const std::string & operation_name) const; - - /** - * @brief Set the topic data provider for component-topic mapping - * @param provider Pointer to TopicDataProvider (must outlive this strategy) - */ - void set_topic_data_provider(TopicDataProvider * provider); - - /** - * @brief Set the type introspection for operation schema enrichment - * @param introspection Pointer to TypeIntrospection (must outlive this strategy) - */ - void set_type_introspection(TypeIntrospection * introspection); - - /** - * @brief Refresh the cached topic map - */ - void refresh_topic_map(); - - /** - * @brief Check if topic map has been built at least once - * @return true if topic map is ready, false if not yet built - */ - bool is_topic_map_ready() const; - - private: - /// Extract area ID from namespace (e.g., "/powertrain/engine" -> "powertrain") - std::string extract_area_from_namespace(const std::string & ns); - - /// Extract the last segment from a path (e.g., "/a/b/c" -> "c") - std::string extract_name_from_path(const std::string & path); - - /// Check if a service path belongs to a component namespace - bool path_belongs_to_namespace(const std::string & path, const std::string & ns) const; - - /// Check if a service path is an internal ROS2 service - static bool is_internal_service(const std::string & service_path); - - rclcpp::Node * node_; - RuntimeConfig config_; - TopicDataProvider * topic_data_provider_{nullptr}; - TypeIntrospection * type_introspection_{nullptr}; - - // Cached services and actions for lookup - std::vector cached_services_; - std::vector cached_actions_; - - // Cached topic map for performance (rebuilt on demand or periodically) - std::map cached_topic_map_; - bool topic_map_ready_{false}; -}; - -} // namespace discovery -} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/fault_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/fault_manager.hpp index fee24eee4..ac0d7670b 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/fault_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/fault_manager.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 mfaferek93 +// 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. @@ -14,141 +14,5 @@ #pragma once -#include -#include -#include -#include -#include -#include - -#include "ros2_medkit_msgs/msg/environment_data.hpp" -#include "ros2_medkit_msgs/msg/fault.hpp" -#include "ros2_medkit_msgs/srv/clear_fault.hpp" -#include "ros2_medkit_msgs/srv/get_fault.hpp" -#include "ros2_medkit_msgs/srv/get_rosbag.hpp" -#include "ros2_medkit_msgs/srv/get_snapshots.hpp" -#include "ros2_medkit_msgs/srv/list_faults.hpp" -#include "ros2_medkit_msgs/srv/list_rosbags.hpp" -#include "ros2_medkit_msgs/srv/report_fault.hpp" - -namespace ros2_medkit_gateway { - -using json = nlohmann::json; - -/// Result of a fault operation -struct FaultResult { - bool success; - json data; - std::string error_message; -}; - -/// Result of get_fault operation with full message types -struct FaultWithEnvResult { - bool success; - std::string error_message; - ros2_medkit_msgs::msg::Fault fault; - ros2_medkit_msgs::msg::EnvironmentData environment_data; -}; - -/// Manager for fault management operations -/// Provides interface to the ros2_medkit_fault_manager services -class FaultManager { - public: - explicit FaultManager(rclcpp::Node * node); - - /// Report a fault from a component - /// @param fault_code Unique fault identifier - /// @param severity Fault severity (0=INFO, 1=WARN, 2=ERROR, 3=CRITICAL) - /// @param description Human-readable description - /// @param source_id Component identifier (namespace path) - /// @return FaultResult with success status - FaultResult report_fault(const std::string & fault_code, uint8_t severity, const std::string & description, - const std::string & source_id); - - /// Get all faults, optionally filtered by component - /// @param source_id Optional component identifier to filter by (empty = all) - /// @param include_prefailed Include PREFAILED status faults (debounce not yet confirmed) - /// @param include_confirmed Include CONFIRMED status faults - /// @param include_cleared Include CLEARED status faults - /// @param include_healed Include HEALED and PREPASSED status faults - /// @param include_muted Include muted faults (correlation symptoms) in response - /// @param include_clusters Include cluster info in response - /// @return FaultResult with array of faults (and optionally muted_faults and clusters) - FaultResult list_faults(const std::string & source_id = "", bool include_prefailed = true, - bool include_confirmed = true, bool include_cleared = false, bool include_healed = false, - bool include_muted = false, bool include_clusters = false); - - /// Get a specific fault by code with environment data - /// @param fault_code Fault identifier - /// @param source_id Optional component identifier to verify fault belongs to component - /// @return FaultWithEnvResult with fault and environment_data, or error if not found - FaultWithEnvResult get_fault_with_env(const std::string & fault_code, const std::string & source_id = ""); - - /// Get a specific fault by code (JSON result - legacy) - /// @param fault_code Fault identifier - /// @param source_id Optional component identifier to verify fault belongs to component - /// @return FaultResult with fault data or error if not found - /// @note Thread-safe: delegates to get_fault_with_env() which acquires get_mutex_. - /// Do NOT call this method while holding get_mutex_. - FaultResult get_fault(const std::string & fault_code, const std::string & source_id = ""); - - /// Clear a fault - /// @param fault_code Fault identifier to clear - /// @return FaultResult with success status - FaultResult clear_fault(const std::string & fault_code); - - /// Get snapshots for a fault - /// @param fault_code Fault identifier - /// @param topic Optional topic filter (empty = all topics) - /// @return FaultResult with snapshot data (JSON in data field) - FaultResult get_snapshots(const std::string & fault_code, const std::string & topic = ""); - - /// Get rosbag file info for a fault - /// @param fault_code Fault identifier - /// @return FaultResult with rosbag file path and metadata - FaultResult get_rosbag(const std::string & fault_code); - - /// Get all rosbag files for an entity (batch operation) - /// @param entity_fqn Entity fully qualified name for prefix matching - /// @return FaultResult with arrays of rosbag metadata - FaultResult list_rosbags(const std::string & entity_fqn); - - /// Check if fault manager service is available - /// @return true if services are available - bool is_available() const; - - /// Convert Fault message to JSON (static utility for reuse by SSE handler) - static json fault_to_json(const ros2_medkit_msgs::msg::Fault & fault); - - private: - /// Wait for services to become available - bool wait_for_services(std::chrono::duration timeout); - - rclcpp::Node * node_; - - /// Service clients - rclcpp::Client::SharedPtr report_fault_client_; - rclcpp::Client::SharedPtr get_fault_client_; - rclcpp::Client::SharedPtr list_faults_client_; - rclcpp::Client::SharedPtr clear_fault_client_; - rclcpp::Client::SharedPtr get_snapshots_client_; - rclcpp::Client::SharedPtr get_rosbag_client_; - rclcpp::Client::SharedPtr list_rosbags_client_; - - /// Service timeout - double service_timeout_sec_{5.0}; - std::string fault_manager_base_path_{"/fault_manager"}; - - /// Per-client mutexes for thread-safe service calls. - /// Split by service client so that read operations (list, get) are not blocked - /// by slow write operations (report_fault with snapshot capture). - mutable std::mutex report_mutex_; - mutable std::mutex list_mutex_; - mutable std::mutex get_mutex_; - mutable std::mutex clear_mutex_; - mutable std::mutex snapshots_mutex_; - mutable std::mutex rosbag_mutex_; - mutable std::mutex list_rosbags_mutex_; -}; - -} // namespace ros2_medkit_gateway +// Backwards-compatibility shim - header relocated to core/managers/. +#include "ros2_medkit_gateway/core/managers/fault_manager.hpp" diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/gateway_node.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/gateway_node.hpp index e07b50fdf..a82984a60 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/gateway_node.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/gateway_node.hpp @@ -49,8 +49,15 @@ #include "ros2_medkit_gateway/fault_manager.hpp" #include "ros2_medkit_gateway/log_manager.hpp" #include "ros2_medkit_gateway/operation_manager.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_action_transport.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_fault_service_transport.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_log_source.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_parameter_transport.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_service_transport.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_topic_subscription_transport.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp" +#include "ros2_medkit_gateway/ros2/trigger_topic_subscriber.hpp" #include "ros2_medkit_gateway/trigger_fault_subscriber.hpp" -#include "ros2_medkit_gateway/trigger_topic_subscriber.hpp" namespace ros2_medkit_gateway { @@ -211,9 +218,10 @@ class GatewayNode : public rclcpp::Node { * indicated area / component. The entry point is safe to call from any * thread: refresh passes triggered by plugin notifications, the periodic * refresh timer, and startup are serialized by an internal mutex inside - * `refresh_cache()` because refresh touches discovery state (e.g., - * `HybridDiscoveryStrategy::refresh()`) that itself assumes single-threaded - * execution - `ThreadSafeEntityCache`'s own mutex is not sufficient. + * `refresh_cache()` because refresh touches discovery state (e.g., the + * merge pipeline cached inside `DiscoveryManager`) that itself assumes + * single-threaded execution - `ThreadSafeEntityCache`'s own mutex is not + * sufficient. */ void handle_entity_change_notification(const EntityChangeScope & scope); @@ -240,6 +248,10 @@ class GatewayNode : public rclcpp::Node { // Configuration parameters std::string server_host_; int server_port_; + // Safety-backstop refresh interval in milliseconds. The primary refresh + // trigger is the rclcpp graph event polled by `graph_check_timer_`; this + // value drives `backstop_timer_` which guarantees liveness if a graph + // event is ever missed (lost wakeup, rclcpp anomaly, etc.). int refresh_interval_ms_; bool filter_internal_nodes_{true}; CorsConfig cors_config_; @@ -252,6 +264,33 @@ class GatewayNode : public rclcpp::Node { // as a raw TopicDataProvider* to DataAccessManager. std::shared_ptr topic_data_provider_; + // Topic transport adapter shared with DataAccessManager. Held here so the + // provider attach/detach hooks can forward into the adapter alongside the + // manager and discovery side updates. + std::shared_ptr topic_transport_; + + // Service / action transport adapters shared with OperationManager. Held + // here so their lifetime matches the gateway's executor (transports own + // rclcpp clients + subscriptions and must outlive the manager). + std::shared_ptr service_transport_; + std::shared_ptr action_transport_; + + // Parameter transport adapter shared with ConfigurationManager. Owns the + // parameter-client cache + defaults cache + spin_mutex; the manager forwards + // shutdown() into it before rclcpp::shutdown() runs. + std::shared_ptr parameter_transport_; + + // Fault-service transport adapter shared with FaultManager. Owns the seven + // rclcpp service clients, the seven per-client mutexes, and the + // ros2_medkit_msgs <-> JSON conversion helpers that previously lived inside + // FaultManager. + std::shared_ptr fault_service_transport_; + + // /rosout source adapter shared with LogManager. Owns the + // rclcpp::Subscription + msg-to-LogEntry + // conversion that previously lived inside LogManager. + std::shared_ptr log_source_; + // Managers std::unique_ptr discovery_mgr_; std::unique_ptr data_access_mgr_; @@ -281,6 +320,8 @@ class GatewayNode : public rclcpp::Node { std::unique_ptr trigger_mgr_; std::unique_ptr trigger_fault_subscriber_; std::unique_ptr trigger_topic_subscriber_; + // Adapter routing manager-side subscribe() calls onto trigger_topic_subscriber_. + std::shared_ptr trigger_topic_transport_; // Aggregation infrastructure (destroyed in order: mdns -> rest_server -> aggregation) // mDNS threads must stop before rest_server to avoid callbacks during shutdown. @@ -293,16 +334,31 @@ class GatewayNode : public rclcpp::Node { // Cache with thread safety ThreadSafeEntityCache thread_safe_cache_; - // Timer for periodic refresh - rclcpp::TimerBase::SharedPtr refresh_timer_; + // Graph-change-driven discovery refresh. + // + // `graph_event_` is the rclcpp::Event signalled whenever the ROS 2 graph + // changes (node up/down, topic/service/action add or remove). It is a + // shared_ptr handed out by `rclcpp::Node::get_graph_event()`; the executor + // owns the underlying notification and we just poll-and-clear it. + // + // `graph_check_timer_` polls `graph_event_->check_and_clear()` at a fast + // cadence (100 ms) and runs `refresh_cache()` only when the event fires. + // On a stable graph this keeps idle CPU near zero. + // + // `backstop_timer_` runs `refresh_cache()` unconditionally at the slower + // `refresh_interval_ms_` cadence. Its sole purpose is liveness in the + // unlikely case a graph event is missed. + rclcpp::Event::SharedPtr graph_event_; + rclcpp::TimerBase::SharedPtr graph_check_timer_; + rclcpp::TimerBase::SharedPtr backstop_timer_; // Serializes `refresh_cache()` across the refresh timer, plugin // `notify_entities_changed` calls and any other caller. Required because // the refresh pipeline touches discovery state that is not itself - // thread-safe (e.g., `HybridDiscoveryStrategy::refresh()`). Recursive so - // that `handle_entity_change_notification` can also cover the preceding - // `reload_manifest()` call without deadlocking when it subsequently - // invokes `refresh_cache()`. + // thread-safe (the merge pipeline cached inside `DiscoveryManager`). + // Recursive so that `handle_entity_change_notification` can also cover + // the preceding `reload_manifest()` call without deadlocking when it + // subsequently invokes `refresh_cache()`. std::recursive_mutex refresh_mutex_; // Timer for periodic cleanup of old action goals diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/fault_handlers.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/fault_handlers.hpp index f84a066f8..014fdf0f8 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/fault_handlers.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/http/handlers/fault_handlers.hpp @@ -15,10 +15,9 @@ #pragma once #include +#include #include "ros2_medkit_gateway/http/handlers/handler_context.hpp" -#include "ros2_medkit_msgs/msg/environment_data.hpp" -#include "ros2_medkit_msgs/msg/fault.hpp" namespace ros2_medkit_gateway { namespace handlers { @@ -79,20 +78,31 @@ class FaultHandlers { void handle_clear_all_faults_global(const httplib::Request & req, httplib::Response & res); /** - * @brief Build SOVD-compliant fault response with environment data. + * @brief Build SOVD-compliant fault response from already-converted JSON. * - * Creates a response containing: + * Consumes the JSON shape returned by FaultManager::get_fault_with_env - + * `fault_json` is the per-fault flat representation (the FaultManager has + * already translated severity_label, status string, timestamps, etc.) and + * `env_data_json` carries `extended_data_records` plus a `snapshots` array + * with intermediate-shape entries (freeze_frame: type/name/data/topic/ + * message_type/captured_at_ns; rosbag: type/name/fault_code/size_bytes/ + * duration_sec/format). + * + * Builds the final SOVD response containing: * - "item" with fault details and SOVD status object - * - "environment_data" with extended_data_records and snapshots + * - "environment_data" with extended_data_records and snapshots; for + * freeze_frame snapshots the handler parses "data", extracts the primary + * value and packs the full payload into x-medkit; for rosbag snapshots + * the handler appends `bulk_data_uri = entity_path + "/bulk-data/rosbags/" + fault_code`. * - "x-medkit" extensions with occurrence_count, severity_label, etc. * - * @param fault The fault message from fault_manager - * @param env_data Environment data with snapshots - * @param entity_path Entity path for bulk_data_uri generation (e.g., "/apps/motor") + * @param fault_json Per-fault JSON (as produced by the transport adapter). + * @param env_data_json Environment-data JSON (as produced by the transport). + * @param entity_path Entity path used to construct rosbag bulk_data_uri. * @return SOVD-compliant JSON response */ - static nlohmann::json build_sovd_fault_response(const ros2_medkit_msgs::msg::Fault & fault, - const ros2_medkit_msgs::msg::EnvironmentData & env_data, + static nlohmann::json build_sovd_fault_response(const nlohmann::json & fault_json, + const nlohmann::json & env_data_json, const std::string & entity_path); private: diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/log_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/log_manager.hpp index a23610098..00508b871 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/log_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/log_manager.hpp @@ -14,185 +14,5 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ros2_medkit_gateway/core/log_types.hpp" -#include "ros2_medkit_gateway/core/providers/log_provider.hpp" - -namespace ros2_medkit_gateway { - -using json = nlohmann::json; - -class PluginManager; // forward declaration — full include in .cpp -class ResourceChangeNotifier; // forward declaration - -/** - * @brief Manages /rosout log collection via the default ring-buffer backend. - * - * Subscribes to /rosout, normalizes logger names (strips leading '/'), - * and stores entries per node-name in fixed-size deque ring buffers. - * - * Plugin integration (two modes): - * - **Observer mode** (default): If a LogProvider plugin is registered, get_logs() - * and get_config() delegate to it. on_log_entry() is called on ALL LogProvider - * observers for every /rosout message. Observers returning true suppress ring-buffer - * storage. - * - **Full ingestion** (manages_ingestion() == true): The primary LogProvider owns - * the entire log pipeline. LogManager skips the /rosout subscription and ring buffer - * entirely. All queries and config operations delegate to the plugin. - * - * FQN normalization: - * - entity.fqn from the entity cache has a leading '/' (e.g. "/powertrain/engine/temp_sensor") - * - /rosout msg.name does NOT have a leading '/' (rcl_node_get_logger_name convention) - * - Callers pass raw FQNs from entity.fqn; LogManager strips leading '/' internally. - */ -class LogManager { - public: - /// Default maximum number of entries retained per node in the ring buffer - static constexpr size_t kDefaultBufferSize = 200; - - /** - * @brief Construct LogManager - * - * If the primary LogProvider's manages_ingestion() returns true, the /rosout - * subscription and ring buffer are skipped entirely. Otherwise subscribes to - * /rosout as usual. - * - * @param node ROS 2 node (used for subscription and logging) - * @param plugin_mgr PluginManager for LogProvider lookup (may be nullptr) - * @param max_buffer_size Ring buffer size per node (override for unit testing) - */ - explicit LogManager(rclcpp::Node * node, PluginManager * plugin_mgr = nullptr, - size_t max_buffer_size = kDefaultBufferSize); - - ~LogManager(); - - LogManager(const LogManager &) = delete; - LogManager & operator=(const LogManager &) = delete; - LogManager(LogManager &&) = delete; - LogManager & operator=(LogManager &&) = delete; - - /** - * @brief Query log entries for a set of node FQNs - * - * If a LogProvider plugin is registered, delegates to it. - * Otherwise uses the local ring buffer. - * - * @param node_fqns Node FQNs from entity.fqn (WITH leading '/' — normalized internally) - * @param prefix_match If true, match all buffered nodes whose name starts with the given prefix - * (used for Component queries). If false, exact match (App queries). - * @param min_severity Additional severity filter from query parameter. Empty = no override. - * @param context_filter Substring filter applied to log entry's name (logger name). Empty = no filter. - * @param entity_id Entity ID for config lookup. Empty = use defaults. - * @return JSON array of LogEntry objects sorted by id ascending, capped at entity config max_entries. - */ - tl::expected get_logs(const std::vector & node_fqns, bool prefix_match, - const std::string & min_severity, const std::string & context_filter, - const std::string & entity_id); - - /// Get current log configuration for entity (returns defaults if unconfigured) - tl::expected get_config(const std::string & entity_id) const; - - /** - * @brief Update log configuration for an entity - * @return Empty string on success, error message on validation failure - */ - std::string update_config(const std::string & entity_id, const std::optional & severity_filter, - const std::optional & max_entries); - - /** - * @brief Programmatically add a log entry (e.g. from trigger log_settings) - * - * Creates a LogEntry and pushes it to the internal ring buffer using the - * same path as on_rosout(). If a ResourceChangeNotifier is set, emits a - * "logs" CREATED notification so triggers can observe log changes. - * - * @param entity_id Entity to associate the log with (used as logger name) - * @param severity SOVD severity string (debug, info, warning, error, fatal) - * @param message Human-readable log message - * @param metadata Additional JSON metadata stored in the message (appended) - */ - void add_log_entry(const std::string & entity_id, const std::string & severity, const std::string & message, - const nlohmann::json & metadata); - - /// Set the ResourceChangeNotifier for emitting log change events. - /// Called by GatewayNode after both LogManager and the notifier are available. - void set_notifier(ResourceChangeNotifier * notifier); - - /// Callback that maps a ROS 2 node FQN to a manifest entity ID. - /// Returns empty string if the FQN cannot be resolved. - using NodeToEntityFn = std::function; - - /// Set the node-to-entity resolver for trigger notifications. - /// When set, on_rosout() resolves node names to manifest entity IDs before - /// notifying the ResourceChangeNotifier. - void set_node_to_entity_resolver(NodeToEntityFn resolver); - - // ---- Static utilities (no ROS 2 required — safe in unit tests) ---- - - /// Convert ROS 2 uint8 log level -> SOVD severity string ("debug" for unknown levels) - static std::string level_to_severity(uint8_t level); - - /// Convert SOVD severity string -> ROS 2 uint8 log level (0 for invalid/empty) - static uint8_t severity_to_level(const std::string & severity); - - /// Check if a severity string is valid (one of: debug, info, warning, error, fatal) - static bool is_valid_severity(const std::string & severity); - - /// Format a LogEntry as SOVD JSON (id, timestamp, severity, message, context) - static json entry_to_json(const LogEntry & entry); - - /// Strip leading '/' from a node FQN for ring-buffer key normalization - static std::string normalize_fqn(const std::string & fqn); - - // ---- Test injection (for unit tests — do not use in production code) ---- - - /** - * @brief Inject a log entry directly into the ring buffer (bypasses /rosout subscription) - * - * Used by unit tests to populate the buffer without a live ROS 2 graph. - * In production the buffer is populated exclusively by the /rosout callback. - */ - void inject_entry_for_testing(LogEntry entry); - - private: - void on_rosout(const rcl_interfaces::msg::Log::ConstSharedPtr & msg); - - /// Get the effective LogProvider: plugin if registered, else nullptr (use local buffer) - LogProvider * effective_provider() const; - - rclcpp::Node * node_; - PluginManager * plugin_mgr_; - ResourceChangeNotifier * notifier_ = nullptr; - /// Write-once: must be set before ROS executor starts spinning. - /// After that, only read from subscription callbacks (no mutex needed). - NodeToEntityFn node_to_entity_resolver_; - size_t max_buffer_size_; - - rclcpp::Subscription::SharedPtr rosout_sub_; - - // Ring buffers keyed by normalized node name (no leading '/') - // e.g. "powertrain/engine/temp_sensor" -> deque - std::unordered_map> buffers_; - mutable std::mutex buffers_mutex_; - - // Per-entity configuration keyed by entity_id - std::unordered_map configs_; - mutable std::mutex configs_mutex_; - - // Monotonically increasing entry ID (never resets; starts at 1) - std::atomic next_id_{1}; -}; - -} // namespace ros2_medkit_gateway +// Backwards-compatibility shim - header relocated to core/managers/. +#include "ros2_medkit_gateway/core/managers/log_manager.hpp" diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/operation_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/operation_manager.hpp index 7e366368c..d1c4c43c6 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/operation_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/operation_manager.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 mfaferek93 +// 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. @@ -14,287 +14,5 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ros2_medkit_gateway/compat/generic_client_compat.hpp" -#include "ros2_medkit_gateway/core/discovery/models/common.hpp" -#include "ros2_medkit_gateway/discovery/discovery_manager.hpp" -#include "ros2_medkit_serialization/json_serializer.hpp" -#include "ros2_medkit_serialization/service_action_types.hpp" - -namespace ros2_medkit_gateway { - -class ResourceChangeNotifier; - -using json = nlohmann::json; - -/// Result of a synchronous service call -struct ServiceCallResult { - bool success; - json response; - std::string error_message; -}; - -/// Action goal status (matches ROS2 action_msgs/msg/GoalStatus) -enum class ActionGoalStatus : int8_t { - UNKNOWN = 0, - ACCEPTED = 1, - EXECUTING = 2, - CANCELING = 3, - SUCCEEDED = 4, - CANCELED = 5, - ABORTED = 6 -}; - -/// Convert status enum to string -std::string action_status_to_string(ActionGoalStatus status); - -/// Result of sending an action goal -struct ActionSendGoalResult { - bool success; - std::string goal_id; // UUID hex string - bool goal_accepted; - std::string error_message; -}; - -/// Result of canceling an action goal -struct ActionCancelResult { - bool success; - int8_t return_code; // 0=accepted, 1=rejected, 2=unknown_id, 3=terminated - std::string error_message; -}; - -/// Result of getting action result -struct ActionGetResultResult { - bool success; - ActionGoalStatus status; - json result; - std::string error_message; -}; - -/// Tracked action goal info (stored locally) -struct ActionGoalInfo { - std::string goal_id; - std::string action_path; // e.g., /powertrain/engine/long_calibration - std::string action_type; // e.g., example_interfaces/action/Fibonacci - std::string entity_id; // SOVD entity ID (e.g., engine) for trigger notifications - ActionGoalStatus status; - json last_feedback; - std::chrono::system_clock::time_point created_at; - std::chrono::system_clock::time_point last_update; -}; - -/// Manager for ROS2 operations (services and actions) -/// Handles service calls synchronously and action calls asynchronously -class OperationManager { - public: - explicit OperationManager(rclcpp::Node * node, DiscoveryManager * discovery_manager); - - ~OperationManager(); - - OperationManager(const OperationManager &) = delete; - OperationManager & operator=(const OperationManager &) = delete; - OperationManager(OperationManager &&) = delete; - OperationManager & operator=(OperationManager &&) = delete; - - /// Explicitly release subscriptions, clients, and tracked goals. - /// Call while executor is still running to allow safe callback cleanup. - /// Called automatically by destructor, but GatewayNode calls it earlier - /// during its shutdown sequence. - void shutdown(); - - /// Set optional notifier for broadcasting operation status changes to trigger subsystem. - void set_notifier(ResourceChangeNotifier * notifier); - - /// Call a ROS2 service synchronously - /// @param service_path Full service path (e.g., "/powertrain/engine/calibrate") - /// @param service_type Service type (e.g., "std_srvs/srv/Trigger") - /// @param request JSON request body - /// @return ServiceCallResult with response or error - ServiceCallResult call_service(const std::string & service_path, const std::string & service_type, - const json & request); - - /// Find and call a service by component and operation name - /// Uses discovery cache to resolve service path and type if not provided - /// @param component_ns Component namespace (e.g., "/powertrain/engine") - /// @param operation_name Operation name (e.g., "calibrate") - /// @param service_type Optional service type override - /// @param request JSON request body - /// @return ServiceCallResult with response or error - ServiceCallResult call_component_service(const std::string & component_ns, const std::string & operation_name, - const std::optional & service_type, const json & request); - - /// Validate message type format (package/srv/Type or package/action/Type) - static bool is_valid_message_type(const std::string & type); - - /// Validate UUID hex string format (32 hex characters) - static bool is_valid_uuid_hex(const std::string & uuid_hex); - - /// Check if type is a service type (contains /srv/) - static bool is_service_type(const std::string & type); - - /// Check if type is an action type (contains /action/) - static bool is_action_type(const std::string & type); - - // ==================== ACTION OPERATIONS ==================== - - /// Send a goal to an action server using native rclcpp_action internal services - /// @param action_path Full action path (e.g., "/powertrain/engine/long_calibration") - /// @param action_type Action type (e.g., "example_interfaces/action/Fibonacci") - /// @param goal JSON goal data - /// @param entity_id SOVD entity ID for trigger notifications (e.g., "engine") - /// @return ActionSendGoalResult with goal_id or error - ActionSendGoalResult send_action_goal(const std::string & action_path, const std::string & action_type, - const json & goal, const std::string & entity_id = ""); - - /// Send a goal to an action using component namespace and operation name - /// Uses discovery cache to resolve action path and type if not provided - /// @param component_ns Component namespace (e.g., "/powertrain/engine") - /// @param operation_name Operation name (e.g., "long_calibration") - /// @param action_type Optional action type override - /// @param goal JSON goal data - /// @param entity_id SOVD entity ID for trigger notifications (e.g., "engine") - /// @return ActionSendGoalResult with goal_id or error - ActionSendGoalResult send_component_action_goal(const std::string & component_ns, const std::string & operation_name, - const std::optional & action_type, const json & goal, - const std::string & entity_id = ""); - - /// Cancel a running action goal using ros2 action cancel - /// @param action_path Full action path - /// @param goal_id Goal UUID hex string - /// @return ActionCancelResult - ActionCancelResult cancel_action_goal(const std::string & action_path, const std::string & goal_id); - - /// Get the result of a completed action - /// This is a blocking call that waits for the action to complete - /// @param action_path Full action path - /// @param action_type Action type - /// @param goal_id Goal UUID hex string - /// @return ActionGetResultResult with result or error - ActionGetResultResult get_action_result(const std::string & action_path, const std::string & action_type, - const std::string & goal_id); - - /// Get tracked goal info by goal_id - /// @param goal_id Goal UUID hex string - /// @return Optional ActionGoalInfo if found - std::optional get_tracked_goal(const std::string & goal_id) const; - - /// List all tracked goals - /// @return Vector of all tracked goals - std::vector list_tracked_goals() const; - - /// Get all goals for a specific action path - /// @param action_path Full action path (e.g., "/powertrain/engine/long_calibration") - /// @return Vector of goals for that action, sorted by last_update (newest first) - std::vector get_goals_for_action(const std::string & action_path) const; - - /// Get the most recent goal for a specific action path - /// @param action_path Full action path - /// @return Optional ActionGoalInfo if any goal exists for this action - std::optional get_latest_goal_for_action(const std::string & action_path) const; - - /// Update goal status in tracking - /// @param goal_id Goal UUID hex string - /// @param status New status - void update_goal_status(const std::string & goal_id, ActionGoalStatus status); - - /// Update goal feedback in tracking - /// @param goal_id Goal UUID hex string - /// @param feedback New feedback JSON - void update_goal_feedback(const std::string & goal_id, const json & feedback); - - /// Remove completed goals older than specified duration - /// @param max_age Maximum age of completed goals to keep - void cleanup_old_goals(std::chrono::seconds max_age = std::chrono::seconds(300)); - - /// Subscribe to action status topic for real-time updates - /// Called automatically when a goal is sent - /// @param action_path Full action path (e.g., "/powertrain/engine/long_calibration") - void subscribe_to_action_status(const std::string & action_path); - - /// Unsubscribe from action status topic - /// Called when no more active goals exist for this action - /// @param action_path Full action path - void unsubscribe_from_action_status(const std::string & action_path); - - private: - /// Set of clients for an action (internal services) - struct ActionClientSet { - compat::GenericServiceClient::SharedPtr send_goal_client; - compat::GenericServiceClient::SharedPtr get_result_client; - compat::GenericServiceClient::SharedPtr cancel_goal_client; - std::string action_type; // Store type for later use - }; - - /// Convert UUID hex string to JSON array of byte values - json uuid_hex_to_json_array(const std::string & uuid_hex); - - /// Generate a random UUID - std::array generate_uuid(); - - /// Convert UUID bytes to JSON array - json uuid_bytes_to_json_array(const std::array & uuid); - - /// Track a new goal - void track_goal(const std::string & goal_id, const std::string & action_path, const std::string & action_type, - const std::string & entity_id); - - /// Get or create a cached GenericServiceClient for a service - compat::GenericServiceClient::SharedPtr get_or_create_service_client(const std::string & service_path, - const std::string & service_type); - - /// Get or create cached action clients for an action - ActionClientSet & get_or_create_action_clients(const std::string & action_path, const std::string & action_type); - - /// Make cache key from service path and type - static std::string make_client_key(const std::string & service_path, const std::string & service_type); - - rclcpp::Node * node_; - DiscoveryManager * discovery_manager_; - ResourceChangeNotifier * notifier_ = nullptr; - - /// Random number generator for UUID generation - std::mt19937 rng_; - std::mutex rng_mutex_; - - /// Native JSON serializer for service calls - std::shared_ptr serializer_; - - /// Cache for GenericServiceClient instances (key = "service_path|service_type") - mutable std::shared_mutex clients_mutex_; - std::map generic_clients_; - - /// Cache for action client sets (key = action_path) - std::map action_clients_; - - /// Timeout for service calls in seconds (configurable via service_call_timeout_sec param) - int service_call_timeout_sec_; - - /// Map of goal_id -> ActionGoalInfo for tracking active goals - mutable std::mutex goals_mutex_; - std::map tracked_goals_; - - /// Callback for action status topic updates - void on_action_status(const std::string & action_path, const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg); - - /// Convert goal UUID bytes to hex string - std::string uuid_bytes_to_hex(const std::array & uuid) const; - - /// Map of action_path -> status subscription - mutable std::mutex subscriptions_mutex_; - std::map::SharedPtr> status_subscriptions_; -}; - -} // namespace ros2_medkit_gateway +// Backwards-compatibility shim - header relocated to core/managers/. +#include "ros2_medkit_gateway/core/managers/operation_manager.hpp" diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/conversions/fault_msg_conversions.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/conversions/fault_msg_conversions.hpp new file mode 100644 index 000000000..2869f394f --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/conversions/fault_msg_conversions.hpp @@ -0,0 +1,55 @@ +// 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 "ros2_medkit_msgs/msg/environment_data.hpp" +#include "ros2_medkit_msgs/msg/fault.hpp" + +namespace ros2_medkit_gateway::ros2::conversions { + +/// Convert a ros2_medkit_msgs::msg::Fault to JSON. +/// +/// Produces the flat per-fault representation consumed by `list_faults` items, +/// the SSE fault-event payload, and the trigger subsystem's notifier change +/// values. Timestamps become seconds-as-double; severity gains a label. +/// +/// Lives at the ROS-coupled boundary because three independent call sites +/// each translate `ros2_medkit_msgs::msg::Fault` directly into JSON: the +/// fault-service transport adapter, the SSE fault handler (subscribes to the +/// FaultEvent topic directly), and the trigger fault subscriber. Keeping the +/// helper free standing avoids code duplication while preserving the neutral +/// FaultManager contract. +nlohmann::json fault_to_json(const ros2_medkit_msgs::msg::Fault & fault); + +/// Convert a ros2_medkit_msgs::msg::EnvironmentData to JSON. +/// +/// Produces: +/// { +/// "extended_data_records": { "first_occurrence": "...", "last_occurrence": "..." }, +/// "snapshots": [ +/// { "type": "freeze_frame", "name": "...", "data": , "x-medkit": {...} }, +/// { "type": "rosbag", "name": "...", "fault_code": "...", "size_bytes": ..., +/// "duration_sec": ..., "format": "..." } +/// ] +/// } +/// +/// `bulk_data_uri` is intentionally NOT populated for rosbag snapshots; the +/// per-request URL depends on the entity_path that only the handler knows. +/// The handler post-processes rosbag snapshot entries to add it. +nlohmann::json environment_data_to_json(const ros2_medkit_msgs::msg::EnvironmentData & env_data); + +} // namespace ros2_medkit_gateway::ros2::conversions diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/providers/ros2_runtime_introspection.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/providers/ros2_runtime_introspection.hpp new file mode 100644 index 000000000..9a2a4eaff --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/providers/ros2_runtime_introspection.hpp @@ -0,0 +1,158 @@ +// 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/core/data/topic_data_provider.hpp" +#include "ros2_medkit_gateway/core/discovery/models/app.hpp" +#include "ros2_medkit_gateway/core/discovery/models/area.hpp" +#include "ros2_medkit_gateway/core/discovery/models/common.hpp" +#include "ros2_medkit_gateway/core/discovery/models/component.hpp" +#include "ros2_medkit_gateway/core/discovery/models/function.hpp" +#include "ros2_medkit_gateway/core/providers/introspection_provider.hpp" +#include "ros2_medkit_serialization/type_introspection.hpp" + +#include +#include +#include +#include +#include + +namespace ros2_medkit_gateway::ros2 { + +/** + * @brief rclcpp adapter implementing IntrospectionProvider via ROS 2 graph queries. + * + * Owns the introspection of the live ROS 2 graph (node names, services, actions, + * topics) that previously lived inside RuntimeDiscoveryStrategy. Exposes the + * `IntrospectionProvider::introspect()` contract used by the merge pipeline so + * built-in graph queries are treated identically to plugin-provided introspection. + * + * In addition to the IntrospectionProvider contract, this class exposes the + * service/action discovery and topic-map management methods that the gateway's + * data, operation, and discovery layers consume directly (independent of the + * merge pipeline). + * + * Threading: the underlying rclcpp graph queries are not thread-safe across + * concurrent introspection. Callers (DiscoveryManager / RuntimeLayer) serialize + * access via the gateway's refresh lock. + */ +class Ros2RuntimeIntrospection : public IntrospectionProvider { + public: + /** + * @brief Per-introspection configuration knobs. + */ + struct RuntimeConfig { + /// Map ROS 2 namespaces to Function entities when true. + bool create_functions_from_namespaces{true}; + }; + + /** + * @param node Non-owning ROS node used for graph queries. + */ + explicit Ros2RuntimeIntrospection(rclcpp::Node * node); + + ~Ros2RuntimeIntrospection() override = default; + + Ros2RuntimeIntrospection(const Ros2RuntimeIntrospection &) = delete; + Ros2RuntimeIntrospection & operator=(const Ros2RuntimeIntrospection &) = delete; + Ros2RuntimeIntrospection(Ros2RuntimeIntrospection &&) = delete; + Ros2RuntimeIntrospection & operator=(Ros2RuntimeIntrospection &&) = delete; + + /// Update runtime introspection configuration. + void set_config(const RuntimeConfig & config); + + // --------------------------------------------------------------------------- + // IntrospectionProvider + // --------------------------------------------------------------------------- + + /** + * @brief Build new entities from the current ROS 2 graph. + * + * Returns Apps (one per node) and Functions (one per namespace) as + * `new_entities`. Areas and Components are intentionally never produced by + * runtime introspection - they come from the manifest layer or the + * HostInfoProvider. + */ + IntrospectionResult introspect(const IntrospectionInput & input) override; + + // --------------------------------------------------------------------------- + // Direct access (used by RuntimeLayer for gap-fill linking and by managers + // that need service/action/topic introspection independent of the pipeline). + // --------------------------------------------------------------------------- + + /// Discover the live nodes as Apps. Always queries the ROS 2 graph; do not + /// call from hot paths. + std::vector discover_apps(); + + /// Group nodes by namespace into Function entities (no graph query). + std::vector discover_functions(const std::vector & apps); + + /// Group nodes by namespace into Function entities (queries the graph). + std::vector discover_functions(); + + /// Discover all services in the ROS 2 graph with schema enrichment. + std::vector discover_services(); + + /// Discover all actions (synthesised from `/_action/send_goal` services). + std::vector discover_actions(); + + /// Find a service by component namespace and operation name (cache lookup). + std::optional find_service(const std::string & component_ns, const std::string & operation_name) const; + + /// Find an action by component namespace and operation name (cache lookup). + std::optional find_action(const std::string & component_ns, const std::string & operation_name) const; + + /// Provide the topic data provider used to enrich Apps with publish/subscribe + /// topics. Pointer must outlive this provider. + void set_topic_data_provider(TopicDataProvider * provider); + + /// Provide the type introspection used to enrich service/action schemas. + /// Pointer must outlive this provider. + void set_type_introspection(ros2_medkit_serialization::TypeIntrospection * introspection); + + /// Rebuild the cached ` -> ComponentTopics` map from the topic + /// data provider. Called by the gateway's refresh path. + void refresh_topic_map(); + + /// True once the topic map has been built at least once. + bool is_topic_map_ready() const; + + private: + /// Extract area ID from namespace ("/powertrain/engine" -> "powertrain"). + static std::string extract_area_from_namespace(const std::string & ns); + + /// Extract the last segment from a path ("/a/b/c" -> "c"). + static std::string extract_name_from_path(const std::string & path); + + /// True if the path lives directly under the given namespace. + bool path_belongs_to_namespace(const std::string & path, const std::string & ns) const; + + /// True if a service path looks like internal ROS 2 infrastructure + /// (parameter services, action internals, etc.). + static bool is_internal_service(const std::string & service_path); + + rclcpp::Node * node_; + RuntimeConfig config_; + TopicDataProvider * topic_data_provider_{nullptr}; + ros2_medkit_serialization::TypeIntrospection * type_introspection_{nullptr}; + + std::vector cached_services_; + std::vector cached_actions_; + + std::map cached_topic_map_; + bool topic_map_ready_{false}; +}; + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_action_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_action_transport.hpp new file mode 100644 index 000000000..69b6d1411 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_action_transport.hpp @@ -0,0 +1,101 @@ +// 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 +#include +#include +#include + +#include "ros2_medkit_gateway/compat/generic_client_compat.hpp" +#include "ros2_medkit_gateway/core/transports/action_transport.hpp" +#include "ros2_medkit_serialization/json_serializer.hpp" + +namespace ros2_medkit_gateway::ros2 { + +/** + * @brief rclcpp adapter implementing ActionTransport. + * + * Owns the action-internal-service client cache (send_goal / get_result / + * cancel_goal) and the GoalStatusArray subscription cache that + * OperationManager previously held directly. Subscriptions captured via + * lambda are torn down before any other member destructs to satisfy the + * subscription-destructor pattern (see CLAUDE.md: callbacks must not fire + * on a partially-destroyed object). + */ +class Ros2ActionTransport : public ActionTransport { + public: + /** + * @param node Non-owning ROS node used for client + subscription creation. + */ + explicit Ros2ActionTransport(rclcpp::Node * node); + + ~Ros2ActionTransport() override; + + Ros2ActionTransport(const Ros2ActionTransport &) = delete; + Ros2ActionTransport & operator=(const Ros2ActionTransport &) = delete; + Ros2ActionTransport(Ros2ActionTransport &&) = delete; + Ros2ActionTransport & operator=(Ros2ActionTransport &&) = delete; + + ActionSendGoalResult send_goal(const std::string & action_path, const std::string & action_type, const json & goal, + std::chrono::duration timeout) override; + + ActionCancelResult cancel_goal(const std::string & action_path, const std::string & goal_id, + std::chrono::duration timeout) override; + + ActionGetResultResult get_result(const std::string & action_path, const std::string & action_type, + const std::string & goal_id, std::chrono::duration timeout) override; + + void subscribe_status(const std::string & action_path, StatusCallback callback) override; + + void unsubscribe_status(const std::string & action_path) override; + + private: + /// Cached internal-service clients for one action path. + struct ActionClientSet { + compat::GenericServiceClient::SharedPtr send_goal_client; + compat::GenericServiceClient::SharedPtr get_result_client; + compat::GenericServiceClient::SharedPtr cancel_goal_client; + std::string action_type; + }; + + ActionClientSet & get_or_create_clients(const std::string & action_path, const std::string & action_type); + + void on_status_msg(const std::string & action_path, const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg); + + rclcpp::Node * node_; + std::shared_ptr serializer_; + + /// Set on the first shutdown signal so callbacks short-circuit while the + /// adapter is being torn down. + std::atomic shutdown_requested_{false}; + + mutable std::shared_mutex clients_mutex_; + std::map action_clients_; + + /// Status subscriptions and the user-provided callbacks they fan into. + /// Cleared in the destructor (and in unsubscribe_status) before any other + /// member destructs. + mutable std::mutex subscriptions_mutex_; + std::map::SharedPtr> status_subscriptions_; + std::map status_callbacks_; +}; + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_fault_service_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_fault_service_transport.hpp new file mode 100644 index 000000000..7d8ddf961 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_fault_service_transport.hpp @@ -0,0 +1,111 @@ +// 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 "ros2_medkit_gateway/core/transports/fault_service_transport.hpp" +#include "ros2_medkit_msgs/srv/clear_fault.hpp" +#include "ros2_medkit_msgs/srv/get_fault.hpp" +#include "ros2_medkit_msgs/srv/get_rosbag.hpp" +#include "ros2_medkit_msgs/srv/get_snapshots.hpp" +#include "ros2_medkit_msgs/srv/list_faults.hpp" +#include "ros2_medkit_msgs/srv/list_rosbags.hpp" +#include "ros2_medkit_msgs/srv/report_fault.hpp" + +namespace ros2_medkit_gateway::ros2 { + +/** + * @brief rclcpp adapter implementing FaultServiceTransport. + * + * Owns the seven `rclcpp::Client` instances and the + * seven per-client mutexes that previously lived inside FaultManager. Each + * mutex serialises one service direction so read operations (list, get) are + * not blocked by slow write operations (report_fault triggers snapshot + * capture inside the fault manager node). + * + * Performs the ros2_medkit_msgs <-> JSON translation internally, returning + * neutral FaultResult and FaultWithEnvJsonResult structures so the FaultManager + * body lives in the ROS-free build layer. + */ +class Ros2FaultServiceTransport : public FaultServiceTransport { + public: + /** + * @param node Non-owning ROS node used for client creation and to derive + * the configured fault_manager namespace. + */ + explicit Ros2FaultServiceTransport(rclcpp::Node * node); + + ~Ros2FaultServiceTransport() override; + + Ros2FaultServiceTransport(const Ros2FaultServiceTransport &) = delete; + Ros2FaultServiceTransport & operator=(const Ros2FaultServiceTransport &) = delete; + Ros2FaultServiceTransport(Ros2FaultServiceTransport &&) = delete; + Ros2FaultServiceTransport & operator=(Ros2FaultServiceTransport &&) = delete; + + FaultResult report_fault(const std::string & fault_code, uint8_t severity, const std::string & description, + const std::string & source_id) override; + + FaultResult list_faults(const std::string & source_id, bool include_prefailed, bool include_confirmed, + bool include_cleared, bool include_healed, bool include_muted, + bool include_clusters) override; + + FaultWithEnvJsonResult get_fault_with_env(const std::string & fault_code, const std::string & source_id) override; + + FaultResult get_fault(const std::string & fault_code, const std::string & source_id) override; + + FaultResult clear_fault(const std::string & fault_code) override; + + FaultResult get_snapshots(const std::string & fault_code, const std::string & topic) override; + + FaultResult get_rosbag(const std::string & fault_code) override; + + FaultResult list_rosbags(const std::string & entity_fqn) override; + + bool wait_for_services(std::chrono::duration timeout) override; + + bool is_available() const override; + + private: + rclcpp::Node * node_; + + rclcpp::Client::SharedPtr report_fault_client_; + rclcpp::Client::SharedPtr get_fault_client_; + rclcpp::Client::SharedPtr list_faults_client_; + rclcpp::Client::SharedPtr clear_fault_client_; + rclcpp::Client::SharedPtr get_snapshots_client_; + rclcpp::Client::SharedPtr get_rosbag_client_; + rclcpp::Client::SharedPtr list_rosbags_client_; + + double service_timeout_sec_{5.0}; + std::string fault_manager_base_path_{"/fault_manager"}; + + /// Per-client mutexes for thread-safe service calls. + /// Split by service client so that read operations (list, get) are not blocked + /// by slow write operations (report_fault with snapshot capture). + mutable std::mutex report_mutex_; + mutable std::mutex list_mutex_; + mutable std::mutex get_mutex_; + mutable std::mutex clear_mutex_; + mutable std::mutex snapshots_mutex_; + mutable std::mutex rosbag_mutex_; + mutable std::mutex list_rosbags_mutex_; +}; + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_log_source.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_log_source.hpp new file mode 100644 index 000000000..adaf04bb3 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_log_source.hpp @@ -0,0 +1,78 @@ +// 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 "ros2_medkit_gateway/core/transports/log_source.hpp" + +namespace ros2_medkit_gateway::ros2 { + +/** + * @brief rclcpp adapter implementing LogSource by subscribing to /rosout. + * + * Owns the rclcpp::Subscription that previously + * lived inside LogManager. Performs the rcl_interfaces::msg::Log -> LogEntry + * conversion (level, name, message, function/file/line, timestamp) and + * delivers each entry to the registered callback. The manager body then lives + * in the ROS-free build layer. + * + * Threading: the underlying rclcpp callback is invoked on the gateway's + * executor thread. The shutdown guard pattern ensures that any in-flight + * callback short-circuits before the subscription is released, so the + * registered EntryCallback is guaranteed not to fire after stop() returns. + */ +class Ros2LogSource : public LogSource { + public: + /** + * @param node Non-owning ROS node used for subscription creation. + */ + explicit Ros2LogSource(rclcpp::Node * node); + + ~Ros2LogSource() override; + + Ros2LogSource(const Ros2LogSource &) = delete; + Ros2LogSource & operator=(const Ros2LogSource &) = delete; + Ros2LogSource(Ros2LogSource &&) = delete; + Ros2LogSource & operator=(Ros2LogSource &&) = delete; + + /// Subscribe to /rosout and route each message through to `callback`. + /// Idempotent: a second start() replaces the previous callback and reuses + /// the existing subscription. + void start(EntryCallback callback) override; + + /// Stop delivering entries. Idempotent. After stop() returns the registered + /// callback is guaranteed not to fire again, even if rclcpp has not yet + /// drained queued messages. + void stop() override; + + private: + rclcpp::Node * node_; + + /// Set to true on stop() (or destruction) before the subscription is + /// released, so any in-flight callback short-circuits before touching + /// members that are about to destruct. + std::atomic shutdown_requested_{false}; + + /// Guards callback_ + rosout_sub_ across start()/stop() races. + std::mutex mutex_; + EntryCallback callback_; + rclcpp::Subscription::SharedPtr rosout_sub_; +}; + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_parameter_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_parameter_transport.hpp new file mode 100644 index 000000000..992ac867e --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_parameter_transport.hpp @@ -0,0 +1,163 @@ +// 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 +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/core/configuration/parameter_types.hpp" +#include "ros2_medkit_gateway/core/transports/parameter_transport.hpp" + +namespace ros2_medkit_gateway::ros2 { + +/** + * @brief rclcpp adapter implementing ParameterTransport. + * + * Owns the rclcpp::SyncParametersClient cache, the rclcpp::Parameter defaults + * cache, the negative-cache for unreachable nodes, the spin_mutex serialising + * parameter-client spins, and the JSON <-> rclcpp::ParameterValue conversion + * helpers that previously lived inside ConfigurationManager. + * + * The adapter exposes only the neutral ParameterTransport surface to the + * manager; all rclcpp types stay confined to this translation unit. + */ +class Ros2ParameterTransport : public ParameterTransport { + public: + /** + * @param node Non-owning ROS node used to declare parameters and to derive + * the gateway's own FQN for the self-node short circuit. + * @param service_timeout_sec Timeout for SyncParametersClient::wait_for_service + * and underlying parameter-service calls. + * @param negative_cache_ttl_sec How long an unavailable node remains in the + * negative cache before another IPC attempt. + * Set to 0 to disable. + */ + Ros2ParameterTransport(rclcpp::Node * node, double service_timeout_sec, double negative_cache_ttl_sec); + + ~Ros2ParameterTransport() override; + + Ros2ParameterTransport(const Ros2ParameterTransport &) = delete; + Ros2ParameterTransport & operator=(const Ros2ParameterTransport &) = delete; + Ros2ParameterTransport(Ros2ParameterTransport &&) = delete; + Ros2ParameterTransport & operator=(Ros2ParameterTransport &&) = delete; + + // ParameterTransport implementation ----------------------------------------- + + bool is_self_node(const std::string & node_name) const override; + + ParameterResult list_parameters(const std::string & node_name) override; + ParameterResult get_parameter(const std::string & node_name, const std::string & param_name) override; + ParameterResult set_parameter(const std::string & node_name, const std::string & param_name, + const json & value) override; + + ParameterResult list_own_parameters() override; + ParameterResult get_own_parameter(const std::string & param_name) override; + + ParameterResult get_default(const std::string & node_name, const std::string & param_name) override; + ParameterResult list_defaults(const std::string & node_name) override; + + bool is_node_available(const std::string & node_name) const override; + void invalidate(const std::string & node_name) override; + void shutdown() override; + + private: + /// Get or create a cached SyncParametersClient for the given node. + std::shared_ptr get_param_client(const std::string & node_name); + + /// Cache default values for a node (called on first access). + /// @pre spin_mutex_ must be held by the caller. + void cache_default_values(const std::string & node_name); + + /// Check if a node is in the negative cache (recently unavailable). + bool is_node_unavailable(const std::string & node_name) const; + + /// Mark a node as unavailable in the negative cache. + void mark_node_unavailable(const std::string & node_name); + + /// Convert ROS 2 parameter type to string. + std::string parameter_type_to_string(rclcpp::ParameterType type) const; + + /// Convert ROS 2 ParameterValue to JSON. + json parameter_value_to_json(const rclcpp::ParameterValue & value) const; + + /// Convert JSON value to ROS 2 ParameterValue. + rclcpp::ParameterValue json_to_parameter_value(const json & value, rclcpp::ParameterType hint_type) const; + + /// Get the service timeout as a chrono duration. + std::chrono::duration get_service_timeout() const; + + /// Try to acquire spin_mutex_ with timeout. + /// Returns unique_lock on success, nullopt on timeout (result populated with error). + std::optional> try_acquire_spin_lock(ParameterResult & result); + + rclcpp::Node * node_; + + /// Timeout for waiting for parameter services. + double service_timeout_sec_; + + /// Negative cache TTL in seconds (0 = disabled). + double negative_cache_ttl_sec_; + + /// Gateway's own fully qualified node name (for self-query detection). + std::string own_node_fqn_; + + /// Shutdown flag - prevents use-after-free on param_node_ after shutdown. + std::atomic shutdown_requested_{false}; + + /// Negative cache: nodes whose parameter service was recently unavailable. + /// Avoids repeated blocking waits on nodes that don't have parameter services. + mutable std::shared_mutex negative_cache_mutex_; + std::unordered_map unavailable_nodes_; + + /// Cache of default parameter values per node. + /// Key: node_name, Value: map of param_name -> Parameter. + mutable std::mutex defaults_mutex_; + std::map> default_values_; + + /// Mutex to serialize spin operations on param_node_. + /// SyncParametersClient::wait_for_service/get_parameters/etc spin param_node_ + /// internally via spin_node_until_future_complete which is NOT thread-safe. + /// Declared BEFORE param_node_ so it outlives the node during destruction. + mutable std::timed_mutex spin_mutex_; + + /// Internal node for parameter client operations. + /// Created once in constructor - must be in DDS graph early for fast service discovery. + std::shared_ptr param_node_; + + /// Cached SyncParametersClient per target node. + mutable std::mutex clients_mutex_; + std::map> param_clients_; + + /// Maximum entries in negative cache before hard eviction. + static constexpr size_t kMaxNegativeCacheSize = 500; + + /// Margin added to service_timeout_sec_ for spin_mutex acquisition timeout. + /// Must exceed service_timeout_sec_ to avoid spurious TIMEOUT errors. + static constexpr double kSpinLockMarginSec{1.0}; + + /// Timeout for shutdown to wait for in-flight IPC. + static constexpr std::chrono::seconds kShutdownTimeout{10}; +}; + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_service_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_service_transport.hpp new file mode 100644 index 000000000..c61936b57 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_service_transport.hpp @@ -0,0 +1,67 @@ +// 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 + +#include "ros2_medkit_gateway/compat/generic_client_compat.hpp" +#include "ros2_medkit_gateway/core/transports/service_transport.hpp" +#include "ros2_medkit_serialization/json_serializer.hpp" + +namespace ros2_medkit_gateway::ros2 { + +/** + * @brief rclcpp adapter implementing ServiceTransport. + * + * Owns the GenericServiceClient cache and the JsonSerializer that + * OperationManager previously held directly. Service-call body lifted + * verbatim from the legacy OperationManager::call_service. + */ +class Ros2ServiceTransport : public ServiceTransport { + public: + /** + * @param node Non-owning ROS node used for client creation. + */ + explicit Ros2ServiceTransport(rclcpp::Node * node); + + ~Ros2ServiceTransport() override; + + Ros2ServiceTransport(const Ros2ServiceTransport &) = delete; + Ros2ServiceTransport & operator=(const Ros2ServiceTransport &) = delete; + Ros2ServiceTransport(Ros2ServiceTransport &&) = delete; + Ros2ServiceTransport & operator=(Ros2ServiceTransport &&) = delete; + + ServiceCallResult call(const std::string & service_path, const std::string & service_type, const json & request, + std::chrono::duration timeout) override; + + private: + static std::string make_client_key(const std::string & service_path, const std::string & service_type); + + compat::GenericServiceClient::SharedPtr get_or_create_client(const std::string & service_path, + const std::string & service_type); + + rclcpp::Node * node_; + std::shared_ptr serializer_; + + mutable std::shared_mutex clients_mutex_; + std::map clients_; +}; + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_topic_subscription_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_topic_subscription_transport.hpp new file mode 100644 index 000000000..2a3ee2150 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_topic_subscription_transport.hpp @@ -0,0 +1,87 @@ +// 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 "ros2_medkit_gateway/core/transports/topic_subscription_transport.hpp" +#include "ros2_medkit_gateway/ros2/trigger_topic_subscriber.hpp" + +namespace ros2_medkit_gateway::ros2 { + +/** + * @brief rclcpp adapter implementing TopicSubscriptionTransport by wrapping + * TriggerTopicSubscriber. + * + * Each subscribe() allocates a unique handle key, registers the user's + * SampleCallback against the underlying TriggerTopicSubscriber, and returns + * a handle whose destructor unsubscribes that key. The trigger manager + * stores these handles inside its tracking map; dropping a trigger drops + * its handle which deterministically tears down the underlying + * rclcpp::GenericSubscription via TriggerTopicSubscriber::unsubscribe(). + * + * The adapter is non-owning of the TriggerTopicSubscriber: the subscriber + * is owned by GatewayNode and outlives the adapter (via the trigger + * subsystem's destruction order). + */ +class Ros2TopicSubscriptionTransport : public TopicSubscriptionTransport { + public: + /** + * @param subscriber Non-owning pointer to the underlying + * TriggerTopicSubscriber. Must outlive every handle + * returned by subscribe(). + */ + explicit Ros2TopicSubscriptionTransport(TriggerTopicSubscriber * subscriber); + + ~Ros2TopicSubscriptionTransport() override = default; + + Ros2TopicSubscriptionTransport(const Ros2TopicSubscriptionTransport &) = delete; + Ros2TopicSubscriptionTransport & operator=(const Ros2TopicSubscriptionTransport &) = delete; + Ros2TopicSubscriptionTransport(Ros2TopicSubscriptionTransport &&) = delete; + Ros2TopicSubscriptionTransport & operator=(Ros2TopicSubscriptionTransport &&) = delete; + + std::unique_ptr subscribe(const std::string & topic_path, const std::string & msg_type, + SampleCallback callback) override; + + private: + /// Concrete handle: holds the subscriber pointer + handle key. The dtor + /// unsubscribes the handle key, satisfying the subscription-destructor + /// pattern. + class Handle : public TopicSubscriptionHandle { + public: + Handle(TriggerTopicSubscriber * subscriber, std::string key); + ~Handle() override; + + Handle(const Handle &) = delete; + Handle & operator=(const Handle &) = delete; + Handle(Handle &&) = delete; + Handle & operator=(Handle &&) = delete; + + private: + TriggerTopicSubscriber * subscriber_; + std::string key_; + }; + + std::string allocate_key(); + + TriggerTopicSubscriber * subscriber_; + std::atomic next_key_{1}; +}; + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp new file mode 100644 index 000000000..04b7dfe79 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp @@ -0,0 +1,98 @@ +// 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 +#include +#include + +#include "ros2_medkit_gateway/core/transports/topic_transport.hpp" +#include "ros2_medkit_serialization/json_serializer.hpp" +#include "ros2_medkit_serialization/type_introspection.hpp" + +namespace ros2_medkit_gateway { + +class TopicDataProvider; + +namespace ros2 { + +/** + * @brief rclcpp adapter implementing the TopicTransport port. + * + * Owns the publisher cache, the JsonSerializer instance and the + * TypeIntrospection helper (lives in `ros2_medkit_serialization` alongside + * the rest of the rosidl glue) that the manager previously held directly. + * The sample path delegates to an attached TopicDataProvider; the adapter + * itself does not maintain a sampling executor. + */ +class Ros2TopicTransport : public TopicTransport { + public: + /** + * @param node Non-owning ROS node used for graph queries and generic + * publisher creation. + * @param default_sample_timeout_sec Used when callers pass a non-positive + * timeout into sample(). + */ + Ros2TopicTransport(rclcpp::Node * node, double default_sample_timeout_sec); + ~Ros2TopicTransport() override = default; + + Ros2TopicTransport(const Ros2TopicTransport &) = delete; + Ros2TopicTransport & operator=(const Ros2TopicTransport &) = delete; + Ros2TopicTransport(Ros2TopicTransport &&) = delete; + Ros2TopicTransport & operator=(Ros2TopicTransport &&) = delete; + + /** + * @brief Attach the per-entity TopicDataProvider used for the sample path. + * + * Non-owning. The wiring layer drops the provider before destroying it by + * passing nullptr. Concurrent set during steady-state traffic is not + * supported; the wiring contract expects this to be called from gateway + * lifecycle code only. + */ + void set_data_provider(TopicDataProvider * provider); + + json publish(const std::string & topic_path, const std::string & msg_type, const json & data, + std::chrono::duration timeout) override; + + TopicSample sample(const std::string & topic_name, std::chrono::duration timeout) override; + + std::pair count_publishers_subscribers(const std::string & topic_name) const override; + + ros2_medkit_serialization::TypeIntrospection * get_type_introspection() const override { + return type_introspection_.get(); + } + + private: + rclcpp::GenericPublisher::SharedPtr get_or_create_publisher(const std::string & topic_path, + const std::string & msg_type); + + rclcpp::Node * node_; + std::shared_ptr serializer_; + std::unique_ptr type_introspection_; + + mutable std::shared_mutex publishers_mutex_; + std::unordered_map publishers_; + + TopicDataProvider * data_provider_{nullptr}; + double default_sample_timeout_sec_; +}; + +} // namespace ros2 +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/trigger_topic_subscriber.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/trigger_topic_subscriber.hpp new file mode 100644 index 000000000..da8aa8967 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/trigger_topic_subscriber.hpp @@ -0,0 +1,152 @@ +// 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 +#include +#include + +#include + +#include "ros2_medkit_serialization/json_serializer.hpp" + +namespace ros2_medkit_gateway { + +/** + * @brief rclcpp-coupled topic subscription executor for the trigger subsystem. + * + * Each trigger that observes a "data" resource registers exactly one + * subscription here, identified by a caller-provided handle key. The + * subscriber owns the rclcpp::GenericSubscription, performs the CDR -> JSON + * deserialization, and delivers each sample to the handle's callback. When + * the topic's type cannot be resolved at subscribe time (publisher not yet + * up), the entry is queued as "pending" and retried every 5 seconds. Pending + * entries time out after 60 seconds. + * + * Threading: subscribe()/unsubscribe()/shutdown() are mutex-guarded. + * Per-handle callbacks are invoked from rclcpp executor threads. The + * subscription-destructor pattern (callbacks cannot fire on a partially + * destroyed subscriber) is enforced by the shutdown_flag_ guard followed by + * subscription map clearing inside shutdown(). + * + * The lifetime of an individual subscription is tied to the corresponding + * handle key: unsubscribe(handle_key) atomically removes both the user + * callback and the underlying rclcpp::Subscription, so any in-flight + * dispatch sees the empty map and short-circuits. + * + * The retry callback (set via set_retry_callback) is fired on every retry + * tick. The trigger manager wires this to retry_unresolved_triggers() so + * that triggers whose resource_path was not yet resolvable to a topic at + * creation time get a chance to convert into a real subscription. + */ +class TriggerTopicSubscriber { + public: + /// Callback delivering one deserialized sample as JSON. Invoked on the + /// rclcpp executor thread. + using SampleCallback = std::function; + + /** + * @brief Construct the subscriber. + * @param node Non-owning ROS node used to create rclcpp::GenericSubscription + * and the retry wall timer. + */ + explicit TriggerTopicSubscriber(rclcpp::Node * node); + + ~TriggerTopicSubscriber(); + + TriggerTopicSubscriber(const TriggerTopicSubscriber &) = delete; + TriggerTopicSubscriber & operator=(const TriggerTopicSubscriber &) = delete; + TriggerTopicSubscriber(TriggerTopicSubscriber &&) = delete; + TriggerTopicSubscriber & operator=(TriggerTopicSubscriber &&) = delete; + + /** + * @brief Subscribe to a topic under a unique handle key. + * + * Each call yields one rclcpp::GenericSubscription dedicated to the + * caller's handle. If @p msg_type is empty the type is resolved from the + * ROS graph; if no publisher is yet advertising the topic, the entry is + * queued for retry. + * + * Idempotent: a second call with an already-registered key replaces both + * the type and the callback for that key. + * + * @param topic_name Fully qualified ROS 2 topic name (e.g. "/sensor/temp"). + * @param msg_type Fully qualified message type, or empty for auto-resolve. + * @param handle_key Caller-allocated unique key. The transport adapter uses + * this to disambiguate per-trigger subscriptions. + * @param callback Invoked for every successfully deserialized sample. + */ + void subscribe(const std::string & topic_name, const std::string & msg_type, const std::string & handle_key, + SampleCallback callback); + + /// Drop the subscription for @p handle_key. Both the user callback and the + /// rclcpp::GenericSubscription are released; in-flight dispatch is + /// guaranteed not to fire after this returns. + void unsubscribe(const std::string & handle_key); + + /// Shut down all subscriptions. Idempotent. + void shutdown(); + + /// Callback invoked on every retry tick (alongside pending-subscription + /// resolution). The trigger manager wires this to retry_unresolved_triggers(). + using RetryCallback = std::function; + void set_retry_callback(RetryCallback cb); + + private: + /// Per-handle live subscription state. + struct LiveEntry { + rclcpp::GenericSubscription::SharedPtr subscription; + std::string topic_name; + std::string msg_type; + SampleCallback callback; + }; + + /// Per-handle pending entry awaiting topic-type resolution. + struct PendingEntry { + std::string topic_name; + SampleCallback callback; + std::chrono::steady_clock::time_point created_at; + }; + + /// Timeout for pending subscriptions before giving up (seconds). + static constexpr int kPendingTimeoutSec = 60; + + /// Interval between retry attempts for pending subscriptions (seconds). + static constexpr int kRetryIntervalSec = 5; + + /// Create a GenericSubscription for @p handle_key with the resolved type. + /// Caller must hold mutex_. + void create_subscription_internal(const std::string & handle_key, const std::string & topic_name, + const std::string & msg_type, SampleCallback callback); + + /// Retry callback for pending subscriptions. Called by retry_timer_. + void retry_pending_subscriptions(); + + rclcpp::Node * node_; + std::shared_ptr serializer_; + std::mutex mutex_; + std::unordered_map live_; + std::unordered_map pending_; + rclcpp::TimerBase::SharedPtr retry_timer_; + RetryCallback retry_callback_; + std::atomic shutdown_flag_{false}; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_fault_subscriber.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_fault_subscriber.hpp index 89ec97791..1fc5ba4dc 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_fault_subscriber.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_fault_subscriber.hpp @@ -34,7 +34,7 @@ namespace ros2_medkit_gateway { * * Entity ID is derived from the first reporting source of the fault. The * fault_code is used as the resource_path. The full fault JSON (via - * FaultManager::fault_to_json) is passed as the change value. + * ros2::conversions::fault_to_json) is passed as the change value. */ class TriggerFaultSubscriber { public: diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_topic_subscriber.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_topic_subscriber.hpp index 7c1f67b41..8a188960c 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_topic_subscriber.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_topic_subscriber.hpp @@ -14,144 +14,5 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "ros2_medkit_gateway/core/resource_change_notifier.hpp" -#include "ros2_medkit_serialization/json_serializer.hpp" - -namespace ros2_medkit_gateway { - -/** - * @brief Manages persistent ROS 2 topic subscriptions for data triggers. - * - * When a trigger observes a "data" resource, TriggerTopicSubscriber creates a - * GenericSubscription for the corresponding topic, deserializes incoming - * messages to JSON, and forwards them to the ResourceChangeNotifier for - * trigger evaluation. - * - * Subscriptions are ref-counted: multiple triggers on the same topic share a - * single ROS 2 subscription. When the last trigger on a topic is removed, the - * subscription is destroyed. - * - * When a topic's type cannot be determined at subscribe() time (e.g., the - * publisher hasn't started yet), the subscription is queued as "pending" and - * retried every 5 seconds by a wall timer. Pending subscriptions time out - * after 60 seconds. - */ -class TriggerTopicSubscriber { - public: - /** - * @brief Construct the subscriber. - * @param node Raw pointer to the ROS 2 node for creating subscriptions (caller manages lifetime) - * @param notifier ResourceChangeNotifier to forward data events to - */ - TriggerTopicSubscriber(rclcpp::Node * node, ResourceChangeNotifier & notifier); - - ~TriggerTopicSubscriber(); - - // Non-copyable, non-movable - TriggerTopicSubscriber(const TriggerTopicSubscriber &) = delete; - TriggerTopicSubscriber & operator=(const TriggerTopicSubscriber &) = delete; - TriggerTopicSubscriber(TriggerTopicSubscriber &&) = delete; - TriggerTopicSubscriber & operator=(TriggerTopicSubscriber &&) = delete; - - /** - * @brief Subscribe to a topic for data trigger monitoring. - * - * If the topic is already subscribed, adds the entity_id to the set. - * Otherwise, creates a GenericSubscription that - * deserializes messages to JSON and forwards them to the notifier. - * - * If the topic type cannot be determined (e.g., no publishers yet), - * the subscription is queued as pending and retried periodically. - * - * @param topic_name ROS 2 topic name (e.g., "/sensor/temperature") - * @param resource_path Resource path for notification matching (e.g., "/temperature") - * @param entity_id Entity ID for the notification (e.g., "temp_sensor") - */ - void subscribe(const std::string & topic_name, const std::string & resource_path, const std::string & entity_id); - - /** - * @brief Unsubscribe an entity from a topic. - * - * Removes the entity_id from the set. - * If the set becomes empty, the ROS 2 subscription is destroyed. - * - * @param topic_name ROS 2 topic name to unsubscribe from - * @param entity_id Entity ID to remove from the subscription - */ - void unsubscribe(const std::string & topic_name, const std::string & entity_id); - - /** - * @brief Shutdown all subscriptions. - * - * Clears all active subscriptions. Safe to call multiple times. - */ - void shutdown(); - - /// Callback invoked by the retry timer alongside topic type resolution. - /// Used to let TriggerManager re-resolve triggers whose topic names - /// were unknown at creation time. - using RetryCallback = std::function; - void set_retry_callback(RetryCallback cb); - - private: - /// Per-topic subscription state with multi-entity support. - struct SubscriptionEntry { - rclcpp::GenericSubscription::SharedPtr subscription; - std::string resource_path; ///< Resource path for notifications (e.g., "/temperature") - std::unordered_set entity_ids; ///< All entities watching this topic - }; - - /// Pending subscription awaiting topic type resolution. - struct PendingSubscription { - std::string resource_path; - std::unordered_set entity_ids; - std::chrono::steady_clock::time_point created_at; - }; - - /// Timeout for pending subscriptions before giving up (seconds). - static constexpr int kPendingTimeoutSec = 60; - - /// Interval between retry attempts for pending subscriptions (seconds). - static constexpr int kRetryIntervalSec = 5; - - /** - * @brief Create a GenericSubscription for a topic with known type. - * - * Extracted from subscribe() so both subscribe() and retry logic can share it. - * Caller must hold mutex_. - * - * @param topic_name ROS 2 topic name - * @param msg_type Fully qualified message type (e.g., "std_msgs/msg/Float64") - * @param resource_path Resource path for notifications - * @param entity_ids Set of entity IDs watching this topic - */ - void create_subscription_internal(const std::string & topic_name, const std::string & msg_type, - const std::string & resource_path, - const std::unordered_set & entity_ids); - - /// Retry callback for pending subscriptions. Called by retry_timer_. - void retry_pending_subscriptions(); - - rclcpp::Node * node_; - ResourceChangeNotifier & notifier_; - std::shared_ptr serializer_; - std::mutex mutex_; - std::unordered_map subscriptions_; - std::unordered_map pending_; ///< Topics awaiting type resolution - rclcpp::TimerBase::SharedPtr retry_timer_; - RetryCallback retry_callback_; - std::atomic shutdown_flag_{false}; -}; - -} // namespace ros2_medkit_gateway +// Backwards-compatibility shim - header relocated to ros2/. +#include "ros2_medkit_gateway/ros2/trigger_topic_subscriber.hpp" diff --git a/src/ros2_medkit_gateway/launch/gateway.launch.py b/src/ros2_medkit_gateway/launch/gateway.launch.py index 41fcbdf82..50d39d3ee 100644 --- a/src/ros2_medkit_gateway/launch/gateway.launch.py +++ b/src/ros2_medkit_gateway/launch/gateway.launch.py @@ -53,7 +53,10 @@ def generate_launch_description(): declare_refresh_arg = DeclareLaunchArgument( 'refresh_interval_ms', default_value='2000', - description='Cache refresh interval in milliseconds') + description=( + 'Safety-backstop refresh interval in milliseconds. Primary ' + 'refresh is graph-event driven (~100 ms latency); this only ' + 'controls the periodic forced refresh.')) # Build parameter overrides - only inject plugin path if found param_overrides = { diff --git a/src/ros2_medkit_gateway/launch/gateway_https.launch.py b/src/ros2_medkit_gateway/launch/gateway_https.launch.py index f90e6a6d3..5d9051c9c 100644 --- a/src/ros2_medkit_gateway/launch/gateway_https.launch.py +++ b/src/ros2_medkit_gateway/launch/gateway_https.launch.py @@ -223,7 +223,11 @@ def generate_launch_description(): DeclareLaunchArgument( 'refresh_interval_ms', default_value='2000', - description='Cache refresh interval in milliseconds' + description=( + 'Safety-backstop refresh interval in milliseconds. Primary ' + 'refresh is graph-event driven; this only controls the ' + 'periodic forced refresh.' + ) ), DeclareLaunchArgument( diff --git a/src/ros2_medkit_gateway/src/core/managers/configuration_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/configuration_manager.cpp new file mode 100644 index 000000000..bf9f4dbc6 --- /dev/null +++ b/src/ros2_medkit_gateway/src/core/managers/configuration_manager.cpp @@ -0,0 +1,148 @@ +// 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/core/managers/configuration_manager.hpp" + +#include + +namespace ros2_medkit_gateway { + +ConfigurationManager::ConfigurationManager(std::shared_ptr transport) + : transport_(std::move(transport)) { +} + +ConfigurationManager::~ConfigurationManager() { + shutdown(); +} + +void ConfigurationManager::shutdown() { + if (shutdown_.exchange(true)) { + return; // Already shut down + } + if (transport_) { + transport_->shutdown(); + } +} + +ParameterResult ConfigurationManager::shut_down_result() { + return {false, {}, "ConfigurationManager is shut down", ParameterErrorCode::SHUT_DOWN}; +} + +ParameterResult ConfigurationManager::list_parameters(const std::string & node_name) { + if (shutdown_.load()) { + return shut_down_result(); + } + if (transport_->is_self_node(node_name)) { + return transport_->list_own_parameters(); + } + return transport_->list_parameters(node_name); +} + +ParameterResult ConfigurationManager::get_parameter(const std::string & node_name, const std::string & param_name) { + if (shutdown_.load()) { + return shut_down_result(); + } + if (transport_->is_self_node(node_name)) { + return transport_->get_own_parameter(param_name); + } + return transport_->get_parameter(node_name, param_name); +} + +ParameterResult ConfigurationManager::set_parameter(const std::string & node_name, const std::string & param_name, + const json & value) { + if (shutdown_.load()) { + return shut_down_result(); + } + return transport_->set_parameter(node_name, param_name, value); +} + +ParameterResult ConfigurationManager::reset_parameter(const std::string & node_name, const std::string & param_name) { + if (shutdown_.load()) { + return shut_down_result(); + } + + // Ask the transport for the cached default. Returns NO_DEFAULTS_CACHED / + // NOT_FOUND / SERVICE_UNAVAILABLE / TIMEOUT etc. as appropriate. + auto default_result = transport_->get_default(node_name, param_name); + if (!default_result.success) { + return default_result; + } + + // Apply the default by composing set_parameter. The transport echoes the + // committed descriptor in the result; we re-stamp the response so callers + // observe the same shape they did before the refactor. + const json default_value = default_result.data; + auto set_result = transport_->set_parameter(node_name, param_name, default_value); + if (!set_result.success) { + return set_result; + } + + if (set_result.data.is_object()) { + set_result.data["reset_to_default"] = true; + } + return set_result; +} + +ParameterResult ConfigurationManager::reset_all_parameters(const std::string & node_name) { + if (shutdown_.load()) { + return shut_down_result(); + } + + // Fetch all cached defaults for the node up front. Each entry is + // {"name", "value", "type"}; we fan out via set_parameter and aggregate. + auto defaults_result = transport_->list_defaults(node_name); + if (!defaults_result.success) { + return defaults_result; + } + + size_t reset_count = 0; + size_t failed_count = 0; + json failed_params = json::array(); + + if (defaults_result.data.is_array()) { + for (const auto & entry : defaults_result.data) { + if (!entry.is_object() || !entry.contains("name")) { + ++failed_count; + continue; + } + const std::string param_name = entry.at("name").get(); + const json param_value = entry.value("value", json{}); + auto set_result = transport_->set_parameter(node_name, param_name, param_value); + if (set_result.success) { + ++reset_count; + } else { + ++failed_count; + failed_params.push_back(param_name); + } + } + } + + ParameterResult result; + json response; + response["node_name"] = node_name; + response["reset_count"] = reset_count; + response["failed_count"] = failed_count; + if (!failed_params.empty()) { + response["failed_parameters"] = failed_params; + } + result.success = (failed_count == 0); + result.data = response; + if (failed_count > 0) { + result.error_message = "Some parameters could not be reset"; + result.error_code = ParameterErrorCode::INTERNAL_ERROR; + } + return result; +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/core/managers/data_access_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/data_access_manager.cpp new file mode 100644 index 000000000..da6b132b5 --- /dev/null +++ b/src/ros2_medkit_gateway/src/core/managers/data_access_manager.cpp @@ -0,0 +1,82 @@ +// 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/core/managers/data_access_manager.hpp" + +#include +#include + +namespace ros2_medkit_gateway { + +DataAccessManager::DataAccessManager(std::shared_ptr transport, double topic_sample_timeout_sec) + : transport_(std::move(transport)), topic_sample_timeout_sec_(topic_sample_timeout_sec) { + // Validate topic_sample_timeout_sec_ against the supported range [0.1, 30.0]. + // Out-of-range values fall back to a safe default rather than aborting; the + // value is operator-supplied so we prefer graceful degradation here. + if (topic_sample_timeout_sec_ < 0.1 || topic_sample_timeout_sec_ > 30.0) { + topic_sample_timeout_sec_ = 1.0; + } +} + +json DataAccessManager::publish_to_topic(const std::string & topic_path, const std::string & msg_type, + const json & data, double timeout_sec) { + return transport_->publish(topic_path, msg_type, data, std::chrono::duration(timeout_sec)); +} + +json DataAccessManager::get_topic_sample_with_fallback(const std::string & topic_name, double timeout_sec) { + // Use configured parameter when callers pass a sentinel negative timeout. + const double effective_timeout = (timeout_sec < 0.0) ? topic_sample_timeout_sec_ : timeout_sec; + return get_topic_sample_native(topic_name, effective_timeout); +} + +json DataAccessManager::get_topic_sample_native(const std::string & topic_name, double timeout_sec) { + // Fast path: when no publishers are present, return metadata-only without + // engaging the transport. This mirrors the legacy behaviour of returning + // a metadata frame for idle topics rather than throwing. + auto [pubs, subs] = transport_->count_publishers_subscribers(topic_name); + if (pubs == 0) { + json out; + out["topic"] = topic_name; + out["status"] = "metadata_only"; + out["publisher_count"] = pubs; + out["subscriber_count"] = subs; + return out; + } + + TopicSample sample = transport_->sample(topic_name, std::chrono::duration(timeout_sec)); + + json out; + out["topic"] = sample.topic.empty() ? topic_name : sample.topic; + out["status"] = sample.status; + out["publisher_count"] = sample.publisher_count; + out["subscriber_count"] = sample.subscriber_count; + if (!sample.type.empty()) { + out["type"] = sample.type; + if (!sample.type_info.empty() && !sample.type_info.is_null()) { + out["type_info"] = sample.type_info; + } + } + if (sample.status == "data" && !sample.data.is_null()) { + out["data"] = sample.data; + out["timestamp"] = + std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()) + .count(); + } + if (!sample.success && !sample.error_message.empty()) { + out["error"] = sample.error_message; + } + return out; +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/core/managers/fault_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/fault_manager.cpp new file mode 100644 index 000000000..68f22b20a --- /dev/null +++ b/src/ros2_medkit_gateway/src/core/managers/fault_manager.cpp @@ -0,0 +1,68 @@ +// 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/core/managers/fault_manager.hpp" + +#include + +namespace ros2_medkit_gateway { + +FaultManager::FaultManager(std::shared_ptr transport) : transport_(std::move(transport)) { +} + +FaultResult FaultManager::report_fault(const std::string & fault_code, uint8_t severity, + const std::string & description, const std::string & source_id) { + return transport_->report_fault(fault_code, severity, description, source_id); +} + +FaultResult FaultManager::list_faults(const std::string & source_id, bool include_prefailed, bool include_confirmed, + bool include_cleared, bool include_healed, bool include_muted, + bool include_clusters) { + return transport_->list_faults(source_id, include_prefailed, include_confirmed, include_cleared, include_healed, + include_muted, include_clusters); +} + +FaultWithEnvJsonResult FaultManager::get_fault_with_env(const std::string & fault_code, const std::string & source_id) { + return transport_->get_fault_with_env(fault_code, source_id); +} + +FaultResult FaultManager::get_fault(const std::string & fault_code, const std::string & source_id) { + return transport_->get_fault(fault_code, source_id); +} + +FaultResult FaultManager::clear_fault(const std::string & fault_code) { + return transport_->clear_fault(fault_code); +} + +FaultResult FaultManager::get_snapshots(const std::string & fault_code, const std::string & topic) { + return transport_->get_snapshots(fault_code, topic); +} + +FaultResult FaultManager::get_rosbag(const std::string & fault_code) { + return transport_->get_rosbag(fault_code); +} + +FaultResult FaultManager::list_rosbags(const std::string & entity_fqn) { + return transport_->list_rosbags(entity_fqn); +} + +bool FaultManager::is_available() const { + return transport_->is_available(); +} + +bool FaultManager::wait_for_services(std::chrono::duration timeout) { + return transport_->wait_for_services(timeout); +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/log_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/log_manager.cpp similarity index 80% rename from src/ros2_medkit_gateway/src/log_manager.cpp rename to src/ros2_medkit_gateway/src/core/managers/log_manager.cpp index 1df9dfcd3..571a9f565 100644 --- a/src/ros2_medkit_gateway/src/log_manager.cpp +++ b/src/ros2_medkit_gateway/src/core/managers/log_manager.cpp @@ -12,21 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ros2_medkit_gateway/log_manager.hpp" +#include "ros2_medkit_gateway/core/managers/log_manager.hpp" #include +#include #include #include +#include #include +#include -#include "ros2_medkit_gateway/core/plugins/plugin_manager.hpp" #include "ros2_medkit_gateway/core/resource_change_notifier.hpp" namespace ros2_medkit_gateway { namespace { -// Log level aliases matching rcl_interfaces::msg::Log constants -using Log = rcl_interfaces::msg::Log; + +// SOVD log severity levels mirror the rcl_interfaces::msg::Log uint8 constants +// (DEBUG=10, INFO=20, WARN=30, ERROR=40, FATAL=50). These are part of the +// neutral data contract carried in LogEntry::level - the source adapter +// performs the ROS-side mapping; the manager body operates on numeric levels +// only. +constexpr uint8_t kLevelDebug = 10; +constexpr uint8_t kLevelInfo = 20; +constexpr uint8_t kLevelWarn = 30; +constexpr uint8_t kLevelError = 40; +constexpr uint8_t kLevelFatal = 50; + } // namespace // --------------------------------------------------------------------------- @@ -35,15 +47,15 @@ using Log = rcl_interfaces::msg::Log; std::string LogManager::level_to_severity(uint8_t level) { switch (level) { - case Log::DEBUG: + case kLevelDebug: return "debug"; - case Log::INFO: + case kLevelInfo: return "info"; - case Log::WARN: + case kLevelWarn: return "warning"; - case Log::ERROR: + case kLevelError: return "error"; - case Log::FATAL: + case kLevelFatal: return "fatal"; default: return "debug"; @@ -52,19 +64,19 @@ std::string LogManager::level_to_severity(uint8_t level) { uint8_t LogManager::severity_to_level(const std::string & severity) { if (severity == "debug") { - return Log::DEBUG; + return kLevelDebug; } if (severity == "info") { - return Log::INFO; + return kLevelInfo; } if (severity == "warning") { - return Log::WARN; + return kLevelWarn; } if (severity == "error") { - return Log::ERROR; + return kLevelError; } if (severity == "fatal") { - return Log::FATAL; + return kLevelFatal; } return 0; } @@ -112,61 +124,61 @@ json LogManager::entry_to_json(const LogEntry & e) { } // --------------------------------------------------------------------------- -// Constructor +// Constructor / destructor // --------------------------------------------------------------------------- -LogManager::LogManager(rclcpp::Node * node, PluginManager * plugin_mgr, size_t max_buffer_size) - : node_(node), plugin_mgr_(plugin_mgr), max_buffer_size_(max_buffer_size) { +LogManager::LogManager(std::shared_ptr source, LogProviderRegistry * provider_registry, + size_t max_buffer_size) + : source_(std::move(source)), provider_registry_(provider_registry), max_buffer_size_(max_buffer_size) { auto * provider = effective_provider(); if (provider && provider->manages_ingestion()) { - RCLCPP_INFO(node_->get_logger(), - "LogManager: primary LogProvider manages ingestion - skipping /rosout subscription"); + // Primary LogProvider owns the entire pipeline - never start the source, + // so no subscription is created. return; } - rosout_sub_ = node_->create_subscription( - "/rosout", rclcpp::QoS(100), [this](const rcl_interfaces::msg::Log::ConstSharedPtr & msg) { - on_rosout(msg); - }); - - RCLCPP_INFO(node_->get_logger(), "LogManager: subscribed to /rosout (buffer_size=%zu)", max_buffer_size_); + if (source_) { + source_->start([this](const LogEntry & entry) { + on_log_entry(entry); + }); + } } LogManager::~LogManager() { - // Clear /rosout subscription before destruction to prevent callbacks on destroyed object - rosout_sub_.reset(); + // Stop the source before any of our members destruct so no callback can + // fire on a half-destroyed manager. The source's stop() is idempotent and + // also runs from its own destructor; calling it here gives a deterministic + // teardown order regardless of how the shared_ptr's last reference drops. + if (source_) { + source_->stop(); + } } // --------------------------------------------------------------------------- -// /rosout callback +// Source-emitted entry handler // --------------------------------------------------------------------------- -void LogManager::on_rosout(const rcl_interfaces::msg::Log::ConstSharedPtr & msg) { - LogEntry entry; +void LogManager::on_log_entry(const LogEntry & source_entry) { + // Stamp a monotonic id (the source emits entries without one; we are the + // single point of id assignment for both source-driven and programmatic + // injection paths). + LogEntry entry = source_entry; entry.id = next_id_.fetch_add(1, std::memory_order_relaxed); - entry.stamp_sec = msg->stamp.sec; - entry.stamp_nanosec = msg->stamp.nanosec; - entry.level = msg->level; - entry.name = msg->name; // already without leading slash per rcl convention - entry.msg = msg->msg; - entry.function = msg->function; - entry.file = msg->file; - entry.line = msg->line; - - // Notify all LogProvider observers — they may forward to OTel, DB, etc. + + // Notify all LogProvider observers - they may forward to OTel, DB, etc. // Exceptions from plugins are caught to prevent a misbehaving plugin from - // crashing the gateway's ROS 2 subscription callback. + // crashing the source's delivery callback. bool suppress_buffer = false; - if (plugin_mgr_) { - for (auto * observer : plugin_mgr_->get_log_observers()) { + if (provider_registry_) { + for (auto * observer : provider_registry_->log_observers()) { try { if (observer->on_log_entry(entry)) { suppress_buffer = true; } } catch (const std::exception & e) { - RCLCPP_WARN(node_->get_logger(), "LogProvider::on_log_entry threw: %s", e.what()); + std::cerr << "LogProvider::on_log_entry threw: " << e.what() << '\n'; } catch (...) { - RCLCPP_WARN(node_->get_logger(), "LogProvider::on_log_entry threw unknown exception"); + std::cerr << "LogProvider::on_log_entry threw unknown exception\n"; } } } @@ -186,7 +198,7 @@ void LogManager::on_rosout(const rcl_interfaces::msg::Log::ConstSharedPtr & msg) } } - // Notify triggers about the log event (resolve ROS node name to entity ID) + // Notify triggers about the log event (resolve logger name to entity ID) if (notifier_) { std::string entity_id; @@ -218,7 +230,7 @@ void LogManager::on_rosout(const rcl_interfaces::msg::Log::ConstSharedPtr & msg) void LogManager::inject_entry_for_testing(LogEntry entry) { std::lock_guard lock(buffers_mutex_); - // Respect buffer cap (same logic as on_rosout) for consistent test behavior + // Respect buffer cap (same logic as on_log_entry) for consistent test behavior if (buffers_.find(entry.name) == buffers_.end() && buffers_.size() >= max_buffer_size_ * 10) { return; // Silently drop logs from new nodes beyond the cap } @@ -237,7 +249,7 @@ void LogManager::add_log_entry(const std::string & entity_id, const std::string const nlohmann::json & metadata) { std::string normalized = normalize_fqn(entity_id); - // Build a LogEntry using the same structure as on_rosout() + // Build a LogEntry using the same structure as source-emitted entries. LogEntry entry; entry.id = next_id_.fetch_add(1, std::memory_order_relaxed); @@ -251,7 +263,7 @@ void LogManager::add_log_entry(const std::string & entity_id, const std::string entry.stamp_nanosec = static_cast(nsecs.count()); entry.level = severity_to_level(severity); if (entry.level == 0) { - entry.level = rcl_interfaces::msg::Log::INFO; // fallback for invalid severity + entry.level = kLevelInfo; // fallback for invalid severity } entry.name = normalized; entry.line = 0; @@ -263,7 +275,7 @@ void LogManager::add_log_entry(const std::string & entity_id, const std::string entry.msg = message; } - // Push to ring buffer (same path as on_rosout) + // Push to ring buffer (same path as source-emitted entries) { std::lock_guard lock(buffers_mutex_); auto & buf = buffers_[entry.name]; @@ -296,8 +308,8 @@ void LogManager::set_node_to_entity_resolver(NodeToEntityFn resolver) { // --------------------------------------------------------------------------- LogProvider * LogManager::effective_provider() const { - if (plugin_mgr_) { - return plugin_mgr_->get_log_provider(); + if (provider_registry_) { + return provider_registry_->primary_log_provider(); } return nullptr; } @@ -326,10 +338,10 @@ tl::expected LogManager::get_logs(const std::vectorget_logger(), "LogProvider::get_logs threw: %s", e.what()); + std::cerr << "LogProvider::get_logs threw: " << e.what() << '\n'; return tl::make_unexpected(std::string("LogProvider plugin error: ") + e.what()); } catch (...) { - RCLCPP_ERROR(node_->get_logger(), "LogProvider::get_logs threw unknown exception"); + std::cerr << "LogProvider::get_logs threw unknown exception\n"; return tl::make_unexpected(std::string("LogProvider plugin error: unknown exception")); } } @@ -419,10 +431,10 @@ tl::expected LogManager::get_config(const std::string & try { return provider->get_config(entity_id); } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "LogProvider::get_config threw: %s", e.what()); + std::cerr << "LogProvider::get_config threw: " << e.what() << '\n'; return tl::make_unexpected(std::string("LogProvider plugin error: ") + e.what()); } catch (...) { - RCLCPP_ERROR(node_->get_logger(), "LogProvider::get_config threw unknown exception"); + std::cerr << "LogProvider::get_config threw unknown exception\n"; return tl::make_unexpected(std::string("LogProvider plugin error: unknown exception")); } } @@ -449,10 +461,10 @@ std::string LogManager::update_config(const std::string & entity_id, const std:: try { return provider->update_config(entity_id, severity_filter, max_entries); } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "LogProvider::update_config threw: %s", e.what()); + std::cerr << "LogProvider::update_config threw: " << e.what() << '\n'; return std::string("LogProvider plugin error: ") + e.what(); } catch (...) { - RCLCPP_ERROR(node_->get_logger(), "LogProvider::update_config threw unknown exception"); + std::cerr << "LogProvider::update_config threw unknown exception\n"; return "LogProvider plugin error: unknown exception"; } } diff --git a/src/ros2_medkit_gateway/src/core/managers/operation_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/operation_manager.cpp new file mode 100644 index 000000000..1abbebd3d --- /dev/null +++ b/src/ros2_medkit_gateway/src/core/managers/operation_manager.cpp @@ -0,0 +1,491 @@ +// 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/core/managers/operation_manager.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/core/resource_change_notifier.hpp" + +namespace ros2_medkit_gateway { + +namespace { + +/// UUID hex string length (16 bytes = 32 hex characters). +constexpr size_t kUuidHexLength = 32; + +} // namespace + +OperationManager::OperationManager(std::shared_ptr service_transport, + std::shared_ptr action_transport, ServiceActionResolver * resolver, + int service_call_timeout_sec) + : service_transport_(std::move(service_transport)) + , action_transport_(std::move(action_transport)) + , resolver_(resolver) + , rng_(std::random_device{}()) + , service_call_timeout_sec_(service_call_timeout_sec) { +} + +OperationManager::~OperationManager() { + shutdown(); +} + +void OperationManager::shutdown() { + // Drop status subscriptions through the transport so the transport can + // tear down its underlying ROS state safely (idempotent on the transport + // side too). + std::map paths_to_drop; + { + std::lock_guard lock(subscriptions_mutex_); + paths_to_drop.swap(subscribed_paths_); + } + for (const auto & [path, _] : paths_to_drop) { + if (action_transport_) { + action_transport_->unsubscribe_status(path); + } + } + + // Clear tracked goals. + std::lock_guard lock(goals_mutex_); + tracked_goals_.clear(); +} + +void OperationManager::set_notifier(ResourceChangeNotifier * notifier) { + notifier_ = notifier; +} + +bool OperationManager::is_valid_message_type(const std::string & type) { + static const std::regex type_regex(R"(^[a-zA-Z_][a-zA-Z0-9_]*/(srv|action|msg)/[a-zA-Z_][a-zA-Z0-9_]*$)"); + return std::regex_match(type, type_regex); +} + +bool OperationManager::is_service_type(const std::string & type) { + return type.find("/srv/") != std::string::npos; +} + +bool OperationManager::is_action_type(const std::string & type) { + return type.find("/action/") != std::string::npos; +} + +bool OperationManager::is_valid_uuid_hex(const std::string & uuid_hex) { + if (uuid_hex.length() != kUuidHexLength) { + return false; + } + return std::all_of(uuid_hex.begin(), uuid_hex.end(), [](char c) { + return std::isxdigit(static_cast(c)); + }); +} + +ServiceCallResult OperationManager::call_service(const std::string & service_path, const std::string & service_type, + const json & request) { + if (!service_transport_) { + ServiceCallResult result; + result.success = false; + result.error_message = "ServiceTransport not configured"; + return result; + } + return service_transport_->call(service_path, service_type, request, + std::chrono::duration(service_call_timeout_sec_)); +} + +ServiceCallResult OperationManager::call_component_service(const std::string & component_ns, + const std::string & operation_name, + const std::optional & service_type, + const json & request) { + ServiceCallResult result; + std::string resolved_type; + std::string service_path; + + if (service_type.has_value() && !service_type->empty()) { + resolved_type = *service_type; + service_path = component_ns; + if (!service_path.empty() && service_path.back() != '/') { + service_path += "/"; + } + service_path += operation_name; + } else { + if (resolver_ == nullptr) { + result.success = false; + result.error_message = "Service not found: " + operation_name + " in namespace " + component_ns; + return result; + } + auto service_info = resolver_->find_service(component_ns, operation_name); + if (!service_info.has_value()) { + result.success = false; + result.error_message = "Service not found: " + operation_name + " in namespace " + component_ns; + return result; + } + resolved_type = service_info->type; + service_path = service_info->full_path; + } + + if (!is_valid_message_type(resolved_type)) { + result.success = false; + result.error_message = "Invalid service type format: " + resolved_type; + return result; + } + + if (!is_service_type(resolved_type)) { + result.success = false; + result.error_message = "Type is not a service type: " + resolved_type; + return result; + } + + return call_service(service_path, resolved_type, request); +} + +// ==================== ACTION OPERATIONS ==================== + +std::array OperationManager::generate_uuid() { + std::lock_guard lock(rng_mutex_); + std::array uuid; + std::uniform_int_distribution dist(0, 255); + for (auto & byte : uuid) { + byte = static_cast(dist(rng_)); + } + // Set version 4 (random UUID). + uuid[6] = (uuid[6] & 0x0f) | 0x40; + // Set variant bits. + uuid[8] = (uuid[8] & 0x3f) | 0x80; + return uuid; +} + +json OperationManager::uuid_bytes_to_json_array(const std::array & uuid) { + json array = json::array(); + for (auto byte : uuid) { + array.push_back(static_cast(byte)); + } + return array; +} + +json OperationManager::uuid_hex_to_json_array(const std::string & uuid_hex) { + json array = json::array(); + for (size_t i = 0; i + 1 < uuid_hex.length() && i < kUuidHexLength; i += 2) { + int byte_val = std::stoi(uuid_hex.substr(i, 2), nullptr, 16); + array.push_back(byte_val); + } + return array; +} + +std::string OperationManager::uuid_bytes_to_hex(const std::array & uuid) { + std::ostringstream ss; + ss << std::hex << std::setfill('0'); + for (const auto & byte : uuid) { + ss << std::setw(2) << static_cast(byte); + } + return ss.str(); +} + +void OperationManager::track_goal(const std::string & goal_id, const std::string & action_path, + const std::string & action_type, const std::string & entity_id) { + std::lock_guard lock(goals_mutex_); + ActionGoalInfo info; + info.goal_id = goal_id; + info.action_path = action_path; + info.action_type = action_type; + info.entity_id = entity_id; + info.status = ActionGoalStatus::ACCEPTED; + info.created_at = std::chrono::system_clock::now(); + info.last_update = info.created_at; + tracked_goals_[goal_id] = info; +} + +ActionSendGoalResult OperationManager::send_action_goal(const std::string & action_path, + const std::string & action_type, const json & goal, + const std::string & entity_id) { + ActionSendGoalResult result; + result.success = false; + result.goal_accepted = false; + + if (!action_transport_) { + result.error_message = "ActionTransport not configured"; + return result; + } + + result = action_transport_->send_goal(action_path, action_type, goal, + std::chrono::duration(service_call_timeout_sec_)); + + if (result.success && result.goal_accepted && !result.goal_id.empty()) { + track_goal(result.goal_id, action_path, action_type, entity_id); + subscribe_to_action_status(action_path); + update_goal_status(result.goal_id, ActionGoalStatus::EXECUTING); + } + + return result; +} + +ActionSendGoalResult OperationManager::send_component_action_goal(const std::string & component_ns, + const std::string & operation_name, + const std::optional & action_type, + const json & goal, const std::string & entity_id) { + ActionSendGoalResult result; + std::string resolved_type; + std::string action_path; + + if (action_type.has_value() && !action_type->empty()) { + resolved_type = *action_type; + action_path = component_ns; + if (!action_path.empty() && action_path.back() != '/') { + action_path += "/"; + } + action_path += operation_name; + } else { + if (resolver_ == nullptr) { + result.success = false; + result.error_message = "Action not found: " + operation_name + " in namespace " + component_ns; + return result; + } + auto action_info = resolver_->find_action(component_ns, operation_name); + if (!action_info.has_value()) { + result.success = false; + result.error_message = "Action not found: " + operation_name + " in namespace " + component_ns; + return result; + } + resolved_type = action_info->type; + action_path = action_info->full_path; + } + + if (!is_valid_message_type(resolved_type)) { + result.success = false; + result.error_message = "Invalid action type format: " + resolved_type; + return result; + } + + if (!is_action_type(resolved_type)) { + result.success = false; + result.error_message = "Type is not an action type: " + resolved_type; + return result; + } + + return send_action_goal(action_path, resolved_type, goal, entity_id); +} + +ActionCancelResult OperationManager::cancel_action_goal(const std::string & action_path, const std::string & goal_id) { + ActionCancelResult result; + result.success = false; + result.return_code = 0; + + if (!is_valid_uuid_hex(goal_id)) { + result.error_message = "Invalid goal_id format: must be 32 hex characters"; + return result; + } + + auto goal_info = get_tracked_goal(goal_id); + if (!goal_info) { + result.error_message = "Unknown goal_id - not tracked"; + return result; + } + + if (!action_transport_) { + result.error_message = "ActionTransport not configured"; + return result; + } + + result = action_transport_->cancel_goal(action_path, goal_id, std::chrono::duration(5.0)); + + if (result.success && result.return_code == 0) { + update_goal_status(goal_id, ActionGoalStatus::CANCELING); + } + return result; +} + +ActionGetResultResult OperationManager::get_action_result(const std::string & action_path, + const std::string & action_type, + const std::string & goal_id) { + ActionGetResultResult result; + result.success = false; + result.status = ActionGoalStatus::UNKNOWN; + + if (!is_valid_uuid_hex(goal_id)) { + result.error_message = "Invalid goal_id format: must be 32 hex characters"; + return result; + } + + if (!action_transport_) { + result.error_message = "ActionTransport not configured"; + return result; + } + + result = action_transport_->get_result(action_path, action_type, goal_id, + std::chrono::duration(service_call_timeout_sec_)); + + if (result.success) { + update_goal_status(goal_id, result.status); + } + return result; +} + +std::optional OperationManager::get_tracked_goal(const std::string & goal_id) const { + std::lock_guard lock(goals_mutex_); + auto it = tracked_goals_.find(goal_id); + if (it != tracked_goals_.end()) { + return it->second; + } + return std::nullopt; +} + +std::vector OperationManager::list_tracked_goals() const { + std::lock_guard lock(goals_mutex_); + std::vector goals; + goals.reserve(tracked_goals_.size()); + for (const auto & [id, info] : tracked_goals_) { + goals.push_back(info); + } + return goals; +} + +std::vector OperationManager::get_goals_for_action(const std::string & action_path) const { + std::vector goals; + { + std::lock_guard lock(goals_mutex_); + for (const auto & [id, info] : tracked_goals_) { + if (info.action_path == action_path) { + goals.push_back(info); + } + } + } + std::sort(goals.begin(), goals.end(), [](const ActionGoalInfo & a, const ActionGoalInfo & b) { + return a.created_at > b.created_at; + }); + return goals; +} + +std::optional OperationManager::get_latest_goal_for_action(const std::string & action_path) const { + auto goals = get_goals_for_action(action_path); + if (goals.empty()) { + return std::nullopt; + } + return goals.front(); +} + +void OperationManager::update_goal_status(const std::string & goal_id, ActionGoalStatus status) { + std::lock_guard lock(goals_mutex_); + auto it = tracked_goals_.find(goal_id); + if (it != tracked_goals_.end()) { + it->second.status = status; + it->second.last_update = std::chrono::system_clock::now(); + } +} + +void OperationManager::update_goal_feedback(const std::string & goal_id, const json & feedback) { + std::lock_guard lock(goals_mutex_); + auto it = tracked_goals_.find(goal_id); + if (it != tracked_goals_.end()) { + it->second.last_feedback = feedback; + it->second.last_update = std::chrono::system_clock::now(); + } +} + +void OperationManager::cleanup_old_goals(std::chrono::seconds max_age) { + std::set actions_to_check; + + { + std::lock_guard lock(goals_mutex_); + auto now = std::chrono::system_clock::now(); + + for (auto it = tracked_goals_.begin(); it != tracked_goals_.end();) { + if (it->second.status == ActionGoalStatus::SUCCEEDED || it->second.status == ActionGoalStatus::CANCELED || + it->second.status == ActionGoalStatus::ABORTED) { + auto age = std::chrono::duration_cast(now - it->second.last_update); + if (age > max_age) { + actions_to_check.insert(it->second.action_path); + it = tracked_goals_.erase(it); + continue; + } + } + ++it; + } + } + + for (const auto & action_path : actions_to_check) { + auto remaining_goals = get_goals_for_action(action_path); + if (remaining_goals.empty()) { + unsubscribe_from_action_status(action_path); + } + } +} + +void OperationManager::subscribe_to_action_status(const std::string & action_path) { + if (!action_transport_) { + return; + } + + { + std::lock_guard lock(subscriptions_mutex_); + if (subscribed_paths_.count(action_path) > 0) { + return; + } + subscribed_paths_[action_path] = true; + } + + action_transport_->subscribe_status( + action_path, [this](const std::string & path, const std::string & goal_id, ActionGoalStatus status) { + on_status_callback(path, goal_id, status); + }); +} + +void OperationManager::unsubscribe_from_action_status(const std::string & action_path) { + bool was_subscribed = false; + { + std::lock_guard lock(subscriptions_mutex_); + auto it = subscribed_paths_.find(action_path); + if (it != subscribed_paths_.end()) { + subscribed_paths_.erase(it); + was_subscribed = true; + } + } + if (was_subscribed && action_transport_) { + action_transport_->unsubscribe_status(action_path); + } +} + +void OperationManager::on_status_callback(const std::string & action_path, const std::string & goal_id, + ActionGoalStatus status) { + ActionGoalStatus previous = ActionGoalStatus::UNKNOWN; + std::string entity_id; + bool changed = false; + bool match = false; + { + std::lock_guard lock(goals_mutex_); + auto it = tracked_goals_.find(goal_id); + if (it == tracked_goals_.end() || it->second.action_path != action_path) { + return; + } + match = true; + previous = it->second.status; + if (previous != status) { + it->second.status = status; + it->second.last_update = std::chrono::system_clock::now(); + changed = true; + entity_id = it->second.entity_id; + } + } + + if (match && changed && notifier_ != nullptr) { + json goal_json; + goal_json["goal_id"] = goal_id; + goal_json["action_path"] = action_path; + goal_json["status"] = action_status_to_string(status); + notifier_->notify("operations", entity_id, goal_id, goal_json); + } +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/trigger_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/trigger_manager.cpp similarity index 87% rename from src/ros2_medkit_gateway/src/trigger_manager.cpp rename to src/ros2_medkit_gateway/src/core/managers/trigger_manager.cpp index a5ba6a781..65c24860d 100644 --- a/src/ros2_medkit_gateway/src/trigger_manager.cpp +++ b/src/ros2_medkit_gateway/src/core/managers/trigger_manager.cpp @@ -20,8 +20,7 @@ #include #include -#include "ros2_medkit_gateway/log_manager.hpp" -#include "ros2_medkit_gateway/trigger_topic_subscriber.hpp" +#include "ros2_medkit_gateway/core/managers/log_manager.hpp" namespace ros2_medkit_gateway { @@ -30,8 +29,13 @@ namespace ros2_medkit_gateway { // --------------------------------------------------------------------------- TriggerManager::TriggerManager(ResourceChangeNotifier & notifier, ConditionRegistry & conditions, TriggerStore & store, - const TriggerConfig & config) - : notifier_(notifier), conditions_(conditions), store_(store), config_(config) { + const TriggerConfig & config, + std::shared_ptr topic_transport) + : notifier_(notifier) + , conditions_(conditions) + , store_(store) + , config_(config) + , topic_transport_(std::move(topic_transport)) { // Subscribe to all resource changes (empty filter = catch-all) NotifierFilter filter; notifier_sub_id_ = notifier_.subscribe(filter, [this](const ResourceChange & change) { @@ -55,6 +59,9 @@ void TriggerManager::shutdown() { state->active.store(false); state->cv.notify_all(); } + // Drop every per-trigger subscription handle so the underlying transport + // tears down its rclcpp resources before the manager goes away. + topic_handles_.clear(); } // --------------------------------------------------------------------------- @@ -106,10 +113,6 @@ void TriggerManager::set_entity_children_fn(EntityChildrenFn fn) { entity_children_fn_ = std::move(fn); } -void TriggerManager::set_topic_subscriber(TriggerTopicSubscriber * subscriber) { - topic_subscriber_ = subscriber; -} - void TriggerManager::set_log_manager(LogManager * log_manager) { log_manager_ = log_manager; } @@ -126,7 +129,7 @@ void TriggerManager::retry_unresolved_triggers() { std::lock_guard lock(triggers_mutex_); - if (unresolved_data_triggers_.empty() || !resolve_topic_fn_ || !topic_subscriber_) { + if (unresolved_data_triggers_.empty() || !resolve_topic_fn_ || !topic_transport_) { return; } @@ -144,13 +147,24 @@ void TriggerManager::retry_unresolved_triggers() { std::string topic_name = resolve_topic_fn_(entry.entity_id, entry.resource_path); if (!topic_name.empty()) { - // Update the trigger's resolved_topic_name + // Update the trigger's resolved_topic_name + register a transport + // handle whose lifetime is tied to the trigger entry (cleared on + // remove()/cleanup_expired_trigger()). auto it = triggers_.find(entry.trigger_id); if (it != triggers_.end()) { std::lock_guard state_lock(it->second->mtx); it->second->info.resolved_topic_name = topic_name; } - topic_subscriber_->subscribe(topic_name, entry.resource_path, entry.entity_id); + const std::string trigger_id = entry.trigger_id; + const std::string entity_id = entry.entity_id; + const std::string resource_path = entry.resource_path; + auto handle = topic_transport_->subscribe( + topic_name, /*msg_type=*/"", [this, entity_id, resource_path](const nlohmann::json & sample) { + notifier_.notify("data", entity_id, resource_path, sample, ChangeType::UPDATED); + }); + if (handle) { + topic_handles_[trigger_id] = std::move(handle); + } resolved_indices.push_back(i); } } @@ -312,9 +326,18 @@ tl::expected TriggerManager::create(const Trigg // Subscribe to topic for data triggers. // When resolved_topic_name is empty (topic not yet discoverable at creation time), // queue for deferred resolution - retry_unresolved_triggers() will pick it up. - if (topic_subscriber_ && req.collection == "data") { + if (topic_transport_ && req.collection == "data") { if (!req.resolved_topic_name.empty()) { - topic_subscriber_->subscribe(req.resolved_topic_name, req.resource_path, req.entity_id); + const std::string entity_id = req.entity_id; + const std::string resource_path = req.resource_path; + auto handle = topic_transport_->subscribe( + req.resolved_topic_name, /*msg_type=*/"", [this, entity_id, resource_path](const nlohmann::json & sample) { + notifier_.notify("data", entity_id, resource_path, sample, ChangeType::UPDATED); + }); + if (handle) { + std::lock_guard lock(triggers_mutex_); + topic_handles_[info_copy.id] = std::move(handle); + } } else if (!req.resource_path.empty()) { std::lock_guard lock(triggers_mutex_); unresolved_data_triggers_.push_back( @@ -387,9 +410,12 @@ bool TriggerManager::remove(const std::string & trigger_id) { std::shared_ptr state; std::string collection; std::string entity_id; - std::string resolved_topic_name; bool persistent = false; OnRemovedCallback on_removed_copy; + // Capture the per-trigger transport handle here so its destructor + // (which unsubscribes from the underlying topic) runs after we drop + // triggers_mutex_, keeping the lock window short. + std::unique_ptr released_handle; { std::lock_guard lock(triggers_mutex_); auto it = triggers_.find(trigger_id); @@ -401,11 +427,15 @@ bool TriggerManager::remove(const std::string & trigger_id) { std::lock_guard sub_lock(state->mtx); collection = state->info.collection; entity_id = state->info.entity_id; - resolved_topic_name = state->info.resolved_topic_name; persistent = state->info.persistent; } remove_from_dispatch_index(trigger_id, collection, entity_id); triggers_.erase(it); + auto h_it = topic_handles_.find(trigger_id); + if (h_it != topic_handles_.end()) { + released_handle = std::move(h_it->second); + topic_handles_.erase(h_it); + } on_removed_copy = on_removed_; // Copy under lock to avoid data race } @@ -413,10 +443,8 @@ bool TriggerManager::remove(const std::string & trigger_id) { state->active.store(false); state->cv.notify_all(); - // Unsubscribe from topic for data triggers - if (topic_subscriber_ && collection == "data" && !resolved_topic_name.empty()) { - topic_subscriber_->unsubscribe(resolved_topic_name, entity_id); - } + // released_handle goes out of scope here, unsubscribing the topic for + // data triggers (no-op for non-data triggers, which never registered one). // Remove from persistent store if applicable (outside triggers_mutex_) if (persistent) { @@ -466,22 +494,15 @@ void TriggerManager::cleanup_expired_trigger(const std::string & trigger_id, bool persistent = false; std::string collection; std::string entity_id; - std::string resolved_topic_name; { std::lock_guard sub_lock(state->mtx); state->info.status = TriggerStatus::TERMINATED; persistent = state->info.persistent; collection = state->info.collection; entity_id = state->info.entity_id; - resolved_topic_name = state->info.resolved_topic_name; } state->cv.notify_all(); - // Unsubscribe from topic for expired data triggers - if (topic_subscriber_ && collection == "data" && !resolved_topic_name.empty()) { - topic_subscriber_->unsubscribe(resolved_topic_name, entity_id); - } - // Persist status change (outside all locks) if (persistent) { nlohmann::json fields; @@ -489,15 +510,26 @@ void TriggerManager::cleanup_expired_trigger(const std::string & trigger_id, (void)store_.update(trigger_id, fields); } - // Remove from dispatch index and triggers map; capture callback under lock + // Remove from dispatch index, triggers map, and topic-handle map; the + // released handle's dtor unsubscribes the underlying topic. The unique_ptr + // is moved out from under the lock so the unsubscribe runs without holding + // triggers_mutex_, mirroring the remove() ordering. OnRemovedCallback on_removed_copy; + std::unique_ptr released_handle; { std::lock_guard lock(triggers_mutex_); remove_from_dispatch_index(trigger_id, collection, entity_id); triggers_.erase(trigger_id); + auto h_it = topic_handles_.find(trigger_id); + if (h_it != topic_handles_.end()) { + released_handle = std::move(h_it->second); + topic_handles_.erase(h_it); + } on_removed_copy = on_removed_; } + // released_handle drops here -> transport unsubscribe. + // Fire on_removed callback (I8 fix) - consistent with remove() if (on_removed_copy) { on_removed_copy(trigger_id); @@ -620,9 +652,19 @@ void TriggerManager::load_persistent_triggers() { add_to_dispatch_index(state->info.id, state->info.collection, state->info.entity_id); - // Re-subscribe to topic for restored data triggers - if (topic_subscriber_ && state->info.collection == "data" && !state->info.resolved_topic_name.empty()) { - topic_subscriber_->subscribe(state->info.resolved_topic_name, state->info.resource_path, state->info.entity_id); + // Re-subscribe to topic for restored data triggers via the transport. + if (topic_transport_ && state->info.collection == "data" && !state->info.resolved_topic_name.empty()) { + const std::string trigger_id = state->info.id; + const std::string entity_id = state->info.entity_id; + const std::string resource_path = state->info.resource_path; + auto handle = + topic_transport_->subscribe(state->info.resolved_topic_name, /*msg_type=*/"", + [this, entity_id, resource_path](const nlohmann::json & sample) { + notifier_.notify("data", entity_id, resource_path, sample, ChangeType::UPDATED); + }); + if (handle) { + topic_handles_[trigger_id] = std::move(handle); + } } triggers_[state->info.id] = std::move(state); diff --git a/src/ros2_medkit_gateway/src/core/operations/operation_types.cpp b/src/ros2_medkit_gateway/src/core/operations/operation_types.cpp new file mode 100644 index 000000000..d1022b781 --- /dev/null +++ b/src/ros2_medkit_gateway/src/core/operations/operation_types.cpp @@ -0,0 +1,39 @@ +// 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/core/operations/operation_types.hpp" + +namespace ros2_medkit_gateway { + +std::string action_status_to_string(ActionGoalStatus status) { + switch (status) { + case ActionGoalStatus::ACCEPTED: + return "accepted"; + case ActionGoalStatus::EXECUTING: + return "executing"; + case ActionGoalStatus::CANCELING: + return "canceling"; + case ActionGoalStatus::SUCCEEDED: + return "succeeded"; + case ActionGoalStatus::CANCELED: + return "canceled"; + case ActionGoalStatus::ABORTED: + return "aborted"; + case ActionGoalStatus::UNKNOWN: + default: + return "unknown"; + } +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/data_access_manager.cpp b/src/ros2_medkit_gateway/src/data_access_manager.cpp deleted file mode 100644 index 90519f93f..000000000 --- a/src/ros2_medkit_gateway/src/data_access_manager.cpp +++ /dev/null @@ -1,210 +0,0 @@ -// Copyright 2025-2026 mfaferek93, 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/data_access_manager.hpp" - -#include -#include -#include -#include -#include - -#include "ros2_medkit_gateway/core/data/topic_data_provider.hpp" -#include "ros2_medkit_gateway/core/exceptions.hpp" -#include "ros2_medkit_serialization/serialization_error.hpp" - -namespace ros2_medkit_gateway { - -DataAccessManager::DataAccessManager(rclcpp::Node * node) - : node_(node) - , serializer_(std::make_shared()) - , type_introspection_( - std::make_unique(ament_index_cpp::get_package_share_directory("ros2_medkit_gateway") + "/scr" - "ipt" - "s")) - , topic_sample_timeout_sec_(node->declare_parameter("topic_sample_timeout_sec", 1.0)) { - // Validate topic_sample_timeout_sec_ against allowed range [0.1, 30.0] - if (topic_sample_timeout_sec_ < 0.1 || topic_sample_timeout_sec_ > 30.0) { - RCLCPP_WARN(node_->get_logger(), - "topic_sample_timeout_sec (%.2f) out of valid range (0.1-30.0), using default: 1.0", - topic_sample_timeout_sec_); - topic_sample_timeout_sec_ = 1.0; - } - - RCLCPP_INFO(node_->get_logger(), - "DataAccessManager initialized (native_sampling=enabled, native_publishing=enabled, " - "topic_sample_timeout=%.2fs). Parallel-sample concurrency tuned via " - "data_provider.max_parallel_samples on the TopicDataProvider.", - topic_sample_timeout_sec_); -} - -rclcpp::GenericPublisher::SharedPtr DataAccessManager::get_or_create_publisher(const std::string & topic_path, - const std::string & msg_type) { - const std::string key = topic_path + "|" + msg_type; - - // Try read lock first (fast path) - { - std::shared_lock lock(publishers_mutex_); - auto it = publishers_.find(key); - if (it != publishers_.end()) { - return it->second; - } - } - - // Need to create - take exclusive lock - std::unique_lock lock(publishers_mutex_); - - // Double-check (another thread might have created it) - auto it = publishers_.find(key); - if (it != publishers_.end()) { - return it->second; - } - - // Create new publisher with default QoS (reliable, keep last 10) - auto publisher = node_->create_generic_publisher(topic_path, msg_type, rclcpp::QoS(10)); - publishers_[key] = publisher; - - RCLCPP_DEBUG(node_->get_logger(), "Created generic publisher for %s (%s)", topic_path.c_str(), msg_type.c_str()); - - return publisher; -} - -json DataAccessManager::publish_to_topic(const std::string & topic_path, const std::string & msg_type, - const json & data, double /* timeout_sec */) { - try { - // Step 1: Get or create cached publisher - auto publisher = get_or_create_publisher(topic_path, msg_type); - - // Step 2: Serialize JSON to CDR format - rclcpp::SerializedMessage serialized_msg = serializer_->serialize(msg_type, data); - - // Step 3: Publish serialized message - publisher->publish(serialized_msg); - - RCLCPP_INFO(node_->get_logger(), "Published to topic '%s' with type '%s' (native)", topic_path.c_str(), - msg_type.c_str()); - - json result = {{"topic", topic_path}, - {"type", msg_type}, - {"status", "published"}, - {"timestamp", std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - .count()}}; - - return result; - - } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { - RCLCPP_ERROR(node_->get_logger(), "Unknown message type '%s': %s", msg_type.c_str(), e.what()); - throw std::runtime_error("Unknown message type '" + msg_type + "': " + e.what()); - - } catch (const ros2_medkit_serialization::SerializationError & e) { - RCLCPP_ERROR(node_->get_logger(), "Failed to serialize message for '%s': %s", topic_path.c_str(), e.what()); - throw std::runtime_error("Failed to serialize message for '" + topic_path + "': " + e.what()); - - } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Failed to publish to topic '%s': %s", topic_path.c_str(), e.what()); - throw std::runtime_error("Failed to publish to topic '" + topic_path + "': " + e.what()); - } -} - -json DataAccessManager::get_topic_sample_with_fallback(const std::string & topic_name, double timeout_sec) { - // Use configured parameter if timeout_sec is negative (default) - double effective_timeout = (timeout_sec < 0) ? topic_sample_timeout_sec_ : timeout_sec; - // Always use native sampling - much faster for idle topics - return get_topic_sample_native(topic_name, effective_timeout); -} - -json DataAccessManager::sample_result_to_json(const TopicSampleResult & sample) { - json result; - result["topic"] = sample.topic_name; - result["timestamp"] = sample.timestamp_ns; - result["publisher_count"] = sample.publisher_count; - result["subscriber_count"] = sample.subscriber_count; - - if (sample.has_data && sample.data) { - result["status"] = "data"; - result["data"] = *sample.data; - } else { - result["status"] = "metadata_only"; - } - - // Add endpoint information with QoS - json publishers_json = json::array(); - for (const auto & pub : sample.publishers) { - publishers_json.push_back(pub.to_json()); - } - result["publishers"] = publishers_json; - - json subscribers_json = json::array(); - for (const auto & sub : sample.subscribers) { - subscribers_json.push_back(sub.to_json()); - } - result["subscribers"] = subscribers_json; - - // Enrich with message type - if (!sample.message_type.empty()) { - result["type"] = sample.message_type; - - // Try to add schema/default value info - try { - TopicTypeInfo type_info = type_introspection_->get_type_info(sample.message_type); - result["type_info"] = {{"schema", type_info.schema}, {"default_value", type_info.default_value}}; - } catch (const std::exception & e) { - RCLCPP_DEBUG(node_->get_logger(), "Could not get type info for '%s': %s", sample.message_type.c_str(), e.what()); - } - } - - return result; -} - -json DataAccessManager::get_topic_sample_native(const std::string & topic_name, double timeout_sec) { - RCLCPP_DEBUG(node_->get_logger(), "get_topic_sample_native: topic='%s', timeout=%.2f", topic_name.c_str(), - timeout_sec); - - if (!topic_data_provider_) { - RCLCPP_ERROR(node_->get_logger(), "TopicDataProvider not configured - sampling '%s' is unavailable", - topic_name.c_str()); - throw TopicNotAvailableException(topic_name); - } - const auto timeout_ms = std::chrono::milliseconds{static_cast(std::max(timeout_sec, 0.0) * 1000.0)}; - auto r = topic_data_provider_->sample(topic_name, timeout_ms); - if (!r) { - const auto & err = r.error(); - RCLCPP_WARN(node_->get_logger(), "TopicDataProvider::sample('%s') failed: %s [%d]", topic_name.c_str(), - err.message.c_str(), err.http_status); - // Preserve http_status: only 404-class errors collapse to "topic not - // available"; every other status (503 shutdown, 500 subscribe-failed, - // 503 cold-wait-cap-exceeded) propagates through ProviderErrorException - // so the handler can emit the original status code. Collapsing to 404 - // silently masks server-side failures from clients with retry logic. - if (err.http_status == 404) { - throw TopicNotAvailableException(topic_name); - } - throw ProviderErrorException(err); - } - TopicSampleResult sample = *r; - - RCLCPP_DEBUG(node_->get_logger(), "get_topic_sample_native: sample returned, has_data=%d, type='%s'", sample.has_data, - sample.message_type.c_str()); - - if (!sample.message_type.empty() || sample.has_data) { - return sample_result_to_json(sample); - } - - // Topic not found at all - RCLCPP_DEBUG(node_->get_logger(), "get_topic_sample_native: topic not available '%s'", topic_name.c_str()); - throw TopicNotAvailableException(topic_name); -} - -} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp b/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp index c6cf21e84..63de692c9 100644 --- a/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp +++ b/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp @@ -16,17 +16,15 @@ #include "ros2_medkit_gateway/core/discovery/layers/manifest_layer.hpp" #include "ros2_medkit_gateway/core/discovery/layers/runtime_layer.hpp" -#include "ros2_medkit_gateway/discovery/hybrid_discovery.hpp" #include "ros2_medkit_gateway/discovery/layers/plugin_layer.hpp" #include "ros2_medkit_gateway/discovery/manifest/runtime_linker.hpp" -#include "ros2_medkit_gateway/discovery/merge_pipeline.hpp" + +#include namespace ros2_medkit_gateway { DiscoveryManager::DiscoveryManager(rclcpp::Node * node) - : node_(node), runtime_strategy_(std::make_unique(node)) { - // Default to runtime strategy - active_strategy_ = runtime_strategy_.get(); + : node_(node), runtime_introspection_(std::make_unique(node)) { } bool DiscoveryManager::initialize(const DiscoveryConfig & config) { @@ -59,7 +57,7 @@ bool DiscoveryManager::initialize(const DiscoveryConfig & config) { "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(); + build_pipeline(); return true; } RCLCPP_ERROR(node_->get_logger(), "Manifest path required for %s mode. Set discovery.manifest_path.", @@ -74,7 +72,7 @@ bool DiscoveryManager::initialize(const DiscoveryConfig & config) { "This fallback is deprecated and will be removed in a future release."); config_.mode = DiscoveryMode::RUNTIME_ONLY; manifest_manager_.reset(); - create_strategy(); + build_pipeline(); return true; } RCLCPP_ERROR(node_->get_logger(), "Manifest load failed in %s mode. Cannot proceed.", @@ -83,7 +81,7 @@ bool DiscoveryManager::initialize(const DiscoveryConfig & config) { } } - create_strategy(); + build_pipeline(); return true; } @@ -107,40 +105,38 @@ void DiscoveryManager::apply_layer_policy_overrides(const std::string & layer_na } } -void DiscoveryManager::create_strategy() { - // Configure runtime strategy with runtime options - discovery::RuntimeDiscoveryStrategy::RuntimeConfig runtime_config; +void DiscoveryManager::build_pipeline() { + // Configure runtime introspection (always-on adapter, used by all modes for + // service/action queries even when the merge pipeline is bypassed). + ros2::Ros2RuntimeIntrospection::RuntimeConfig runtime_config; runtime_config.create_functions_from_namespaces = config_.runtime.create_functions_from_namespaces; - runtime_strategy_->set_config(runtime_config); + runtime_introspection_->set_config(runtime_config); switch (config_.mode) { case DiscoveryMode::MANIFEST_ONLY: - // In MANIFEST_ONLY mode, entity structure comes from manifest (via manifest_manager_), - // NOT from the active_strategy_. The discover_*() methods check the mode and return - // manifest data directly. We still point active_strategy_ to runtime_strategy_ because: - // 1. It's used for service/action introspection when explicitly requested - // 2. It avoids creating a separate ManifestOnlyStrategy class - // 3. It provides a fallback if manifest is unavailable - active_strategy_ = runtime_strategy_.get(); + // Manifest-only: entity structure comes from manifest_manager_; the + // pipeline is not used. Service/action queries still go through + // runtime_introspection_ directly. RCLCPP_INFO(node_->get_logger(), "Discovery mode: manifest_only"); + pipeline_.reset(); break; case DiscoveryMode::HYBRID: { - discovery::MergePipeline pipeline(node_->get_logger()); + auto pipeline = std::make_unique(node_->get_logger()); if (config_.manifest_enabled) { auto manifest_layer = std::make_unique(manifest_manager_.get()); apply_layer_policy_overrides("manifest", *manifest_layer); - pipeline.add_layer(std::move(manifest_layer)); + pipeline->add_layer(std::move(manifest_layer)); } else { RCLCPP_INFO(node_->get_logger(), "Manifest layer disabled in hybrid mode"); } if (config_.runtime_enabled) { - auto runtime_layer = std::make_unique(runtime_strategy_.get()); + auto runtime_layer = std::make_unique(runtime_introspection_.get()); runtime_layer->set_gap_fill_config(config_.merge_pipeline.gap_fill); apply_layer_policy_overrides("runtime", *runtime_layer); - pipeline.add_layer(std::move(runtime_layer)); + pipeline->add_layer(std::move(runtime_layer)); } else { RCLCPP_INFO(node_->get_logger(), "Runtime layer disabled in hybrid mode"); } @@ -152,14 +148,14 @@ void DiscoveryManager::create_strategy() { "Entity discovery relies entirely on plugins."); } - // RuntimeLinker only makes sense when runtime is enabled + // RuntimeLinker only makes sense when runtime is enabled. if (config_.runtime_enabled) { auto manifest_config = manifest_manager_ ? manifest_manager_->get_config() : discovery::ManifestConfig{}; - pipeline.set_linker(std::make_unique(node_), manifest_config); + pipeline->set_linker(std::make_unique(node_), manifest_config); } - hybrid_strategy_ = std::make_unique(node_, std::move(pipeline)); - active_strategy_ = hybrid_strategy_.get(); + pipeline_ = std::move(pipeline); + run_pipeline(); RCLCPP_INFO(node_->get_logger(), "Discovery mode: hybrid (manifest=%s, runtime=%s)", config_.manifest_enabled ? "on" : "off", config_.runtime_enabled ? "on" : "off"); break; @@ -167,18 +163,38 @@ void DiscoveryManager::create_strategy() { case DiscoveryMode::RUNTIME_ONLY: default: - active_strategy_ = runtime_strategy_.get(); RCLCPP_INFO(node_->get_logger(), "Discovery mode: runtime_only (default_component=%s)", host_info_provider_ ? "true" : "false"); + pipeline_.reset(); break; } } +void DiscoveryManager::run_pipeline() { + if (!pipeline_) { + return; + } + // Execute pipeline WITHOUT lock - safe because the layers' add_layer() calls + // happen during initialization (build_pipeline + plugin setup) before the + // EntityCache timer starts, and run_pipeline() runs only on the gateway's + // single-threaded refresh path. + auto new_result = pipeline_->execute(); + + std::lock_guard lock(result_mutex_); + cached_result_ = std::move(new_result); + RCLCPP_INFO(node_->get_logger(), "Hybrid discovery refreshed: %zu entities", cached_result_.report.total_entities); +} + std::vector DiscoveryManager::discover_areas() { if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_areas(); } - return active_strategy_->discover_areas(); + if (config_.mode == DiscoveryMode::HYBRID) { + std::lock_guard lock(result_mutex_); + return cached_result_.areas; + } + // RUNTIME_ONLY: runtime introspection never produces Areas. + return {}; } std::vector DiscoveryManager::discover_components() { @@ -192,33 +208,46 @@ std::vector DiscoveryManager::discover_components() { return {host_info_provider_->get_default_component()}; } - return active_strategy_->discover_components(); + if (config_.mode == DiscoveryMode::HYBRID) { + std::lock_guard lock(result_mutex_); + return cached_result_.components; + } + + return {}; } std::vector DiscoveryManager::discover_apps() { if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_apps(); } - return active_strategy_->discover_apps(); + if (config_.mode == DiscoveryMode::HYBRID) { + std::lock_guard lock(result_mutex_); + return cached_result_.apps; + } + return runtime_introspection_->discover_apps(); } std::vector DiscoveryManager::discover_functions() { if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_functions(); } - return active_strategy_->discover_functions(); + if (config_.mode == DiscoveryMode::HYBRID) { + std::lock_guard lock(result_mutex_); + return cached_result_.functions; + } + return runtime_introspection_->discover_functions(); } std::vector DiscoveryManager::discover_functions(const std::vector & apps) { if (config_.mode == DiscoveryMode::MANIFEST_ONLY && manifest_manager_ && manifest_manager_->is_manifest_active()) { return manifest_manager_->get_functions(); } - // In RUNTIME_ONLY mode, delegate to the overload that accepts pre-discovered apps - if (config_.mode == DiscoveryMode::RUNTIME_ONLY && runtime_strategy_) { - return runtime_strategy_->discover_functions(apps); + if (config_.mode == DiscoveryMode::HYBRID) { + std::lock_guard lock(result_mutex_); + return cached_result_.functions; } - // HYBRID mode uses cached pipeline results, so apps parameter is not needed - return active_strategy_->discover_functions(); + // RUNTIME_ONLY: avoid redundant graph queries by reusing the apps vector. + return runtime_introspection_->discover_functions(apps); } std::optional DiscoveryManager::get_area(const std::string & id) { @@ -344,54 +373,64 @@ std::vector DiscoveryManager::get_hosts_for_function(const std::str } std::vector DiscoveryManager::discover_services() { - return runtime_strategy_->discover_services(); + return runtime_introspection_->discover_services(); } std::vector DiscoveryManager::discover_actions() { - return runtime_strategy_->discover_actions(); + return runtime_introspection_->discover_actions(); } std::optional DiscoveryManager::find_service(const std::string & component_ns, const std::string & operation_name) const { - return runtime_strategy_->find_service(component_ns, operation_name); + return runtime_introspection_->find_service(component_ns, operation_name); } std::optional DiscoveryManager::find_action(const std::string & component_ns, const std::string & operation_name) const { - return runtime_strategy_->find_action(component_ns, operation_name); + return runtime_introspection_->find_action(component_ns, operation_name); } void DiscoveryManager::set_topic_data_provider(TopicDataProvider * provider) { - runtime_strategy_->set_topic_data_provider(provider); + runtime_introspection_->set_topic_data_provider(provider); } -void DiscoveryManager::set_type_introspection(TypeIntrospection * introspection) { - runtime_strategy_->set_type_introspection(introspection); +void DiscoveryManager::set_type_introspection(ros2_medkit_serialization::TypeIntrospection * introspection) { + runtime_introspection_->set_type_introspection(introspection); } void DiscoveryManager::refresh_topic_map() { - runtime_strategy_->refresh_topic_map(); - if (hybrid_strategy_) { - hybrid_strategy_->refresh(); + runtime_introspection_->refresh_topic_map(); + if (pipeline_) { + run_pipeline(); } } bool DiscoveryManager::is_topic_map_ready() const { - return runtime_strategy_->is_topic_map_ready(); + return runtime_introspection_->is_topic_map_ready(); } void DiscoveryManager::add_plugin_layer(const std::string & plugin_name, IntrospectionProvider * provider) { - if (!hybrid_strategy_) { + if (!pipeline_) { 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)); + pipeline_->add_layer(std::make_unique(plugin_name, provider)); RCLCPP_INFO(node_->get_logger(), "Added plugin layer '%s' to merge pipeline", plugin_name.c_str()); } +void DiscoveryManager::register_introspection_provider(const std::string & name, + std::shared_ptr provider) { + if (!provider) { + return; + } + IntrospectionProvider * raw = provider.get(); + owned_providers_.push_back(std::move(provider)); + add_plugin_layer(name, raw); +} + void DiscoveryManager::refresh_pipeline() { - if (hybrid_strategy_) { - hybrid_strategy_->refresh(); + if (pipeline_) { + run_pipeline(); } } @@ -400,24 +439,31 @@ discovery::ManifestManager * DiscoveryManager::get_manifest_manager() { } std::string DiscoveryManager::get_strategy_name() const { - if (active_strategy_) { - return active_strategy_->get_name(); + switch (config_.mode) { + case DiscoveryMode::MANIFEST_ONLY: + return "manifest"; + case DiscoveryMode::HYBRID: + return "hybrid"; + case DiscoveryMode::RUNTIME_ONLY: + default: + return "runtime"; } - return "unknown"; } std::optional DiscoveryManager::get_merge_report() const { - if (hybrid_strategy_) { - return hybrid_strategy_->get_merge_report(); + if (!pipeline_) { + return std::nullopt; } - return std::nullopt; + std::lock_guard lock(result_mutex_); + return cached_result_.report; } std::optional DiscoveryManager::get_linking_result() const { - if (hybrid_strategy_) { - return hybrid_strategy_->get_linking_result(); + if (!pipeline_) { + return std::nullopt; } - return std::nullopt; + std::lock_guard lock(result_mutex_); + return cached_result_.linking_result; } bool DiscoveryManager::has_host_info_provider() const { diff --git a/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp b/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp deleted file mode 100644 index 464651d1c..000000000 --- a/src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// 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/hybrid_discovery.hpp" - -#include - -namespace ros2_medkit_gateway { -namespace discovery { - -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_); - return cached_result_.areas; -} - -std::vector HybridDiscoveryStrategy::discover_components() { - std::lock_guard lock(mutex_); - return cached_result_.components; -} - -std::vector HybridDiscoveryStrategy::discover_apps() { - std::lock_guard lock(mutex_); - return cached_result_.apps; -} - -std::vector HybridDiscoveryStrategy::discover_functions() { - std::lock_guard lock(mutex_); - return cached_result_.functions; -} - -void HybridDiscoveryStrategy::refresh() { - // Execute pipeline WITHOUT lock - safe because: - // 1. add_layer() is called during initialization (from DiscoveryManager::initialize() - // and gateway_node plugin setup) before the EntityCache timer starts - // 2. refresh() is only called from the ROS executor timer callback (single-threaded) - // - concurrent refresh() calls cannot occur under the default executor model - auto new_result = pipeline_.execute(); - - // Swap result under lock to protect concurrent discover_*() readers - std::lock_guard lock(mutex_); - cached_result_ = std::move(new_result); - if (node_) { - RCLCPP_INFO(node_->get_logger(), "Hybrid discovery refreshed: %zu entities", cached_result_.report.total_entities); - } -} - -MergeReport HybridDiscoveryStrategy::get_merge_report() const { - std::lock_guard lock(mutex_); - return cached_result_.report; -} - -LinkingResult HybridDiscoveryStrategy::get_linking_result() const { - std::lock_guard lock(mutex_); - return cached_result_.linking_result; -} - -std::vector HybridDiscoveryStrategy::get_orphan_nodes() const { - std::lock_guard lock(mutex_); - return cached_result_.linking_result.orphan_nodes; -} - -void HybridDiscoveryStrategy::add_layer(std::unique_ptr layer) { - pipeline_.add_layer(std::move(layer)); -} - -} // 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 index 84020722b..1c990d832 100644 --- a/src/ros2_medkit_gateway/src/discovery/layers/runtime_layer.cpp +++ b/src/ros2_medkit_gateway/src/discovery/layers/runtime_layer.cpp @@ -68,7 +68,8 @@ size_t filter_apps_by_namespace(std::vector & apps, const GapFillConfig & c } // namespace -RuntimeLayer::RuntimeLayer(RuntimeDiscoveryStrategy * runtime_strategy) : runtime_strategy_(runtime_strategy) { +RuntimeLayer::RuntimeLayer(ros2::Ros2RuntimeIntrospection * runtime_introspection) + : runtime_introspection_(runtime_introspection) { policies_ = {{FieldGroup::IDENTITY, MergePolicy::FALLBACK}, {FieldGroup::HIERARCHY, MergePolicy::FALLBACK}, {FieldGroup::LIVE_DATA, MergePolicy::AUTHORITATIVE}, @@ -80,7 +81,7 @@ LayerOutput RuntimeLayer::discover() { LayerOutput output; last_filtered_count_ = 0; linking_apps_.clear(); - if (!runtime_strategy_) { + if (!runtime_introspection_) { return output; } @@ -90,7 +91,7 @@ LayerOutput RuntimeLayer::discover() { // Discover apps once. Always save unfiltered apps for post-merge linking. // The linker needs all runtime apps to bind manifest apps to live nodes, // regardless of gap-fill settings. - auto apps = runtime_strategy_->discover_apps(); + auto apps = runtime_introspection_->discover_apps(); linking_apps_ = apps; if (gap_fill_config_.allow_heuristic_apps) { @@ -100,7 +101,7 @@ LayerOutput RuntimeLayer::discover() { if (gap_fill_config_.allow_heuristic_functions) { // Use the pre-discovered apps to avoid redundant ROS 2 graph introspection - output.functions = runtime_strategy_->discover_functions(linking_apps_); + output.functions = runtime_introspection_->discover_functions(linking_apps_); } return output; @@ -123,17 +124,17 @@ void RuntimeLayer::set_gap_fill_config(GapFillConfig config) { } std::vector RuntimeLayer::discover_services() { - if (!runtime_strategy_) { + if (!runtime_introspection_) { return {}; } - return runtime_strategy_->discover_services(); + return runtime_introspection_->discover_services(); } std::vector RuntimeLayer::discover_actions() { - if (!runtime_strategy_) { + if (!runtime_introspection_) { return {}; } - return runtime_strategy_->discover_actions(); + return runtime_introspection_->discover_actions(); } } // namespace discovery diff --git a/src/ros2_medkit_gateway/src/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 75b509119..f189ef43c 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -142,7 +142,10 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki // Declare parameters with defaults declare_parameter("server.host", "127.0.0.1"); declare_parameter("server.port", 8080); - declare_parameter("refresh_interval_ms", 10000); + // Safety-backstop refresh interval. Primary discovery refresh is driven + // by rclcpp graph events; this only controls the periodic forced + // refresh used when a graph event would otherwise be missed. + declare_parameter("refresh_interval_ms", 30000); declare_parameter("fault_manager.namespace", ""); declare_parameter("fault_manager.service_timeout_sec", 5.0); declare_parameter("cors.allowed_origins", std::vector{}); @@ -160,6 +163,9 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki declare_parameter("logs.buffer_size", 200); // Ring buffer capacity per node; entries exceeding this are dropped (oldest first) + // Topic-sample default timeout used by the data-access path. + declare_parameter("topic_sample_timeout_sec", 1.0); + // TLS/HTTPS parameters declare_parameter("server.tls.enabled", false); declare_parameter("server.tls.cert_file", ""); @@ -351,15 +357,19 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki RCLCPP_WARN(get_logger(), "Binding to 0.0.0.0 - REST API accessible from ALL network interfaces!"); } - // Validate refresh interval + // Validate backstop interval. Discovery is primarily driven by rclcpp + // graph events polled every 100 ms; this value only governs the + // safety-backstop timer that periodically forces a refresh in case a + // graph event is missed. if (refresh_interval_ms_ < 100 || refresh_interval_ms_ > 60000) { - RCLCPP_WARN(get_logger(), "Invalid refresh interval %dms. Must be between 100-60000ms. Using default 10000ms.", + RCLCPP_WARN(get_logger(), + "Invalid backstop refresh interval %dms. Must be between 100-60000ms. Using default 30000ms.", refresh_interval_ms_); - refresh_interval_ms_ = 10000; + refresh_interval_ms_ = 30000; } // Log configuration - RCLCPP_INFO(get_logger(), "Configuration: REST API at %s:%d, refresh interval: %dms", server_host_.c_str(), + RCLCPP_INFO(get_logger(), "Configuration: REST API at %s:%d, backstop refresh interval: %dms", server_host_.c_str(), server_port_, refresh_interval_ms_); if (cors_config_.enabled) { @@ -559,10 +569,42 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki throw std::runtime_error("Discovery initialization failed"); } - data_access_mgr_ = std::make_unique(this); - operation_mgr_ = std::make_unique(this, discovery_mgr_.get()); - config_mgr_ = std::make_unique(this); - fault_mgr_ = std::make_unique(this); + const auto topic_sample_timeout_sec = get_parameter("topic_sample_timeout_sec").as_double(); + topic_transport_ = std::make_shared(this, topic_sample_timeout_sec); + data_access_mgr_ = std::make_unique(topic_transport_, topic_sample_timeout_sec); + service_transport_ = std::make_shared(this); + action_transport_ = std::make_shared(this); + const auto service_call_timeout_sec = + static_cast(declare_parameter("service_call_timeout_sec", static_cast(10))); + operation_mgr_ = std::make_unique(service_transport_, action_transport_, discovery_mgr_.get(), + service_call_timeout_sec); + + // Declare parameter-service tuning parameters here (they used to be + // declared inside ConfigurationManager). The manager body is now neutral + // and the transport adapter receives the resolved values. + rcl_interfaces::msg::ParameterDescriptor param_timeout_desc; + param_timeout_desc.description = "Timeout for ROS 2 parameter service calls (configurations endpoint)"; + rcl_interfaces::msg::FloatingPointRange param_timeout_range; + param_timeout_range.from_value = 0.1; + param_timeout_range.to_value = 10.0; + param_timeout_desc.floating_point_range.push_back(param_timeout_range); + const double parameter_service_timeout_sec = + declare_parameter("parameter_service_timeout_sec", 2.0, param_timeout_desc); + + rcl_interfaces::msg::ParameterDescriptor param_cache_desc; + param_cache_desc.description = "Negative cache TTL for unavailable parameter services (0 = disabled)"; + rcl_interfaces::msg::FloatingPointRange param_cache_range; + param_cache_range.from_value = 0.0; + param_cache_range.to_value = 3600.0; + param_cache_desc.floating_point_range.push_back(param_cache_range); + const double parameter_service_negative_cache_sec = + declare_parameter("parameter_service_negative_cache_sec", 60.0, param_cache_desc); + + parameter_transport_ = std::make_shared(this, parameter_service_timeout_sec, + parameter_service_negative_cache_sec); + config_mgr_ = std::make_unique(parameter_transport_); + fault_service_transport_ = std::make_shared(this); + fault_mgr_ = std::make_unique(fault_service_transport_); // Initialize bulk data store auto bd_storage_dir = get_parameter("bulk_data.storage_dir").as_string(); @@ -673,7 +715,8 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki RCLCPP_WARN(get_logger(), "logs.buffer_size %" PRId64 " clamped to %" PRId64, raw_buffer_size, clamped); } auto log_buffer_size = static_cast(clamped); - log_mgr_ = std::make_unique(this, plugin_mgr_.get(), log_buffer_size); + log_source_ = std::make_shared(this); + log_mgr_ = std::make_unique(log_source_, plugin_mgr_.get(), log_buffer_size); // Initialize update manager auto updates_enabled = get_parameter("updates.enabled").as_bool(); @@ -834,8 +877,14 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki trigger_config.max_triggers = static_cast(get_parameter("triggers.max_triggers").as_int()); trigger_config.on_restart_behavior = get_parameter("triggers.on_restart_behavior").as_string(); + // Subscribe to data topics for data triggers. The adapter is created + // before the trigger manager because the manager takes the transport + // by shared_ptr at construction time. + trigger_topic_subscriber_ = std::make_unique(this); + trigger_topic_transport_ = std::make_shared(trigger_topic_subscriber_.get()); + trigger_mgr_ = std::make_unique(*resource_change_notifier_, *condition_registry_, *trigger_store_, - trigger_config); + trigger_config, trigger_topic_transport_); // Set entity hierarchy resolver using thread-safe cache trigger_mgr_->set_entity_children_fn( @@ -882,10 +931,6 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki // Subscribe to fault events and forward to notifier trigger_fault_subscriber_ = std::make_unique(this, *resource_change_notifier_); - // Subscribe to data topics for data triggers - trigger_topic_subscriber_ = std::make_unique(this, *resource_change_notifier_); - trigger_mgr_->set_topic_subscriber(trigger_topic_subscriber_.get()); - // Wire deferred topic resolution: when a trigger's resource_path couldn't // be resolved to a ROS 2 topic at creation time, retry periodically using // the entity cache to find matching topics. @@ -1346,8 +1391,43 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki // Initial discovery refresh_cache(); - // Setup periodic refresh with configurable interval - refresh_timer_ = create_wall_timer(std::chrono::milliseconds(refresh_interval_ms_), [this]() { + // Primary refresh trigger: ROS 2 graph events. + // + // `get_graph_event()` returns a shared rclcpp::Event the executor signals + // whenever the graph changes (node up/down, topic/service/action add or + // remove). We poll `check_and_clear()` every 100 ms and only run the + // (relatively expensive) `refresh_cache()` pipeline when the event has + // fired. On a stable graph this keeps idle CPU near zero. + // + // 100 ms is a deliberate balance: spawn-detection latency stays low + // (graph-event arrival + at most one tick), while the polling overhead is + // a single atomic check per tick. + graph_event_ = this->get_graph_event(); + graph_check_timer_ = create_wall_timer(std::chrono::milliseconds(100), [this]() { + if (!graph_event_) { + return; + } + if (!graph_event_->check_and_clear()) { + return; + } + refresh_cache(); + // Note: orphan-trigger sweep is deliberately NOT run from the graph-event + // path. Graph events fire on every node up/down at high frequency, + // including the cold-start window where DDS discovery has only seen a + // partial node set. Running the sweep there would treat + // not-yet-discovered entities as orphans and incorrectly remove + // restored persistent triggers. The backstop timer below runs at the + // configured (slower) refresh cadence, giving DDS time to converge + // before any orphan check. + }); + + // Safety backstop: unconditional refresh at the configured (low) cadence. + // Guarantees liveness if a graph event is ever missed for any reason + // (lost wakeup, rclcpp anomaly, etc.). Default 30 s is rare enough to be + // negligible on idle CPU yet bounded enough to recover quickly. This is + // also the only place we sweep orphan triggers, because by this cadence + // the entity cache is guaranteed to reflect a settled DDS view. + backstop_timer_ = create_wall_timer(std::chrono::milliseconds(refresh_interval_ms_), [this]() { refresh_cache(); // Sweep triggers whose entities disappeared from discovery @@ -1447,6 +1527,9 @@ void GatewayNode::set_topic_data_provider(std::shared_ptr pro if (data_access_mgr_) { data_access_mgr_->set_topic_data_provider(topic_data_provider_.get()); } + if (topic_transport_) { + topic_transport_->set_data_provider(topic_data_provider_.get()); + } if (discovery_mgr_) { discovery_mgr_->set_topic_data_provider(topic_data_provider_.get()); } @@ -1648,9 +1731,9 @@ void GatewayNode::trigger_reentrant_notification_for_testing(const EntityChangeS void GatewayNode::refresh_cache() { // Serialize refresh passes across the refresh timer, plugin // `notify_entities_changed` notifications, and any other caller. The - // discovery pipeline (e.g., HybridDiscoveryStrategy::refresh()) is not - // thread-safe on its own - holding this lock for the full pass is cheap - // compared with the network I/O the caller typically just performed. + // discovery pipeline cached inside DiscoveryManager is not thread-safe on + // its own - holding this lock for the full pass is cheap compared with + // the network I/O the caller typically just performed. std::lock_guard refresh_lock(refresh_mutex_); // Mark the thread as "inside a refresh pass" so that a plugin calling diff --git a/src/ros2_medkit_gateway/src/http/handlers/data_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/data_handlers.cpp index e7c739a0f..fe7cc2643 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/data_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/data_handlers.cpp @@ -25,6 +25,7 @@ #include "ros2_medkit_gateway/core/plugins/plugin_manager.hpp" #include "ros2_medkit_gateway/core/providers/data_provider.hpp" #include "ros2_medkit_gateway/gateway_node.hpp" +#include "ros2_medkit_serialization/type_introspection.hpp" using json = nlohmann::json; diff --git a/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp index b88863358..a87cecc18 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp @@ -163,69 +163,94 @@ json extract_primary_value(const std::string & message_type, const json & full_d } // namespace -// Static method: Build SOVD-compliant fault response with environment data -json FaultHandlers::build_sovd_fault_response(const ros2_medkit_msgs::msg::Fault & fault, - const ros2_medkit_msgs::msg::EnvironmentData & env_data, +// Static method: Build SOVD-compliant fault response from transport-supplied JSON. +// +// The transport adapter performs ros2_medkit_msgs -> JSON translation; the +// handler post-processes the intermediate snapshot shape (freeze_frame parse, +// rosbag bulk_data_uri) using the entity_path it has at request time. +json FaultHandlers::build_sovd_fault_response(const json & fault_json, const json & env_data_json, const std::string & entity_path) { json response; + const std::string fault_code = fault_json.value("fault_code", ""); + const std::string status = fault_json.value("status", ""); + const uint8_t severity = fault_json.value("severity", static_cast(0)); + // === SOVD "item" structure === - response["item"] = {{"code", fault.fault_code}, - {"fault_name", fault.description}, - {"severity", fault.severity}, - {"status", build_status_object(fault.status)}}; + response["item"] = {{"code", fault_code}, + {"fault_name", fault_json.value("description", "")}, + {"severity", severity}, + {"status", build_status_object(status)}}; // === SOVD "environment_data" === json snapshots = json::array(); + if (env_data_json.contains("snapshots") && env_data_json["snapshots"].is_array()) { + for (const auto & s : env_data_json["snapshots"]) { + json snap; + const std::string type = s.value("type", ""); + snap["type"] = type; + snap["name"] = s.value("name", ""); + + if (type == "freeze_frame") { + // Parse JSON data string and extract primary value. + const std::string raw_data = s.value("data", ""); + const std::string topic = s.value("topic", ""); + const std::string message_type = s.value("message_type", ""); + const int64_t captured_at_ns = s.value("captured_at_ns", static_cast(0)); + try { + json full_data = json::parse(raw_data); + snap["data"] = extract_primary_value(message_type, full_data); + snap["x-medkit"] = {{"topic", topic}, + {"message_type", message_type}, + {"full_data", full_data}, + {"captured_at", to_iso8601_ns(captured_at_ns)}}; + } catch (const json::exception & e) { + snap["data"] = raw_data; + snap["x-medkit"] = {{"topic", topic}, {"message_type", message_type}, {"parse_error", e.what()}}; + } + } else if (type == "rosbag") { + // Build absolute URI using entity path + fault_code as the bulk-data ID. + // This must match the download handler which looks up rosbags by fault_code, + // and handle_list_descriptors which also uses fault_code as the descriptor ID. + const std::string snap_fault_code = s.value("fault_code", fault_code); + std::string bulk_data_uri = entity_path; + bulk_data_uri += "/bulk-data/rosbags/"; + bulk_data_uri += snap_fault_code; + snap["bulk_data_uri"] = std::move(bulk_data_uri); + if (s.contains("size_bytes")) { + snap["size_bytes"] = s["size_bytes"]; + } + if (s.contains("duration_sec")) { + snap["duration_sec"] = s["duration_sec"]; + } + if (s.contains("format")) { + snap["format"] = s["format"]; + } + } - for (const auto & s : env_data.snapshots) { - json snap; - snap["type"] = s.type; - snap["name"] = s.name; - - if (s.type == "freeze_frame") { - // Parse JSON data and extract primary value - try { - json full_data = json::parse(s.data); - snap["data"] = extract_primary_value(s.message_type, full_data); - snap["x-medkit"] = {{"topic", s.topic}, - {"message_type", s.message_type}, - {"full_data", full_data}, - {"captured_at", to_iso8601_ns(s.captured_at_ns)}}; - } catch (const json::exception & e) { - // Invalid JSON - include raw data - snap["data"] = s.data; - snap["x-medkit"] = {{"topic", s.topic}, {"message_type", s.message_type}, {"parse_error", e.what()}}; - } - } else if (s.type == "rosbag") { - // Build absolute URI using entity path + fault_code as the bulk-data ID. - // This must match the download handler which looks up rosbags by fault_code, - // and handle_list_descriptors which also uses fault_code as the descriptor ID. - snap["bulk_data_uri"] = entity_path + "/bulk-data/rosbags/" + fault.fault_code; - snap["size_bytes"] = s.size_bytes; - snap["duration_sec"] = s.duration_sec; - snap["format"] = s.format; - } - - snapshots.push_back(snap); + snapshots.push_back(snap); + } } - response["environment_data"] = { - {"extended_data_records", - {{"first_occurrence", to_iso8601_ns(env_data.extended_data_records.first_occurrence_ns)}, - {"last_occurrence", to_iso8601_ns(env_data.extended_data_records.last_occurrence_ns)}}}, - {"snapshots", snapshots}}; + json env_obj; + if (env_data_json.contains("extended_data_records")) { + env_obj["extended_data_records"] = env_data_json["extended_data_records"]; + } else { + env_obj["extended_data_records"] = {{"first_occurrence", ""}, {"last_occurrence", ""}}; + } + env_obj["snapshots"] = snapshots; + response["environment_data"] = env_obj; // === x-medkit extensions === json reporting_sources = json::array(); - for (const auto & src : fault.reporting_sources) { - reporting_sources.push_back(src); + if (fault_json.contains("reporting_sources") && fault_json["reporting_sources"].is_array()) { + reporting_sources = fault_json["reporting_sources"]; } - response["x-medkit"] = {{"occurrence_count", fault.occurrence_count}, + response["x-medkit"] = {{"occurrence_count", fault_json.value("occurrence_count", static_cast(0))}, {"reporting_sources", reporting_sources}, - {"severity_label", severity_to_label(fault.severity)}, - {"status_raw", fault.status}}; + {"severity_label", severity_to_label(severity)}, + {"status_raw", status}}; return response; } @@ -636,8 +661,11 @@ void FaultHandlers::handle_get_fault(const httplib::Request & req, httplib::Resp auto result = fault_mgr->get_fault_with_env(fault_code, namespace_path); if (result.success) { - // Build SOVD-compliant response with environment data - auto response = build_sovd_fault_response(result.fault, result.environment_data, entity_path_info->entity_path); + // Build SOVD-compliant response from the transport-supplied JSON shape. + // The transport handed back `result.data = { "fault": {...}, "environment_data": {...} }`. + const auto & fault_json = result.data.value("fault", json::object()); + const auto & env_data_json = result.data.value("environment_data", json::object()); + auto response = build_sovd_fault_response(fault_json, env_data_json, entity_path_info->entity_path); HandlerContext::send_json(res, response); } else { diff --git a/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp index 34437889c..7978ffcba 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp @@ -24,6 +24,7 @@ #include "ros2_medkit_gateway/core/providers/operation_provider.hpp" #include "ros2_medkit_gateway/gateway_node.hpp" #include "ros2_medkit_gateway/operation_manager.hpp" +#include "ros2_medkit_serialization/type_introspection.hpp" using json = nlohmann::json; diff --git a/src/ros2_medkit_gateway/src/http/handlers/sse_fault_handler.cpp b/src/ros2_medkit_gateway/src/http/handlers/sse_fault_handler.cpp index 8010dbb49..3046599b4 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/sse_fault_handler.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/sse_fault_handler.cpp @@ -21,9 +21,9 @@ #include #include "ros2_medkit_gateway/core/http/error_codes.hpp" -#include "ros2_medkit_gateway/fault_manager.hpp" #include "ros2_medkit_gateway/fault_manager_paths.hpp" #include "ros2_medkit_gateway/gateway_node.hpp" +#include "ros2_medkit_gateway/ros2/conversions/fault_msg_conversions.hpp" namespace ros2_medkit_gateway { namespace handlers { @@ -220,7 +220,7 @@ std::string SSEFaultHandler::format_sse_event(const ros2_medkit_msgs::msg::Fault nlohmann::json json_event; json_event["event_type"] = sanitized_event_type; - json_event["fault"] = FaultManager::fault_to_json(event.fault); + json_event["fault"] = ros2::conversions::fault_to_json(event.fault); // Convert timestamp to seconds with nanosecond precision double timestamp_sec = static_cast(event.timestamp.sec) + static_cast(event.timestamp.nanosec) * 1e-9; diff --git a/src/ros2_medkit_gateway/src/openapi/schema_builder.hpp b/src/ros2_medkit_gateway/src/openapi/schema_builder.hpp index 6bf365ddf..86ffa5ed7 100644 --- a/src/ros2_medkit_gateway/src/openapi/schema_builder.hpp +++ b/src/ros2_medkit_gateway/src/openapi/schema_builder.hpp @@ -50,7 +50,7 @@ class SchemaBuilder { /// SOVD GenericError schema (7.4.2) static nlohmann::json generic_error(); - /// Fault list item schema (flat format from FaultManager::fault_to_json) + /// Fault list item schema (flat format from ros2::conversions::fault_to_json) static nlohmann::json fault_list_item_schema(); /// Fault detail schema (SOVD nested format from FaultHandlers::build_sovd_fault_response) diff --git a/src/ros2_medkit_gateway/src/operation_manager.cpp b/src/ros2_medkit_gateway/src/operation_manager.cpp deleted file mode 100644 index 40e83a963..000000000 --- a/src/ros2_medkit_gateway/src/operation_manager.cpp +++ /dev/null @@ -1,938 +0,0 @@ -// Copyright 2025 mfaferek93 -// -// 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/operation_manager.hpp" - -#include -#include -#include -#include -#include - -#include "ros2_medkit_gateway/core/resource_change_notifier.hpp" -#include "ros2_medkit_serialization/json_serializer.hpp" -#include "ros2_medkit_serialization/message_cleanup.hpp" -#include "ros2_medkit_serialization/serialization_error.hpp" -#include "ros2_medkit_serialization/service_action_types.hpp" -#include "ros2_medkit_serialization/type_cache.hpp" - -namespace ros2_medkit_gateway { - -/// UUID hex string length (16 bytes = 32 hex characters) -constexpr size_t kUuidHexLength = 32; - -/// Default timeout for service calls in seconds -constexpr int kDefaultServiceCallTimeoutSec = 10; - -OperationManager::OperationManager(rclcpp::Node * node, DiscoveryManager * discovery_manager) - : node_(node) - , discovery_manager_(discovery_manager) - , rng_(std::random_device{}()) - , serializer_(std::make_shared()) - , service_call_timeout_sec_( - static_cast(node->declare_parameter("service_call_timeout_sec", kDefaultServiceCallTimeoutSec))) { - RCLCPP_INFO(node_->get_logger(), "OperationManager initialized with native serialization"); -} - -OperationManager::~OperationManager() { - shutdown(); -} - -void OperationManager::shutdown() { - // Clear subscriptions to stop receiving callbacks. - // Must happen while executor can still process pending callbacks safely. - { - std::lock_guard lock(subscriptions_mutex_); - status_subscriptions_.clear(); - } - - // Clear tracked goals - { - std::lock_guard lock(goals_mutex_); - tracked_goals_.clear(); - } - - // Clear all service/action clients to prevent handle_response() on destroyed - // pending_requests_. On Humble (compat shim), destroying a GenericServiceClient - // with unfulfilled promises causes "terminate called without an active exception" - // if a future still references the shared state. - { - std::unique_lock lock(clients_mutex_); - action_clients_.clear(); - generic_clients_.clear(); - } -} - -void OperationManager::set_notifier(ResourceChangeNotifier * notifier) { - notifier_ = notifier; -} - -bool OperationManager::is_valid_message_type(const std::string & type) { - // Valid formats: - // - package/srv/Type (service) - // - package/action/Type (action) - // - package/msg/Type (message - for publishing) - static const std::regex type_regex(R"(^[a-zA-Z_][a-zA-Z0-9_]*/(srv|action|msg)/[a-zA-Z_][a-zA-Z0-9_]*$)"); - return std::regex_match(type, type_regex); -} - -bool OperationManager::is_service_type(const std::string & type) { - return type.find("/srv/") != std::string::npos; -} - -bool OperationManager::is_action_type(const std::string & type) { - return type.find("/action/") != std::string::npos; -} - -bool OperationManager::is_valid_uuid_hex(const std::string & uuid_hex) { - if (uuid_hex.length() != kUuidHexLength) { - return false; - } - return std::all_of(uuid_hex.begin(), uuid_hex.end(), [](char c) { - return std::isxdigit(static_cast(c)); - }); -} - -std::string OperationManager::make_client_key(const std::string & service_path, const std::string & service_type) { - return service_path + "|" + service_type; -} - -compat::GenericServiceClient::SharedPtr -OperationManager::get_or_create_service_client(const std::string & service_path, const std::string & service_type) { - const std::string key = make_client_key(service_path, service_type); - - // Try read lock first (fast path) - { - std::shared_lock lock(clients_mutex_); - auto it = generic_clients_.find(key); - if (it != generic_clients_.end()) { - return it->second; - } - } - - // Need to create - take exclusive lock - std::unique_lock lock(clients_mutex_); - - // Double-check (another thread might have created it) - auto it = generic_clients_.find(key); - if (it != generic_clients_.end()) { - return it->second; - } - - // Create new client - auto client = compat::create_generic_service_client(node_, service_path, service_type); - generic_clients_[key] = client; - - RCLCPP_DEBUG(node_->get_logger(), "Created generic client for %s (%s)", service_path.c_str(), service_type.c_str()); - - return client; -} - -ServiceCallResult OperationManager::call_service(const std::string & service_path, const std::string & service_type, - const json & request) { - ServiceCallResult result; - - try { - using ros2_medkit_serialization::ServiceActionTypes; - - // Step 1: Get or create cached client - auto client = get_or_create_service_client(service_path, service_type); - - // Step 2: Wait for service availability - if (!client->wait_for_service(std::chrono::seconds(5))) { - result.success = false; - result.error_message = "Service not available: " + service_path; - return result; - } - - // Step 3: Build request/response type names - std::string request_type = ServiceActionTypes::get_service_request_type(service_type); - std::string response_type = ServiceActionTypes::get_service_response_type(service_type); - - // Step 4: Create request message from JSON - json request_data = request.empty() || request.is_null() ? json::object() : request; - - // Get defaults for request type and merge with provided data - try { - json defaults = serializer_->get_defaults(request_type); - // Merge: request overrides defaults - for (auto it = request_data.begin(); it != request_data.end(); ++it) { - defaults[it.key()] = it.value(); - } - request_data = defaults; - } catch (const ros2_medkit_serialization::TypeNotFoundError &) { - // If we can't get defaults, just use provided data - } - - // Convert JSON to ROS message (deserialized form, not CDR) - // Note: GenericClient expects void* pointing to deserialized message structure - RosMessage_Cpp ros_request = serializer_->from_json(request_type, request_data); - - RCLCPP_INFO(node_->get_logger(), "Calling service: %s (type: %s)", service_path.c_str(), service_type.c_str()); - - // Step 5: Send request using GenericClient (expects void* to deserialized message) - auto future_and_id = client->async_send_request(ros_request.data); - - // Step 6: Wait for response with timeout - auto timeout = std::chrono::seconds(service_call_timeout_sec_); - auto future_status = future_and_id.wait_for(timeout); - - if (future_status != std::future_status::ready) { - // Clean up pending request on timeout - client->remove_pending_request(future_and_id.request_id); - ros2_medkit_serialization::destroy_ros_message(&ros_request); - result.success = false; - result.error_message = - "Service call timed out (" + std::to_string(service_call_timeout_sec_) + "s): " + service_path; - return result; - } - - // Clean up request message after sending - ros2_medkit_serialization::destroy_ros_message(&ros_request); - - // Step 7: Get response and deserialize - auto response_ptr = future_and_id.get(); - - // The response is a void* pointing to the deserialized response message data - // We need to convert it back to JSON via the serializer - // GenericClient returns a SharedResponse (shared_ptr) - // The data is already deserialized by rclcpp internally - - // For now, we need to serialize the response back to get JSON - // This is a bit roundabout but works with the current GenericClient API - // TODO: Optimize by accessing response data directly when possible - - if (response_ptr != nullptr) { - // Create a temporary serialized message to deserialize from - // The response_ptr points to deserialized message data - // We need to re-serialize it to use our deserializer - - // Alternative approach: use the type info to read fields directly - // For now, try to get the response via YAML (simpler but less efficient) - - // Actually, GenericClient gives us a deserialized message already - // We can use dynmsg to convert it to JSON directly - try { - auto type_info = ros2_medkit_serialization::TypeCache::instance().get_message_type_info(response_type); - if (type_info != nullptr) { - result.response = serializer_->to_json(type_info, response_ptr.get()); - result.success = true; - RCLCPP_DEBUG(node_->get_logger(), "Service call succeeded: %s", result.response.dump().c_str()); - } else { - result.success = false; - result.error_message = "Unknown response type: " + response_type; - } - } catch (const std::exception & e) { - result.success = false; - result.error_message = std::string("Failed to deserialize response: ") + e.what(); - } - } else { - result.success = false; - result.error_message = "Service returned null response"; - } - - } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { - RCLCPP_ERROR(node_->get_logger(), "Type not found for service '%s': %s", service_path.c_str(), e.what()); - result.success = false; - result.error_message = std::string("Unknown service type: ") + e.what(); - - } catch (const ros2_medkit_serialization::SerializationError & e) { - RCLCPP_ERROR(node_->get_logger(), "Serialization failed for service '%s': %s", service_path.c_str(), e.what()); - result.success = false; - result.error_message = std::string("Invalid request format: ") + e.what(); - - } catch (const std::exception & e) { - std::string error_msg = e.what(); - RCLCPP_ERROR(node_->get_logger(), "Service call failed for '%s': %s", service_path.c_str(), e.what()); - result.success = false; - result.error_message = error_msg; - } - - return result; -} - -ServiceCallResult OperationManager::call_component_service(const std::string & component_ns, - const std::string & operation_name, - const std::optional & service_type, - const json & request) { - ServiceCallResult result; - - // Determine service type - use provided or look up from discovery - std::string resolved_type; - std::string service_path; - - if (service_type.has_value() && !service_type->empty()) { - resolved_type = *service_type; - // Construct service path from component namespace and operation name - service_path = component_ns; - if (!service_path.empty() && service_path.back() != '/') { - service_path += "/"; - } - service_path += operation_name; - } else { - // Look up from discovery cache - auto service_info = discovery_manager_->find_service(component_ns, operation_name); - if (!service_info.has_value()) { - result.success = false; - result.error_message = "Service not found: " + operation_name + " in namespace " + component_ns; - return result; - } - resolved_type = service_info->type; - service_path = service_info->full_path; - } - - // Validate type format - if (!is_valid_message_type(resolved_type)) { - result.success = false; - result.error_message = "Invalid service type format: " + resolved_type; - return result; - } - - // Verify it's a service type - if (!is_service_type(resolved_type)) { - result.success = false; - result.error_message = "Type is not a service type: " + resolved_type; - return result; - } - - return call_service(service_path, resolved_type, request); -} - -// ==================== ACTION OPERATIONS ==================== - -std::string action_status_to_string(ActionGoalStatus status) { - switch (status) { - case ActionGoalStatus::UNKNOWN: - return "unknown"; - case ActionGoalStatus::ACCEPTED: - return "accepted"; - case ActionGoalStatus::EXECUTING: - return "executing"; - case ActionGoalStatus::CANCELING: - return "canceling"; - case ActionGoalStatus::SUCCEEDED: - return "succeeded"; - case ActionGoalStatus::CANCELED: - return "canceled"; - case ActionGoalStatus::ABORTED: - return "aborted"; - default: - return "unknown"; - } -} - -std::array OperationManager::generate_uuid() { - std::lock_guard lock(rng_mutex_); - std::array uuid; - std::uniform_int_distribution dist(0, 255); - for (auto & byte : uuid) { - byte = static_cast(dist(rng_)); - } - // Set version 4 (random UUID) - uuid[6] = (uuid[6] & 0x0f) | 0x40; - // Set variant bits - uuid[8] = (uuid[8] & 0x3f) | 0x80; - return uuid; -} - -json OperationManager::uuid_bytes_to_json_array(const std::array & uuid) { - json array = json::array(); - for (auto byte : uuid) { - array.push_back(static_cast(byte)); - } - return array; -} - -json OperationManager::uuid_hex_to_json_array(const std::string & uuid_hex) { - json array = json::array(); - for (size_t i = 0; i + 1 < uuid_hex.length() && i < kUuidHexLength; i += 2) { - int byte_val = std::stoi(uuid_hex.substr(i, 2), nullptr, 16); - array.push_back(byte_val); - } - return array; -} - -OperationManager::ActionClientSet & OperationManager::get_or_create_action_clients(const std::string & action_path, - const std::string & action_type) { - std::unique_lock lock(clients_mutex_); - - auto it = action_clients_.find(action_path); - if (it != action_clients_.end()) { - return it->second; - } - - using namespace ros2_medkit_serialization; - - ActionClientSet clients; - clients.action_type = action_type; - - // Send goal service: {action}/_action/send_goal - std::string send_goal_service = action_path + "/_action/send_goal"; - std::string send_goal_type = ServiceActionTypes::get_action_send_goal_service_type(action_type); - clients.send_goal_client = compat::create_generic_service_client(node_, send_goal_service, send_goal_type); - - // Get result service: {action}/_action/get_result - std::string get_result_service = action_path + "/_action/get_result"; - std::string get_result_type = ServiceActionTypes::get_action_get_result_service_type(action_type); - clients.get_result_client = compat::create_generic_service_client(node_, get_result_service, get_result_type); - - // Cancel goal service (standard type for all actions) - std::string cancel_service = action_path + "/_action/cancel_goal"; - clients.cancel_goal_client = - compat::create_generic_service_client(node_, cancel_service, "action_msgs/srv/CancelGoal"); - - RCLCPP_DEBUG(node_->get_logger(), "Created action clients for %s (type: %s)", action_path.c_str(), - action_type.c_str()); - - action_clients_[action_path] = std::move(clients); - return action_clients_[action_path]; -} - -void OperationManager::track_goal(const std::string & goal_id, const std::string & action_path, - const std::string & action_type, const std::string & entity_id) { - std::lock_guard lock(goals_mutex_); - ActionGoalInfo info; - info.goal_id = goal_id; - info.action_path = action_path; - info.action_type = action_type; - info.entity_id = entity_id; - info.status = ActionGoalStatus::ACCEPTED; - info.created_at = std::chrono::system_clock::now(); - info.last_update = info.created_at; - tracked_goals_[goal_id] = info; -} - -ActionSendGoalResult OperationManager::send_action_goal(const std::string & action_path, - const std::string & action_type, const json & goal, - const std::string & entity_id) { - ActionSendGoalResult result; - result.success = false; - result.goal_accepted = false; - - try { - using namespace ros2_medkit_serialization; - - // Step 1: Get or create action clients - auto & clients = get_or_create_action_clients(action_path, action_type); - - // Step 2: Wait for send_goal service - if (!clients.send_goal_client->wait_for_service(std::chrono::seconds(5))) { - result.error_message = "Action server not available: " + action_path; - return result; - } - - // Step 3: Generate UUID for goal - auto uuid_bytes = generate_uuid(); - - // Step 4: Build SendGoal request - // The request has structure: {goal_id: {uuid: [...]}, goal: {...}} - json send_goal_request; - send_goal_request["goal_id"]["uuid"] = uuid_bytes_to_json_array(uuid_bytes); - send_goal_request["goal"] = goal.empty() || goal.is_null() ? json::object() : goal; - - // Step 5: Create request message from JSON and send - std::string request_type = ServiceActionTypes::get_action_send_goal_request_type(action_type); - - RCLCPP_INFO(node_->get_logger(), "SendGoal request type: %s, JSON: %s", request_type.c_str(), - send_goal_request.dump().c_str()); - - // Convert JSON to ROS message (deserialized form, not CDR) - // Note: GenericClient expects void* pointing to deserialized message structure - RosMessage_Cpp ros_request = serializer_->from_json(request_type, send_goal_request); - - RCLCPP_INFO(node_->get_logger(), "Sending action goal: %s (type: %s)", action_path.c_str(), action_type.c_str()); - - // Send using GenericClient (expects void* to deserialized message) - auto future_and_id = clients.send_goal_client->async_send_request(ros_request.data); - - // Wait for response with timeout - auto timeout = std::chrono::seconds(service_call_timeout_sec_); - auto future_status = future_and_id.wait_for(timeout); - - if (future_status != std::future_status::ready) { - clients.send_goal_client->remove_pending_request(future_and_id.request_id); - ros2_medkit_serialization::destroy_ros_message(&ros_request); - result.error_message = "Send goal timed out"; - return result; - } - - // Clean up request message after sending - ros2_medkit_serialization::destroy_ros_message(&ros_request); - - // Step 6: Deserialize response - auto response_ptr = future_and_id.get(); - if (response_ptr == nullptr) { - result.error_message = "Send goal returned null response"; - return result; - } - - std::string response_type = ServiceActionTypes::get_action_send_goal_response_type(action_type); - auto type_info = ros2_medkit_serialization::TypeCache::instance().get_message_type_info(response_type); - if (type_info == nullptr) { - result.error_message = "Unknown response type: " + response_type; - return result; - } - json response = serializer_->to_json(type_info, response_ptr.get()); - - // Step 7: Extract goal_id and acceptance - result.success = true; - result.goal_accepted = response.value("accepted", false); - - if (result.goal_accepted) { - result.goal_id = uuid_bytes_to_hex(uuid_bytes); - track_goal(result.goal_id, action_path, action_type, entity_id); - subscribe_to_action_status(action_path); - update_goal_status(result.goal_id, ActionGoalStatus::EXECUTING); - RCLCPP_INFO(node_->get_logger(), "Action goal accepted with ID: %s", result.goal_id.c_str()); - } else { - result.error_message = "Goal rejected by action server"; - } - - } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Send action goal failed for '%s': %s", action_path.c_str(), e.what()); - result.success = false; - result.error_message = e.what(); - } - - return result; -} - -ActionSendGoalResult OperationManager::send_component_action_goal(const std::string & component_ns, - const std::string & operation_name, - const std::optional & action_type, - const json & goal, const std::string & entity_id) { - ActionSendGoalResult result; - - // Determine action type - use provided or look up from discovery - std::string resolved_type; - std::string action_path; - - if (action_type.has_value() && !action_type->empty()) { - resolved_type = *action_type; - // Construct action path from component namespace and operation name - action_path = component_ns; - if (!action_path.empty() && action_path.back() != '/') { - action_path += "/"; - } - action_path += operation_name; - } else { - // Look up from discovery cache - auto action_info = discovery_manager_->find_action(component_ns, operation_name); - if (!action_info.has_value()) { - result.success = false; - result.error_message = "Action not found: " + operation_name + " in namespace " + component_ns; - return result; - } - resolved_type = action_info->type; - action_path = action_info->full_path; - } - - // Validate type format - if (!is_valid_message_type(resolved_type)) { - result.success = false; - result.error_message = "Invalid action type format: " + resolved_type; - return result; - } - - // Verify it's an action type - if (!is_action_type(resolved_type)) { - result.success = false; - result.error_message = "Type is not an action type: " + resolved_type; - return result; - } - - return send_action_goal(action_path, resolved_type, goal, entity_id); -} - -ActionCancelResult OperationManager::cancel_action_goal(const std::string & action_path, const std::string & goal_id) { - ActionCancelResult result; - result.success = false; - - // Validate goal_id format - if (!is_valid_uuid_hex(goal_id)) { - result.error_message = "Invalid goal_id format: must be 32 hex characters"; - return result; - } - - try { - // Get the tracked goal to find action type - auto goal_info = get_tracked_goal(goal_id); - if (!goal_info) { - result.error_message = "Unknown goal_id - not tracked"; - return result; - } - - // Get or create action clients (use tracked type) - auto & clients = get_or_create_action_clients(action_path, goal_info->action_type); - - if (!clients.cancel_goal_client->wait_for_service(std::chrono::seconds(2))) { - result.error_message = "Cancel service not available"; - return result; - } - - // Build cancel request: {goal_info: {goal_id: {uuid: [...]}}} - json cancel_request; - cancel_request["goal_info"]["goal_id"]["uuid"] = uuid_hex_to_json_array(goal_id); - - // Convert JSON to ROS message (deserialized form, not CDR) - // Note: GenericClient expects void* pointing to deserialized message structure - RosMessage_Cpp ros_request = serializer_->from_json("action_msgs/srv/CancelGoal_Request", cancel_request); - - RCLCPP_INFO(node_->get_logger(), "Canceling action goal: %s (goal_id: %s)", action_path.c_str(), goal_id.c_str()); - - // Send using GenericClient (expects void* to deserialized message) - auto future_and_id = clients.cancel_goal_client->async_send_request(ros_request.data); - - auto future_status = future_and_id.wait_for(std::chrono::seconds(5)); - if (future_status != std::future_status::ready) { - clients.cancel_goal_client->remove_pending_request(future_and_id.request_id); - ros2_medkit_serialization::destroy_ros_message(&ros_request); - result.error_message = "Cancel request timed out"; - return result; - } - ros2_medkit_serialization::destroy_ros_message(&ros_request); - - auto response_ptr = future_and_id.get(); - if (response_ptr == nullptr) { - result.error_message = "Cancel returned null response"; - return result; - } - - auto type_info = - ros2_medkit_serialization::TypeCache::instance().get_message_type_info("action_msgs/srv/CancelGoal_Response"); - if (type_info == nullptr) { - result.error_message = "Unknown response type: action_msgs/srv/CancelGoal_Response"; - return result; - } - json response = serializer_->to_json(type_info, response_ptr.get()); - - result.success = true; - result.return_code = static_cast(response.value("return_code", 0)); - - if (result.return_code == 0) { - update_goal_status(goal_id, ActionGoalStatus::CANCELING); - RCLCPP_INFO(node_->get_logger(), "Cancel request accepted for goal: %s", goal_id.c_str()); - } else { - // Map return codes to error messages - switch (result.return_code) { - case 1: - result.error_message = "Cancel request rejected"; - break; - case 2: - result.error_message = "Unknown goal ID"; - break; - case 3: - result.error_message = "Goal already terminated"; - break; - default: - result.error_message = "Unknown cancel error"; - break; - } - } - - } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Action cancel failed for '%s': %s", action_path.c_str(), e.what()); - result.error_message = e.what(); - } - - return result; -} - -ActionGetResultResult OperationManager::get_action_result(const std::string & action_path, - const std::string & action_type, - const std::string & goal_id) { - ActionGetResultResult result; - result.success = false; - result.status = ActionGoalStatus::UNKNOWN; - - // Validate goal_id format - if (!is_valid_uuid_hex(goal_id)) { - result.error_message = "Invalid goal_id format: must be 32 hex characters"; - return result; - } - - try { - using namespace ros2_medkit_serialization; - - // Get or create action clients - auto & clients = get_or_create_action_clients(action_path, action_type); - - if (!clients.get_result_client->wait_for_service(std::chrono::seconds(2))) { - result.error_message = "Get result service not available"; - return result; - } - - // Build request: {goal_id: {uuid: [...]}} - json get_result_request; - get_result_request["goal_id"]["uuid"] = uuid_hex_to_json_array(goal_id); - - std::string request_type = ServiceActionTypes::get_action_get_result_request_type(action_type); - - // Convert JSON to ROS message (deserialized form, not CDR) - // Note: GenericClient expects void* pointing to deserialized message structure - RosMessage_Cpp ros_request = serializer_->from_json(request_type, get_result_request); - - RCLCPP_INFO(node_->get_logger(), "Getting action result: %s (goal_id: %s)", action_path.c_str(), goal_id.c_str()); - - // Send using GenericClient (expects void* to deserialized message) - auto future_and_id = clients.get_result_client->async_send_request(ros_request.data); - - auto future_status = future_and_id.wait_for(std::chrono::seconds(service_call_timeout_sec_)); - if (future_status != std::future_status::ready) { - clients.get_result_client->remove_pending_request(future_and_id.request_id); - ros2_medkit_serialization::destroy_ros_message(&ros_request); - result.error_message = "Get result timed out"; - return result; - } - ros2_medkit_serialization::destroy_ros_message(&ros_request); - - auto response_ptr = future_and_id.get(); - if (response_ptr == nullptr) { - result.error_message = "Get result returned null response"; - return result; - } - - std::string response_type = ServiceActionTypes::get_action_get_result_response_type(action_type); - auto type_info = ros2_medkit_serialization::TypeCache::instance().get_message_type_info(response_type); - if (type_info == nullptr) { - result.error_message = "Unknown response type: " + response_type; - return result; - } - json response = serializer_->to_json(type_info, response_ptr.get()); - - result.success = true; - - // Extract status from response - if (response.contains("status")) { - int status_val = response["status"].get(); - result.status = static_cast(status_val); - } - - // Extract result from response - if (response.contains("result")) { - result.result = response["result"]; - } else { - result.result = response; - } - - // Update local tracking - update_goal_status(goal_id, result.status); - - RCLCPP_INFO(node_->get_logger(), "Got action result for %s: status=%s", goal_id.c_str(), - action_status_to_string(result.status).c_str()); - - } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Get action result failed for '%s': %s", action_path.c_str(), e.what()); - result.error_message = e.what(); - } - - return result; -} - -std::optional OperationManager::get_tracked_goal(const std::string & goal_id) const { - std::lock_guard lock(goals_mutex_); - auto it = tracked_goals_.find(goal_id); - if (it != tracked_goals_.end()) { - return it->second; - } - return std::nullopt; -} - -std::vector OperationManager::list_tracked_goals() const { - std::lock_guard lock(goals_mutex_); - std::vector goals; - goals.reserve(tracked_goals_.size()); - for (const auto & [id, info] : tracked_goals_) { - goals.push_back(info); - } - return goals; -} - -std::vector OperationManager::get_goals_for_action(const std::string & action_path) const { - std::vector goals; - - // Copy data under lock - { - std::lock_guard lock(goals_mutex_); - for (const auto & [id, info] : tracked_goals_) { - if (info.action_path == action_path) { - goals.push_back(info); - } - } - } // Release lock before sorting - - // Sort by created_at, newest first (most recently created goal) - std::sort(goals.begin(), goals.end(), [](const ActionGoalInfo & a, const ActionGoalInfo & b) { - return a.created_at > b.created_at; - }); - return goals; -} - -std::optional OperationManager::get_latest_goal_for_action(const std::string & action_path) const { - auto goals = get_goals_for_action(action_path); - if (goals.empty()) { - return std::nullopt; - } - return goals.front(); // Already sorted newest first -} - -void OperationManager::update_goal_status(const std::string & goal_id, ActionGoalStatus status) { - std::lock_guard lock(goals_mutex_); - auto it = tracked_goals_.find(goal_id); - if (it != tracked_goals_.end()) { - it->second.status = status; - it->second.last_update = std::chrono::system_clock::now(); - } -} - -void OperationManager::update_goal_feedback(const std::string & goal_id, const json & feedback) { - std::lock_guard lock(goals_mutex_); - auto it = tracked_goals_.find(goal_id); - if (it != tracked_goals_.end()) { - it->second.last_feedback = feedback; - it->second.last_update = std::chrono::system_clock::now(); - } -} - -void OperationManager::cleanup_old_goals(std::chrono::seconds max_age) { - std::set actions_to_check; - - { - std::lock_guard lock(goals_mutex_); - auto now = std::chrono::system_clock::now(); - - for (auto it = tracked_goals_.begin(); it != tracked_goals_.end();) { - // Only remove completed goals (succeeded, canceled, aborted) - if (it->second.status == ActionGoalStatus::SUCCEEDED || it->second.status == ActionGoalStatus::CANCELED || - it->second.status == ActionGoalStatus::ABORTED) { - auto age = std::chrono::duration_cast(now - it->second.last_update); - if (age > max_age) { - actions_to_check.insert(it->second.action_path); - it = tracked_goals_.erase(it); - continue; - } - } - ++it; - } - } // Release goals_mutex_ before checking subscriptions - - // Check if any action paths need to be unsubscribed - for (const auto & action_path : actions_to_check) { - auto remaining_goals = get_goals_for_action(action_path); - if (remaining_goals.empty()) { - unsubscribe_from_action_status(action_path); - } - } -} - -// ==================== NATIVE STATUS SUBSCRIPTION ==================== - -std::string OperationManager::uuid_bytes_to_hex(const std::array & uuid) const { - std::ostringstream ss; - ss << std::hex << std::setfill('0'); - for (const auto & byte : uuid) { - ss << std::setw(2) << static_cast(byte); - } - return ss.str(); -} - -void OperationManager::subscribe_to_action_status(const std::string & action_path) { - std::lock_guard lock(subscriptions_mutex_); - - // Check if already subscribed - if (status_subscriptions_.count(action_path) > 0) { - return; - } - - // Create subscription to status topic - std::string status_topic = action_path + "/_action/status"; - - auto callback = [this, action_path](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { - on_action_status(action_path, msg); - }; - - auto subscription = node_->create_subscription( - status_topic, rclcpp::QoS(10).best_effort(), callback); - - status_subscriptions_[action_path] = subscription; - RCLCPP_INFO(node_->get_logger(), "Subscribed to action status: %s", status_topic.c_str()); -} - -void OperationManager::unsubscribe_from_action_status(const std::string & action_path) { - std::lock_guard lock(subscriptions_mutex_); - - auto it = status_subscriptions_.find(action_path); - if (it != status_subscriptions_.end()) { - status_subscriptions_.erase(it); - RCLCPP_INFO(node_->get_logger(), "Unsubscribed from action status: %s/_action/status", action_path.c_str()); - } -} - -void OperationManager::on_action_status(const std::string & action_path, - const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { - std::lock_guard lock(goals_mutex_); - - for (const auto & status : msg->status_list) { - // Convert UUID bytes to hex string - std::string goal_id = uuid_bytes_to_hex(status.goal_info.goal_id.uuid); - - // Find if we're tracking this goal - auto it = tracked_goals_.find(goal_id); - if (it != tracked_goals_.end() && it->second.action_path == action_path) { - // Map status code to our enum - ActionGoalStatus new_status; - switch (status.status) { - case action_msgs::msg::GoalStatus::STATUS_ACCEPTED: - new_status = ActionGoalStatus::ACCEPTED; - break; - case action_msgs::msg::GoalStatus::STATUS_EXECUTING: - new_status = ActionGoalStatus::EXECUTING; - break; - case action_msgs::msg::GoalStatus::STATUS_CANCELING: - new_status = ActionGoalStatus::CANCELING; - break; - case action_msgs::msg::GoalStatus::STATUS_SUCCEEDED: - new_status = ActionGoalStatus::SUCCEEDED; - break; - case action_msgs::msg::GoalStatus::STATUS_CANCELED: - new_status = ActionGoalStatus::CANCELED; - break; - case action_msgs::msg::GoalStatus::STATUS_ABORTED: - new_status = ActionGoalStatus::ABORTED; - break; - default: - new_status = ActionGoalStatus::UNKNOWN; - break; - } - - // Only update if status changed - if (it->second.status != new_status) { - RCLCPP_INFO(node_->get_logger(), "Goal %s status update: %s -> %s", goal_id.c_str(), - action_status_to_string(it->second.status).c_str(), action_status_to_string(new_status).c_str()); - it->second.status = new_status; - it->second.last_update = std::chrono::system_clock::now(); - - if (notifier_) { - nlohmann::json goal_json; - goal_json["goal_id"] = goal_id; - goal_json["action_path"] = it->second.action_path; - goal_json["status"] = action_status_to_string(new_status); - notifier_->notify("operations", it->second.entity_id, goal_id, goal_json); - } - } - } - } -} - -} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/ros2/conversions/fault_msg_conversions.cpp b/src/ros2_medkit_gateway/src/ros2/conversions/fault_msg_conversions.cpp new file mode 100644 index 000000000..06302c777 --- /dev/null +++ b/src/ros2_medkit_gateway/src/ros2/conversions/fault_msg_conversions.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/ros2/conversions/fault_msg_conversions.hpp" + +#include + +#include "ros2_medkit_gateway/core/http/http_utils.hpp" + +namespace ros2_medkit_gateway::ros2::conversions { + +namespace { + +double to_seconds(const builtin_interfaces::msg::Time & t) { + return t.sec + static_cast(t.nanosec) / 1e9; +} + +} // namespace + +nlohmann::json fault_to_json(const ros2_medkit_msgs::msg::Fault & fault) { + nlohmann::json j; + j["fault_code"] = fault.fault_code; + j["severity"] = fault.severity; + j["description"] = fault.description; + j["first_occurred"] = to_seconds(fault.first_occurred); + j["last_occurred"] = to_seconds(fault.last_occurred); + j["occurrence_count"] = fault.occurrence_count; + j["status"] = fault.status; + j["reporting_sources"] = fault.reporting_sources; + + switch (fault.severity) { + case ros2_medkit_msgs::msg::Fault::SEVERITY_INFO: + j["severity_label"] = "INFO"; + break; + case ros2_medkit_msgs::msg::Fault::SEVERITY_WARN: + j["severity_label"] = "WARN"; + break; + case ros2_medkit_msgs::msg::Fault::SEVERITY_ERROR: + j["severity_label"] = "ERROR"; + break; + case ros2_medkit_msgs::msg::Fault::SEVERITY_CRITICAL: + j["severity_label"] = "CRITICAL"; + break; + default: + j["severity_label"] = "UNKNOWN"; + break; + } + + return j; +} + +nlohmann::json environment_data_to_json(const ros2_medkit_msgs::msg::EnvironmentData & env_data) { + using nlohmann::json; + + json snapshots = json::array(); + for (const auto & s : env_data.snapshots) { + json snap; + snap["type"] = s.type; + snap["name"] = s.name; + + if (s.type == "freeze_frame") { + // Preserve the parsed payload (including primary-value extraction hint + // via "message_type") so the handler can extract the primary value + // without re-parsing. We pass the raw string + message_type so the + // handler keeps full control over primary-field extraction. + snap["data"] = s.data; + snap["topic"] = s.topic; + snap["message_type"] = s.message_type; + snap["captured_at_ns"] = s.captured_at_ns; + } else if (s.type == "rosbag") { + snap["fault_code"] = s.bulk_data_id; + snap["size_bytes"] = s.size_bytes; + snap["duration_sec"] = s.duration_sec; + snap["format"] = s.format; + // bulk_data_uri intentionally omitted - handler appends per-request URL + } + + snapshots.push_back(snap); + } + + json out; + out["extended_data_records"] = { + {"first_occurrence", format_timestamp_ns(env_data.extended_data_records.first_occurrence_ns)}, + {"last_occurrence", format_timestamp_ns(env_data.extended_data_records.last_occurrence_ns)}}; + out["snapshots"] = snapshots; + return out; +} + +} // namespace ros2_medkit_gateway::ros2::conversions diff --git a/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp b/src/ros2_medkit_gateway/src/ros2/providers/ros2_runtime_introspection.cpp similarity index 71% rename from src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp rename to src/ros2_medkit_gateway/src/ros2/providers/ros2_runtime_introspection.cpp index bfb558727..c36412f8f 100644 --- a/src/ros2_medkit_gateway/src/discovery/runtime_discovery.cpp +++ b/src/ros2_medkit_gateway/src/ros2/providers/ros2_runtime_introspection.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 selfpatch +// 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. @@ -12,36 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ros2_medkit_gateway/discovery/runtime_discovery.hpp" +#include "ros2_medkit_gateway/ros2/providers/ros2_runtime_introspection.hpp" #include #include #include -namespace ros2_medkit_gateway { -namespace discovery { +namespace ros2_medkit_gateway::ros2 { + +namespace { /** - * @brief Check if a service path matches an internal ROS2 service suffix. - * - * Uses exact suffix matching to avoid false positives. For example, - * "/my_node/get_parameters" is internal, but "/my_node/get_parameters_backup" is not. + * @brief Exact suffix match on a service path. * - * @param service_path The full service path (e.g., "/my_node/get_parameters") - * @param suffix The suffix to match (e.g., "/get_parameters") - * @return true if the service_path ends with the suffix + * Avoids false positives that a substring search would produce + * (e.g. "/my_node/get_parameters" is internal but + * "/my_node/get_parameters_backup" is not). */ -static bool matches_internal_suffix(const std::string & service_path, const std::string & suffix) { +bool matches_internal_suffix(const std::string & service_path, const std::string & suffix) { if (service_path.length() < suffix.length()) { return false; } return service_path.compare(service_path.length() - suffix.length(), suffix.length(), suffix) == 0; } -// Helper function to check if a service path is internal ROS2 infrastructure -bool RuntimeDiscoveryStrategy::is_internal_service(const std::string & service_path) { - // Use exact suffix matching to avoid false positives - // e.g., "/my_node/get_parameters" is internal, but "/my_node/get_parameters_backup" is NOT +} // namespace + +bool Ros2RuntimeIntrospection::is_internal_service(const std::string & service_path) { return matches_internal_suffix(service_path, "/get_parameters") || matches_internal_suffix(service_path, "/set_parameters") || matches_internal_suffix(service_path, "/list_parameters") || @@ -49,39 +46,37 @@ bool RuntimeDiscoveryStrategy::is_internal_service(const std::string & service_p matches_internal_suffix(service_path, "/get_parameter_types") || matches_internal_suffix(service_path, "/set_parameters_atomically") || matches_internal_suffix(service_path, "/get_type_description") || - service_path.find("/_action/") != std::string::npos; // Action internal services + service_path.find("/_action/") != std::string::npos; } -RuntimeDiscoveryStrategy::RuntimeDiscoveryStrategy(rclcpp::Node * node) : node_(node) { +Ros2RuntimeIntrospection::Ros2RuntimeIntrospection(rclcpp::Node * node) : node_(node) { } -void RuntimeDiscoveryStrategy::set_config(const RuntimeConfig & config) { +void Ros2RuntimeIntrospection::set_config(const RuntimeConfig & config) { config_ = config; - RCLCPP_DEBUG(node_->get_logger(), "Runtime discovery config: functions_from_namespaces=%s", + RCLCPP_DEBUG(node_->get_logger(), "Runtime introspection config: functions_from_namespaces=%s", config_.create_functions_from_namespaces ? "true" : "false"); } -std::vector RuntimeDiscoveryStrategy::discover_areas() { - // Areas come from manifest only - runtime discovery never creates Areas. - return {}; -} - -std::vector RuntimeDiscoveryStrategy::discover_components() { - // Components come from HostInfoProvider or manifest - runtime discovery never creates Components. - return {}; +IntrospectionResult Ros2RuntimeIntrospection::introspect(const IntrospectionInput & /*input*/) { + IntrospectionResult result; + // Areas and Components are never produced by runtime introspection. + // Apps come from the live graph; Functions are derived from namespaces. + auto apps = discover_apps(); + result.new_entities.functions = discover_functions(apps); + result.new_entities.apps = std::move(apps); + return result; } -std::vector RuntimeDiscoveryStrategy::discover_apps() { +std::vector Ros2RuntimeIntrospection::discover_apps() { std::vector apps; - // Pre-build service info map for schema lookups std::unordered_map service_info_map; auto all_services = discover_services(); for (const auto & svc : all_services) { service_info_map[svc.full_path] = svc; } - // Pre-build action info map for schema lookups std::unordered_map action_info_map; auto all_actions = discover_actions(); for (const auto & act : all_actions) { @@ -102,19 +97,16 @@ std::vector RuntimeDiscoveryStrategy::discover_apps() { return {}; } - // Build topic map if not yet ready (first call or after manual refresh) if (topic_data_provider_ && !topic_map_ready_) { refresh_topic_map(); } - // Deduplicate nodes - ROS 2 RMW can report duplicates for nodes with multiple interfaces std::set seen_fqns; - // First pass: detect bare-name collisions across different namespaces so we - // can disambiguate IDs only when necessary (preserving backward compatibility - // for the common case where node names are unique). - // Count distinct namespaces per node name to avoid false positives from - // RMW duplicates (same FQN reported multiple times). + // Detect bare-name collisions across namespaces so IDs are only disambiguated + // when necessary (preserves backward compatibility for the common case where + // node names are unique). Count distinct namespaces per bare name to avoid + // false positives from RMW duplicates (same FQN reported multiple times). std::unordered_map> name_namespaces; for (const auto & name_and_ns : names_and_namespaces) { name_namespaces[name_and_ns.first].insert(name_and_ns.second); @@ -126,7 +118,6 @@ std::vector RuntimeDiscoveryStrategy::discover_apps() { std::string fqn = (ns == "/") ? std::string("/").append(name) : std::string(ns).append("/").append(name); - // Skip duplicate nodes if (seen_fqns.count(fqn) > 0) { RCLCPP_DEBUG(node_->get_logger(), "Skipping duplicate node: %s", fqn.c_str()); continue; @@ -134,10 +125,6 @@ std::vector RuntimeDiscoveryStrategy::discover_apps() { seen_fqns.insert(fqn); App app; - // When multiple namespaces contain nodes with the same bare name - // (e.g., /ns1/controller and /ns2/controller), include the namespace in the - // ID to avoid collisions. Otherwise, use the bare name for backward - // compatibility. if (name_namespaces[name].size() > 1 && ns != "/") { std::string ns_prefix = ns.substr(1); // Remove leading '/' std::replace(ns_prefix.begin(), ns_prefix.end(), '/', '_'); @@ -154,7 +141,6 @@ std::vector RuntimeDiscoveryStrategy::discover_apps() { app.is_online = true; app.bound_fqn = fqn; - // 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) { @@ -196,7 +182,6 @@ std::vector RuntimeDiscoveryStrategy::discover_apps() { ns.c_str(), e.what()); } - // Populate topics from cached map if (topic_data_provider_) { auto it = cached_topic_map_.find(fqn); if (it != cached_topic_map_.end()) { @@ -211,20 +196,18 @@ std::vector RuntimeDiscoveryStrategy::discover_apps() { return apps; } -std::vector RuntimeDiscoveryStrategy::discover_functions() { +std::vector Ros2RuntimeIntrospection::discover_functions() { if (!config_.create_functions_from_namespaces) { return {}; } - // Discover apps fresh when called without pre-discovered apps return discover_functions(discover_apps()); } -std::vector RuntimeDiscoveryStrategy::discover_functions(const std::vector & apps) { +std::vector Ros2RuntimeIntrospection::discover_functions(const std::vector & apps) { if (!config_.create_functions_from_namespaces) { return {}; } - // Group apps by namespace (first segment), similar to discover_areas() logic std::map> ns_to_app_ids; for (const auto & app : apps) { std::string ns = "/"; @@ -237,7 +220,6 @@ std::vector RuntimeDiscoveryStrategy::discover_functions(const std::ve ns_to_app_ids[area].push_back(app.id); } - // Create Function entities from namespace groups std::vector functions; for (const auto & [ns_name, app_ids] : ns_to_app_ids) { if (app_ids.empty()) { @@ -259,12 +241,11 @@ std::vector RuntimeDiscoveryStrategy::discover_functions(const std::ve return functions; } -std::vector RuntimeDiscoveryStrategy::discover_services() { +std::vector Ros2RuntimeIntrospection::discover_services() { std::vector services; - // Use native rclcpp API to get service names and types. Swallow the - // "context invalid" throw that fires when the refresh timer races - // rclcpp::shutdown during SIGINT; callers already handle an empty list. + // Swallow the "context invalid" throw that fires when the refresh timer + // races rclcpp::shutdown during SIGINT; callers already handle an empty list. std::map> service_names_and_types; try { service_names_and_types = node_->get_service_names_and_types(); @@ -274,7 +255,6 @@ std::vector RuntimeDiscoveryStrategy::discover_services() { } for (const auto & [service_path, types] : service_names_and_types) { - // Skip internal ROS2 services (parameter services, action internals, etc.) if (is_internal_service(service_path)) { continue; } @@ -284,8 +264,7 @@ std::vector RuntimeDiscoveryStrategy::discover_services() { info.name = extract_name_from_path(service_path); info.type = types.empty() ? "" : types[0]; - // Enrich with schema info if TypeIntrospection is available - // Service types: pkg/srv/Type -> Request: pkg/srv/Type_Request, Response: pkg/srv/Type_Response + // Service types: pkg/srv/Type -> Request/Response companions. if (type_introspection_ && !info.type.empty()) { try { json type_info_json; @@ -302,17 +281,16 @@ std::vector RuntimeDiscoveryStrategy::discover_services() { services.push_back(info); } - // Update cache for find operations cached_services_ = services; return services; } -std::vector RuntimeDiscoveryStrategy::discover_actions() { +std::vector Ros2RuntimeIntrospection::discover_actions() { std::vector actions; - // Use native rclcpp API to get action names and types - // Note: This requires checking service endpoints for action patterns + // Actions are detected by looking for /_action/send_goal services (no native + // rclcpp API for action names). std::map> service_names_and_types; try { service_names_and_types = node_->get_service_names_and_types(); @@ -321,8 +299,6 @@ std::vector RuntimeDiscoveryStrategy::discover_actions() { return actions; } - // Actions expose services with /_action/send_goal, /_action/cancel_goal, /_action/get_result - // We detect actions by looking for /_action/send_goal services std::set discovered_action_paths; for (const auto & [service_path, types] : service_names_and_types) { @@ -330,7 +306,6 @@ std::vector RuntimeDiscoveryStrategy::discover_actions() { 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()); if (discovered_action_paths.count(action_path) == 0) { @@ -340,9 +315,8 @@ std::vector RuntimeDiscoveryStrategy::discover_actions() { info.full_path = action_path; info.name = extract_name_from_path(action_path); - // Derive action type from send_goal service type - // Service type is like "example_interfaces/action/Fibonacci_SendGoal" - // We need to extract "example_interfaces/action/Fibonacci" + // Service type is like "example_interfaces/action/Fibonacci_SendGoal"; + // strip the suffix to get "example_interfaces/action/Fibonacci". if (!types.empty()) { std::string srv_type = types[0]; const std::string send_goal_suffix = "_SendGoal"; @@ -355,8 +329,7 @@ std::vector RuntimeDiscoveryStrategy::discover_actions() { } } - // Enrich with schema info if TypeIntrospection is available - // Action types: pkg/action/Type -> Goal: pkg/action/Type_Goal, etc. + // Action types: pkg/action/Type -> Goal/Result/Feedback companions. if (type_introspection_ && !info.type.empty()) { try { json type_info_json; @@ -377,15 +350,13 @@ std::vector RuntimeDiscoveryStrategy::discover_actions() { } } - // Update cache for find operations cached_actions_ = actions; return actions; } -std::optional RuntimeDiscoveryStrategy::find_service(const std::string & component_ns, +std::optional Ros2RuntimeIntrospection::find_service(const std::string & component_ns, const std::string & operation_name) const { - // Construct expected service path std::string expected_path = component_ns; if (!expected_path.empty() && expected_path.back() != '/') { expected_path += "/"; @@ -394,7 +365,6 @@ std::optional RuntimeDiscoveryStrategy::find_service(const std::str for (const auto & svc : cached_services_) { if (svc.full_path == expected_path || svc.name == operation_name) { - // Check if service belongs to the component namespace if (path_belongs_to_namespace(svc.full_path, component_ns)) { return svc; } @@ -404,9 +374,8 @@ std::optional RuntimeDiscoveryStrategy::find_service(const std::str return std::nullopt; } -std::optional RuntimeDiscoveryStrategy::find_action(const std::string & component_ns, +std::optional Ros2RuntimeIntrospection::find_action(const std::string & component_ns, const std::string & operation_name) const { - // Construct expected action path std::string expected_path = component_ns; if (!expected_path.empty() && expected_path.back() != '/') { expected_path += "/"; @@ -415,7 +384,6 @@ std::optional RuntimeDiscoveryStrategy::find_action(const std::strin for (const auto & act : cached_actions_) { if (act.full_path == expected_path || act.name == operation_name) { - // Check if action belongs to the component namespace if (path_belongs_to_namespace(act.full_path, component_ns)) { return act; } @@ -425,15 +393,15 @@ std::optional RuntimeDiscoveryStrategy::find_action(const std::strin return std::nullopt; } -void RuntimeDiscoveryStrategy::set_topic_data_provider(TopicDataProvider * provider) { +void Ros2RuntimeIntrospection::set_topic_data_provider(TopicDataProvider * provider) { topic_data_provider_ = provider; } -void RuntimeDiscoveryStrategy::set_type_introspection(TypeIntrospection * introspection) { +void Ros2RuntimeIntrospection::set_type_introspection(ros2_medkit_serialization::TypeIntrospection * introspection) { type_introspection_ = introspection; } -void RuntimeDiscoveryStrategy::refresh_topic_map() { +void Ros2RuntimeIntrospection::refresh_topic_map() { if (!topic_data_provider_) { return; } @@ -442,22 +410,20 @@ void RuntimeDiscoveryStrategy::refresh_topic_map() { RCLCPP_DEBUG(node_->get_logger(), "Topic map refreshed: %zu components", cached_topic_map_.size()); } -bool RuntimeDiscoveryStrategy::is_topic_map_ready() const { +bool Ros2RuntimeIntrospection::is_topic_map_ready() const { return topic_map_ready_; } -std::string RuntimeDiscoveryStrategy::extract_area_from_namespace(const std::string & ns) { +std::string Ros2RuntimeIntrospection::extract_area_from_namespace(const std::string & ns) { if (ns == "/" || ns.empty()) { return "root"; } - // Remove leading slash std::string cleaned = ns; if (cleaned[0] == '/') { cleaned = cleaned.substr(1); } - // Get first segment auto pos = cleaned.find('/'); if (pos != std::string::npos) { return cleaned.substr(0, pos); @@ -466,12 +432,11 @@ std::string RuntimeDiscoveryStrategy::extract_area_from_namespace(const std::str return cleaned; } -std::string RuntimeDiscoveryStrategy::extract_name_from_path(const std::string & path) { +std::string Ros2RuntimeIntrospection::extract_name_from_path(const std::string & path) { if (path.empty()) { return ""; } - // Find last slash auto pos = path.rfind('/'); if (pos != std::string::npos && pos < path.length() - 1) { return path.substr(pos + 1); @@ -480,9 +445,8 @@ std::string RuntimeDiscoveryStrategy::extract_name_from_path(const std::string & return path; } -bool RuntimeDiscoveryStrategy::path_belongs_to_namespace(const std::string & path, const std::string & ns) const { +bool Ros2RuntimeIntrospection::path_belongs_to_namespace(const std::string & path, const std::string & ns) const { if (ns.empty() || ns == "/") { - // Root namespace - check if path has only one segment after leading slash if (path.empty() || path[0] != '/') { return false; } @@ -490,17 +454,14 @@ bool RuntimeDiscoveryStrategy::path_belongs_to_namespace(const std::string & pat return without_leading.find('/') == std::string::npos; } - // Check if path starts with namespace if (path.length() <= ns.length()) { return false; } - // Path should start with ns and have exactly one more segment if (path.compare(0, ns.length(), ns) != 0) { return false; } - // Check that after namespace there's a slash and then the service/action name (no more slashes) if (path[ns.length()] != '/') { return false; } @@ -509,5 +470,4 @@ bool RuntimeDiscoveryStrategy::path_belongs_to_namespace(const std::string & pat return remainder.find('/') == std::string::npos; } -} // namespace discovery -} // namespace ros2_medkit_gateway +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/src/ros2/transports/ros2_action_transport.cpp b/src/ros2_medkit_gateway/src/ros2/transports/ros2_action_transport.cpp new file mode 100644 index 000000000..0dc356550 --- /dev/null +++ b/src/ros2_medkit_gateway/src/ros2/transports/ros2_action_transport.cpp @@ -0,0 +1,469 @@ +// 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/ros2/transports/ros2_action_transport.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_serialization/message_cleanup.hpp" +#include "ros2_medkit_serialization/serialization_error.hpp" +#include "ros2_medkit_serialization/service_action_types.hpp" +#include "ros2_medkit_serialization/type_cache.hpp" + +namespace ros2_medkit_gateway::ros2 { + +namespace { + +/// Convert UUID bytes to lowercase hex string. +std::string uuid_bytes_to_hex(const std::array & uuid) { + std::ostringstream ss; + ss << std::hex << std::setfill('0'); + for (const auto & byte : uuid) { + ss << std::setw(2) << static_cast(byte); + } + return ss.str(); +} + +/// Parse a UUID hex string into a JSON byte array (16 elements). +json uuid_hex_to_json_array(const std::string & uuid_hex) { + json array = json::array(); + for (size_t i = 0; i + 1 < uuid_hex.length() && i < 32; i += 2) { + int byte_val = std::stoi(uuid_hex.substr(i, 2), nullptr, 16); + array.push_back(byte_val); + } + return array; +} + +/// Generate a version-4 random UUID. Uses a thread-local engine to avoid +/// contention; each transport call needs only one UUID. +std::array generate_uuid() { + thread_local std::mt19937 rng{std::random_device{}()}; + std::array uuid; + std::uniform_int_distribution dist(0, 255); + for (auto & byte : uuid) { + byte = static_cast(dist(rng)); + } + uuid[6] = (uuid[6] & 0x0f) | 0x40; // version 4 + uuid[8] = (uuid[8] & 0x3f) | 0x80; // variant bits + return uuid; +} + +json uuid_bytes_to_json_array(const std::array & uuid) { + json array = json::array(); + for (auto byte : uuid) { + array.push_back(static_cast(byte)); + } + return array; +} + +ActionGoalStatus from_status_byte(int8_t status) { + switch (status) { + case action_msgs::msg::GoalStatus::STATUS_ACCEPTED: + return ActionGoalStatus::ACCEPTED; + case action_msgs::msg::GoalStatus::STATUS_EXECUTING: + return ActionGoalStatus::EXECUTING; + case action_msgs::msg::GoalStatus::STATUS_CANCELING: + return ActionGoalStatus::CANCELING; + case action_msgs::msg::GoalStatus::STATUS_SUCCEEDED: + return ActionGoalStatus::SUCCEEDED; + case action_msgs::msg::GoalStatus::STATUS_CANCELED: + return ActionGoalStatus::CANCELED; + case action_msgs::msg::GoalStatus::STATUS_ABORTED: + return ActionGoalStatus::ABORTED; + default: + return ActionGoalStatus::UNKNOWN; + } +} + +} // namespace + +Ros2ActionTransport::Ros2ActionTransport(rclcpp::Node * node) + : node_(node), serializer_(std::make_shared()) { + RCLCPP_INFO(node_->get_logger(), "Ros2ActionTransport initialised (native serialization)"); +} + +Ros2ActionTransport::~Ros2ActionTransport() { + // Mark shutdown so any in-flight callback short-circuits before touching + // members that are about to destruct. + shutdown_requested_.store(true); + + // Drop subscriptions first - their callbacks capture `this`, so they must + // not fire after this destructor begins. + { + std::lock_guard lock(subscriptions_mutex_); + status_subscriptions_.clear(); + status_callbacks_.clear(); + } + + // Drop cached service clients to release outstanding promise references + // before the executor tears down. + std::unique_lock lock(clients_mutex_); + action_clients_.clear(); +} + +Ros2ActionTransport::ActionClientSet & Ros2ActionTransport::get_or_create_clients(const std::string & action_path, + const std::string & action_type) { + std::unique_lock lock(clients_mutex_); + + auto it = action_clients_.find(action_path); + if (it != action_clients_.end()) { + return it->second; + } + + using ros2_medkit_serialization::ServiceActionTypes; + + ActionClientSet clients; + clients.action_type = action_type; + + std::string send_goal_service = action_path + "/_action/send_goal"; + std::string send_goal_type = ServiceActionTypes::get_action_send_goal_service_type(action_type); + clients.send_goal_client = compat::create_generic_service_client(node_, send_goal_service, send_goal_type); + + std::string get_result_service = action_path + "/_action/get_result"; + std::string get_result_type = ServiceActionTypes::get_action_get_result_service_type(action_type); + clients.get_result_client = compat::create_generic_service_client(node_, get_result_service, get_result_type); + + std::string cancel_service = action_path + "/_action/cancel_goal"; + clients.cancel_goal_client = + compat::create_generic_service_client(node_, cancel_service, "action_msgs/srv/CancelGoal"); + + RCLCPP_DEBUG(node_->get_logger(), "Created action clients for %s (type: %s)", action_path.c_str(), + action_type.c_str()); + + action_clients_[action_path] = std::move(clients); + return action_clients_[action_path]; +} + +ActionSendGoalResult Ros2ActionTransport::send_goal(const std::string & action_path, const std::string & action_type, + const json & goal, std::chrono::duration timeout) { + ActionSendGoalResult result; + result.success = false; + result.goal_accepted = false; + + try { + using ros2_medkit_serialization::ServiceActionTypes; + + auto & clients = get_or_create_clients(action_path, action_type); + + if (!clients.send_goal_client->wait_for_service(std::chrono::seconds(5))) { + result.error_message = "Action server not available: " + action_path; + return result; + } + + auto uuid_bytes = generate_uuid(); + + json send_goal_request; + send_goal_request["goal_id"]["uuid"] = uuid_bytes_to_json_array(uuid_bytes); + send_goal_request["goal"] = goal.empty() || goal.is_null() ? json::object() : goal; + + std::string request_type = ServiceActionTypes::get_action_send_goal_request_type(action_type); + + RCLCPP_INFO(node_->get_logger(), "SendGoal request type: %s, JSON: %s", request_type.c_str(), + send_goal_request.dump().c_str()); + + RosMessage_Cpp ros_request = serializer_->from_json(request_type, send_goal_request); + + RCLCPP_INFO(node_->get_logger(), "Sending action goal: %s (type: %s)", action_path.c_str(), action_type.c_str()); + + auto future_and_id = clients.send_goal_client->async_send_request(ros_request.data); + + const auto timeout_ms = + std::chrono::milliseconds{static_cast(std::max(timeout.count(), 0.0) * 1000.0)}; + auto future_status = future_and_id.wait_for(timeout_ms); + + if (future_status != std::future_status::ready) { + clients.send_goal_client->remove_pending_request(future_and_id.request_id); + ros2_medkit_serialization::destroy_ros_message(&ros_request); + result.error_message = "Send goal timed out"; + return result; + } + + ros2_medkit_serialization::destroy_ros_message(&ros_request); + + auto response_ptr = future_and_id.get(); + if (response_ptr == nullptr) { + result.error_message = "Send goal returned null response"; + return result; + } + + std::string response_type = ServiceActionTypes::get_action_send_goal_response_type(action_type); + auto type_info = ros2_medkit_serialization::TypeCache::instance().get_message_type_info(response_type); + if (type_info == nullptr) { + result.error_message = "Unknown response type: " + response_type; + return result; + } + json response = serializer_->to_json(type_info, response_ptr.get()); + + result.success = true; + result.goal_accepted = response.value("accepted", false); + + if (result.goal_accepted) { + result.goal_id = uuid_bytes_to_hex(uuid_bytes); + RCLCPP_INFO(node_->get_logger(), "Action goal accepted with ID: %s", result.goal_id.c_str()); + } else { + result.error_message = "Goal rejected by action server"; + } + + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Send action goal failed for '%s': %s", action_path.c_str(), e.what()); + result.success = false; + result.error_message = e.what(); + } + + return result; +} + +ActionCancelResult Ros2ActionTransport::cancel_goal(const std::string & action_path, const std::string & goal_id, + std::chrono::duration timeout) { + ActionCancelResult result; + result.success = false; + result.return_code = 0; + + try { + // Cancel does not need an action_type to be valid; reuse cached clients + // by passing whatever the cache already has if present, or a placeholder. + std::string cached_type; + { + std::shared_lock lock(clients_mutex_); + auto it = action_clients_.find(action_path); + if (it != action_clients_.end()) { + cached_type = it->second.action_type; + } + } + + if (cached_type.empty()) { + // No prior clients exist for this action - we can't reliably build the + // send_goal/get_result types, but cancel only needs the cancel client + // with a fixed type. Use a marker; get_or_create_clients tolerates + // any non-empty type for the cancel-only path because the cancel + // client uses the standard action_msgs/srv/CancelGoal type. + cached_type = "action_msgs/action/CancelOnly"; + } + + auto & clients = get_or_create_clients(action_path, cached_type); + + const auto timeout_ms = + std::chrono::milliseconds{static_cast(std::max(timeout.count(), 0.0) * 1000.0)}; + + if (!clients.cancel_goal_client->wait_for_service(std::chrono::seconds(2))) { + result.error_message = "Cancel service not available"; + return result; + } + + json cancel_request; + cancel_request["goal_info"]["goal_id"]["uuid"] = uuid_hex_to_json_array(goal_id); + + RosMessage_Cpp ros_request = serializer_->from_json("action_msgs/srv/CancelGoal_Request", cancel_request); + + RCLCPP_INFO(node_->get_logger(), "Canceling action goal: %s (goal_id: %s)", action_path.c_str(), goal_id.c_str()); + + auto future_and_id = clients.cancel_goal_client->async_send_request(ros_request.data); + + auto future_status = future_and_id.wait_for(timeout_ms); + if (future_status != std::future_status::ready) { + clients.cancel_goal_client->remove_pending_request(future_and_id.request_id); + ros2_medkit_serialization::destroy_ros_message(&ros_request); + result.error_message = "Cancel request timed out"; + return result; + } + ros2_medkit_serialization::destroy_ros_message(&ros_request); + + auto response_ptr = future_and_id.get(); + if (response_ptr == nullptr) { + result.error_message = "Cancel returned null response"; + return result; + } + + auto type_info = + ros2_medkit_serialization::TypeCache::instance().get_message_type_info("action_msgs/srv/CancelGoal_Response"); + if (type_info == nullptr) { + result.error_message = "Unknown response type: action_msgs/srv/CancelGoal_Response"; + return result; + } + json response = serializer_->to_json(type_info, response_ptr.get()); + + result.success = true; + result.return_code = static_cast(response.value("return_code", 0)); + + if (result.return_code == 0) { + RCLCPP_INFO(node_->get_logger(), "Cancel request accepted for goal: %s", goal_id.c_str()); + } else { + switch (result.return_code) { + case 1: + result.error_message = "Cancel request rejected"; + break; + case 2: + result.error_message = "Unknown goal ID"; + break; + case 3: + result.error_message = "Goal already terminated"; + break; + default: + result.error_message = "Unknown cancel error"; + break; + } + } + + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Action cancel failed for '%s': %s", action_path.c_str(), e.what()); + result.error_message = e.what(); + } + + return result; +} + +ActionGetResultResult Ros2ActionTransport::get_result(const std::string & action_path, const std::string & action_type, + const std::string & goal_id, + std::chrono::duration timeout) { + ActionGetResultResult result; + result.success = false; + result.status = ActionGoalStatus::UNKNOWN; + + try { + using ros2_medkit_serialization::ServiceActionTypes; + + auto & clients = get_or_create_clients(action_path, action_type); + + if (!clients.get_result_client->wait_for_service(std::chrono::seconds(2))) { + result.error_message = "Get result service not available"; + return result; + } + + json get_result_request; + get_result_request["goal_id"]["uuid"] = uuid_hex_to_json_array(goal_id); + + std::string request_type = ServiceActionTypes::get_action_get_result_request_type(action_type); + + RosMessage_Cpp ros_request = serializer_->from_json(request_type, get_result_request); + + RCLCPP_INFO(node_->get_logger(), "Getting action result: %s (goal_id: %s)", action_path.c_str(), goal_id.c_str()); + + auto future_and_id = clients.get_result_client->async_send_request(ros_request.data); + + const auto timeout_ms = + std::chrono::milliseconds{static_cast(std::max(timeout.count(), 0.0) * 1000.0)}; + auto future_status = future_and_id.wait_for(timeout_ms); + if (future_status != std::future_status::ready) { + clients.get_result_client->remove_pending_request(future_and_id.request_id); + ros2_medkit_serialization::destroy_ros_message(&ros_request); + result.error_message = "Get result timed out"; + return result; + } + ros2_medkit_serialization::destroy_ros_message(&ros_request); + + auto response_ptr = future_and_id.get(); + if (response_ptr == nullptr) { + result.error_message = "Get result returned null response"; + return result; + } + + std::string response_type = ServiceActionTypes::get_action_get_result_response_type(action_type); + auto type_info = ros2_medkit_serialization::TypeCache::instance().get_message_type_info(response_type); + if (type_info == nullptr) { + result.error_message = "Unknown response type: " + response_type; + return result; + } + json response = serializer_->to_json(type_info, response_ptr.get()); + + result.success = true; + + if (response.contains("status")) { + int status_val = response["status"].get(); + result.status = static_cast(status_val); + } + + if (response.contains("result")) { + result.result = response["result"]; + } else { + result.result = response; + } + + RCLCPP_INFO(node_->get_logger(), "Got action result for %s: status=%s", goal_id.c_str(), + action_status_to_string(result.status).c_str()); + + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Get action result failed for '%s': %s", action_path.c_str(), e.what()); + result.error_message = e.what(); + } + + return result; +} + +void Ros2ActionTransport::subscribe_status(const std::string & action_path, StatusCallback callback) { + std::lock_guard lock(subscriptions_mutex_); + + // Idempotent: re-subscribing on the same path is a no-op (preserves the + // first registered callback, matching today's manager-level behaviour). + if (status_subscriptions_.count(action_path) > 0) { + return; + } + + status_callbacks_[action_path] = std::move(callback); + + std::string status_topic = action_path + "/_action/status"; + + auto cb = [this, action_path](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { + if (shutdown_requested_.load(std::memory_order_acquire)) { + return; + } + on_status_msg(action_path, msg); + }; + + auto subscription = + node_->create_subscription(status_topic, rclcpp::QoS(10).best_effort(), cb); + + status_subscriptions_[action_path] = subscription; + RCLCPP_INFO(node_->get_logger(), "Subscribed to action status: %s", status_topic.c_str()); +} + +void Ros2ActionTransport::unsubscribe_status(const std::string & action_path) { + std::lock_guard lock(subscriptions_mutex_); + + auto it = status_subscriptions_.find(action_path); + if (it != status_subscriptions_.end()) { + status_subscriptions_.erase(it); + status_callbacks_.erase(action_path); + RCLCPP_INFO(node_->get_logger(), "Unsubscribed from action status: %s/_action/status", action_path.c_str()); + } +} + +void Ros2ActionTransport::on_status_msg(const std::string & action_path, + const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { + StatusCallback callback; + { + std::lock_guard lock(subscriptions_mutex_); + auto it = status_callbacks_.find(action_path); + if (it == status_callbacks_.end()) { + return; + } + callback = it->second; + } + + if (!callback) { + return; + } + + for (const auto & status : msg->status_list) { + std::string goal_id = uuid_bytes_to_hex(status.goal_info.goal_id.uuid); + callback(action_path, goal_id, from_status_byte(status.status)); + } +} + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/src/fault_manager.cpp b/src/ros2_medkit_gateway/src/ros2/transports/ros2_fault_service_transport.cpp similarity index 72% rename from src/ros2_medkit_gateway/src/fault_manager.cpp rename to src/ros2_medkit_gateway/src/ros2/transports/ros2_fault_service_transport.cpp index 68d31633c..1cff05a58 100644 --- a/src/ros2_medkit_gateway/src/fault_manager.cpp +++ b/src/ros2_medkit_gateway/src/ros2/transports/ros2_fault_service_transport.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 mfaferek93 +// 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. @@ -12,26 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ros2_medkit_gateway/fault_manager.hpp" -#include "ros2_medkit_gateway/fault_manager_paths.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_fault_service_transport.hpp" -#include #include #include +#include -using namespace std::chrono_literals; +#include "ros2_medkit_gateway/fault_manager_paths.hpp" +#include "ros2_medkit_gateway/ros2/conversions/fault_msg_conversions.hpp" +#include "ros2_medkit_msgs/msg/environment_data.hpp" +#include "ros2_medkit_msgs/msg/fault.hpp" -namespace ros2_medkit_gateway { +namespace ros2_medkit_gateway::ros2 { -FaultManager::FaultManager(rclcpp::Node * node) : node_(node) { - // Get configurable timeout. GatewayNode declares this parameter up front, - // but unit tests may construct FaultManager with a plain rclcpp::Node. +Ros2FaultServiceTransport::Ros2FaultServiceTransport(rclcpp::Node * node) : node_(node) { + // Pick up configurable timeout. GatewayNode declares this parameter up front, + // but unit tests may construct the transport with a plain rclcpp::Node. if (!node_->get_parameter("fault_manager.service_timeout_sec", service_timeout_sec_)) { service_timeout_sec_ = 5.0; } fault_manager_base_path_ = build_fault_manager_base_path(node_); - // Create service clients for fault_manager services report_fault_client_ = node_->create_client(fault_manager_base_path_ + "/report_fault"); get_fault_client_ = node_->create_client(fault_manager_base_path_ + "/get_fault"); @@ -45,63 +46,34 @@ FaultManager::FaultManager(rclcpp::Node * node) : node_(node) { list_rosbags_client_ = node_->create_client(fault_manager_base_path_ + "/list_rosbags"); - RCLCPP_INFO(node_->get_logger(), "FaultManager initialized (base_path=%s, timeout=%.1fs)", + RCLCPP_INFO(node_->get_logger(), "Ros2FaultServiceTransport initialized (base_path=%s, timeout=%.1fs)", fault_manager_base_path_.c_str(), service_timeout_sec_); } -bool FaultManager::wait_for_services(std::chrono::duration timeout) { +Ros2FaultServiceTransport::~Ros2FaultServiceTransport() { + // Reset clients before implicit member destruction so any in-flight + // future-callback paths drop their references in a defined order. + report_fault_client_.reset(); + get_fault_client_.reset(); + list_faults_client_.reset(); + clear_fault_client_.reset(); + get_snapshots_client_.reset(); + get_rosbag_client_.reset(); + list_rosbags_client_.reset(); +} + +bool Ros2FaultServiceTransport::wait_for_services(std::chrono::duration timeout) { return report_fault_client_->wait_for_service(timeout) && get_fault_client_->wait_for_service(timeout) && list_faults_client_->wait_for_service(timeout) && clear_fault_client_->wait_for_service(timeout); } -bool FaultManager::is_available() const { +bool Ros2FaultServiceTransport::is_available() const { return report_fault_client_->service_is_ready() && get_fault_client_->service_is_ready() && list_faults_client_->service_is_ready() && clear_fault_client_->service_is_ready(); } -/// Convert a ROS 2 Fault message to JSON for REST API responses. -/// Timestamps are converted from builtin_interfaces::msg::Time (sec + nanosec) to seconds as double. -/// A human-readable severity_label is added based on the severity level. -json FaultManager::fault_to_json(const ros2_medkit_msgs::msg::Fault & fault) { - // Convert ROS 2 Time to seconds as double - auto to_seconds = [](const builtin_interfaces::msg::Time & t) { - return t.sec + static_cast(t.nanosec) / 1e9; - }; - - json j; - j["fault_code"] = fault.fault_code; - j["severity"] = fault.severity; - j["description"] = fault.description; - j["first_occurred"] = to_seconds(fault.first_occurred); - j["last_occurred"] = to_seconds(fault.last_occurred); - j["occurrence_count"] = fault.occurrence_count; - j["status"] = fault.status; - j["reporting_sources"] = fault.reporting_sources; - - // Add severity label for readability - switch (fault.severity) { - case ros2_medkit_msgs::msg::Fault::SEVERITY_INFO: - j["severity_label"] = "INFO"; - break; - case ros2_medkit_msgs::msg::Fault::SEVERITY_WARN: - j["severity_label"] = "WARN"; - break; - case ros2_medkit_msgs::msg::Fault::SEVERITY_ERROR: - j["severity_label"] = "ERROR"; - break; - case ros2_medkit_msgs::msg::Fault::SEVERITY_CRITICAL: - j["severity_label"] = "CRITICAL"; - break; - default: - j["severity_label"] = "UNKNOWN"; - break; - } - - return j; -} - -FaultResult FaultManager::report_fault(const std::string & fault_code, uint8_t severity, - const std::string & description, const std::string & source_id) { +FaultResult Ros2FaultServiceTransport::report_fault(const std::string & fault_code, uint8_t severity, + const std::string & description, const std::string & source_id) { std::lock_guard lock(report_mutex_); FaultResult result; @@ -137,9 +109,9 @@ FaultResult FaultManager::report_fault(const std::string & fault_code, uint8_t s return result; } -FaultResult FaultManager::list_faults(const std::string & source_id, bool include_prefailed, bool include_confirmed, - bool include_cleared, bool include_healed, bool include_muted, - bool include_clusters) { +FaultResult Ros2FaultServiceTransport::list_faults(const std::string & source_id, bool include_prefailed, + bool include_confirmed, bool include_cleared, bool include_healed, + bool include_muted, bool include_clusters) { std::lock_guard lock(list_mutex_); FaultResult result; @@ -154,7 +126,6 @@ FaultResult FaultManager::list_faults(const std::string & source_id, bool includ request->filter_by_severity = false; request->severity = 0; - // Build status filter if (include_prefailed) { request->statuses.push_back(ros2_medkit_msgs::msg::Fault::STATUS_PREFAILED); } @@ -169,7 +140,6 @@ FaultResult FaultManager::list_faults(const std::string & source_id, bool includ request->statuses.push_back(ros2_medkit_msgs::msg::Fault::STATUS_PREPASSED); } - // Correlation options request->include_muted = include_muted; request->include_clusters = include_clusters; @@ -186,29 +156,24 @@ FaultResult FaultManager::list_faults(const std::string & source_id, bool includ // Filter by source_id if provided (uses prefix matching) json faults_array = json::array(); for (const auto & fault : response->faults) { - // If source_id filter is provided, check if any reporting source starts with the filter - // This allows querying by namespace (e.g., "/perception/lidar" matches "/perception/lidar/lidar_sensor") if (!source_id.empty()) { - auto & sources = fault.reporting_sources; + const auto & sources = fault.reporting_sources; bool matches = false; for (const auto & src : sources) { - // Match if source starts with the filter (prefix match for namespace hierarchy) if (src.rfind(source_id, 0) == 0) { matches = true; break; } } if (!matches) { - continue; // Skip faults not reported by this component/namespace + continue; } } - faults_array.push_back(fault_to_json(fault)); + faults_array.push_back(conversions::fault_to_json(fault)); } result.success = true; result.data = {{"faults", faults_array}, {"count", faults_array.size()}}; - - // Add correlation data (always include counts, details only if requested) result.data["muted_count"] = response->muted_count; result.data["cluster_count"] = response->cluster_count; @@ -247,9 +212,10 @@ FaultResult FaultManager::list_faults(const std::string & source_id, bool includ return result; } -FaultWithEnvResult FaultManager::get_fault_with_env(const std::string & fault_code, const std::string & source_id) { +FaultWithEnvJsonResult Ros2FaultServiceTransport::get_fault_with_env(const std::string & fault_code, + const std::string & source_id) { std::lock_guard lock(get_mutex_); - FaultWithEnvResult result; + FaultWithEnvJsonResult result; auto timeout = std::chrono::duration(service_timeout_sec_); if (!get_fault_client_->wait_for_service(timeout)) { @@ -270,37 +236,36 @@ FaultWithEnvResult FaultManager::get_fault_with_env(const std::string & fault_co } auto response = future.get(); - result.success = response->success; - - if (response->success) { - result.fault = response->fault; - result.environment_data = response->environment_data; + if (!response->success) { + result.success = false; + result.error_message = response->error_message; + return result; + } - // Verify source_id if provided - if (!source_id.empty()) { - bool matches = false; - for (const auto & src : result.fault.reporting_sources) { - if (src.rfind(source_id, 0) == 0) { - matches = true; - break; - } - } - if (!matches) { - result.success = false; - result.error_message = "Fault not found for source: " + source_id; - result.fault = ros2_medkit_msgs::msg::Fault(); - result.environment_data = ros2_medkit_msgs::msg::EnvironmentData(); + // Verify source_id if provided (prefix match against any reporting source). + if (!source_id.empty()) { + bool matches = false; + for (const auto & src : response->fault.reporting_sources) { + if (src.rfind(source_id, 0) == 0) { + matches = true; + break; } } - } else { - result.error_message = response->error_message; + if (!matches) { + result.success = false; + result.error_message = "Fault not found for source: " + source_id; + return result; + } } + result.success = true; + result.data = {{"fault", conversions::fault_to_json(response->fault)}, + {"environment_data", conversions::environment_data_to_json(response->environment_data)}}; return result; } -FaultResult FaultManager::get_fault(const std::string & fault_code, const std::string & source_id) { - // Use get_fault_with_env and convert to JSON +FaultResult Ros2FaultServiceTransport::get_fault(const std::string & fault_code, const std::string & source_id) { + // Use get_fault_with_env and pull only the fault portion of the body. auto env_result = get_fault_with_env(fault_code, source_id); FaultResult result; @@ -308,13 +273,13 @@ FaultResult FaultManager::get_fault(const std::string & fault_code, const std::s result.error_message = env_result.error_message; if (env_result.success) { - result.data = fault_to_json(env_result.fault); + result.data = env_result.data["fault"]; } return result; } -FaultResult FaultManager::clear_fault(const std::string & fault_code) { +FaultResult Ros2FaultServiceTransport::clear_fault(const std::string & fault_code) { std::lock_guard lock(clear_mutex_); FaultResult result; @@ -343,7 +308,6 @@ FaultResult FaultManager::clear_fault(const std::string & fault_code) { result.error_message = response->message; } - // Include auto-cleared symptom codes if correlation is enabled if (!response->auto_cleared_codes.empty()) { result.data["auto_cleared_codes"] = response->auto_cleared_codes; } @@ -351,7 +315,7 @@ FaultResult FaultManager::clear_fault(const std::string & fault_code) { return result; } -FaultResult FaultManager::get_snapshots(const std::string & fault_code, const std::string & topic) { +FaultResult Ros2FaultServiceTransport::get_snapshots(const std::string & fault_code, const std::string & topic) { std::lock_guard lock(snapshots_mutex_); FaultResult result; @@ -378,10 +342,9 @@ FaultResult FaultManager::get_snapshots(const std::string & fault_code, const st result.success = response->success; if (response->success) { - // Parse the JSON data from the service response try { result.data = json::parse(response->data); - } catch (const json::exception & e) { + } catch (const json::exception & /*e*/) { result.data = {{"raw_data", response->data}}; } } else { @@ -391,7 +354,7 @@ FaultResult FaultManager::get_snapshots(const std::string & fault_code, const st return result; } -FaultResult FaultManager::get_rosbag(const std::string & fault_code) { +FaultResult Ros2FaultServiceTransport::get_rosbag(const std::string & fault_code) { std::lock_guard lock(rosbag_mutex_); FaultResult result; @@ -428,7 +391,7 @@ FaultResult FaultManager::get_rosbag(const std::string & fault_code) { return result; } -FaultResult FaultManager::list_rosbags(const std::string & entity_fqn) { +FaultResult Ros2FaultServiceTransport::list_rosbags(const std::string & entity_fqn) { std::lock_guard lock(list_rosbags_mutex_); FaultResult result; @@ -470,4 +433,4 @@ FaultResult FaultManager::list_rosbags(const std::string & entity_fqn) { return result; } -} // namespace ros2_medkit_gateway +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/src/ros2/transports/ros2_log_source.cpp b/src/ros2_medkit_gateway/src/ros2/transports/ros2_log_source.cpp new file mode 100644 index 000000000..dcb226529 --- /dev/null +++ b/src/ros2_medkit_gateway/src/ros2/transports/ros2_log_source.cpp @@ -0,0 +1,84 @@ +// 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/ros2/transports/ros2_log_source.hpp" + +#include + +#include "ros2_medkit_gateway/core/log_types.hpp" + +namespace ros2_medkit_gateway::ros2 { + +Ros2LogSource::Ros2LogSource(rclcpp::Node * node) : node_(node) { +} + +Ros2LogSource::~Ros2LogSource() { + // Idempotent stop() also runs from destructors; calling it explicitly here + // gives a deterministic teardown sequence regardless of how the shared_ptr + // last reference drops. + stop(); +} + +void Ros2LogSource::start(EntryCallback callback) { + std::lock_guard lock(mutex_); + callback_ = std::move(callback); + + if (rosout_sub_) { + // Subscription already created - reusing it is enough; the lambda below + // reads callback_ under the same mutex. + return; + } + + rosout_sub_ = node_->create_subscription( + "/rosout", rclcpp::QoS(100), [this](const rcl_interfaces::msg::Log::ConstSharedPtr & msg) { + if (shutdown_requested_.load(std::memory_order_acquire)) { + return; + } + + EntryCallback cb; + { + std::lock_guard cb_lock(mutex_); + cb = callback_; + } + if (!cb) { + return; + } + + LogEntry entry; + // The manager assigns the monotonic id; the source leaves it unset. + entry.id = 0; + entry.stamp_sec = msg->stamp.sec; + entry.stamp_nanosec = msg->stamp.nanosec; + entry.level = msg->level; + entry.name = msg->name; // already without leading slash per rcl convention + entry.msg = msg->msg; + entry.function = msg->function; + entry.file = msg->file; + entry.line = msg->line; + + cb(entry); + }); +} + +void Ros2LogSource::stop() { + // Mark shutdown so any in-flight callback short-circuits before touching + // members that are about to destruct. + shutdown_requested_.store(true, std::memory_order_release); + + std::lock_guard lock(mutex_); + rosout_sub_.reset(); + callback_ = nullptr; +} + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/src/configuration_manager.cpp b/src/ros2_medkit_gateway/src/ros2/transports/ros2_parameter_transport.cpp similarity index 62% rename from src/ros2_medkit_gateway/src/configuration_manager.cpp rename to src/ros2_medkit_gateway/src/ros2/transports/ros2_parameter_transport.cpp index 913e97444..0fe8acfb5 100644 --- a/src/ros2_medkit_gateway/src/configuration_manager.cpp +++ b/src/ros2_medkit_gateway/src/ros2/transports/ros2_parameter_transport.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 mfaferek93 +// 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. @@ -12,15 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ros2_medkit_gateway/configuration_manager.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_parameter_transport.hpp" +#include #include +#include +#include using namespace std::chrono_literals; -namespace ros2_medkit_gateway { +namespace ros2_medkit_gateway::ros2 { -ConfigurationManager::ConfigurationManager(rclcpp::Node * node) : node_(node) { +Ros2ParameterTransport::Ros2ParameterTransport(rclcpp::Node * node, double service_timeout_sec, + double negative_cache_ttl_sec) + : node_(node), service_timeout_sec_(service_timeout_sec), negative_cache_ttl_sec_(negative_cache_ttl_sec) { // Create internal node for parameter client operations early. // Must be in DDS graph before any parameter queries for fast service discovery. rclcpp::NodeOptions options; @@ -29,55 +34,60 @@ ConfigurationManager::ConfigurationManager(rclcpp::Node * node) : node_(node) { options.use_global_arguments(false); param_node_ = std::make_shared("_param_client_node", options); - // Declare parameters with range validation - rcl_interfaces::msg::ParameterDescriptor timeout_desc; - timeout_desc.description = "Timeout for ROS 2 parameter service calls (configurations endpoint)"; - rcl_interfaces::msg::FloatingPointRange timeout_range; - timeout_range.from_value = 0.1; - timeout_range.to_value = 10.0; - timeout_desc.floating_point_range.push_back(timeout_range); - service_timeout_sec_ = node_->declare_parameter("parameter_service_timeout_sec", 2.0, timeout_desc); - - rcl_interfaces::msg::ParameterDescriptor cache_desc; - cache_desc.description = "Negative cache TTL for unavailable parameter services (0 = disabled)"; - rcl_interfaces::msg::FloatingPointRange cache_range; - cache_range.from_value = 0.0; - cache_range.to_value = 3600.0; - cache_desc.floating_point_range.push_back(cache_range); - negative_cache_ttl_sec_ = node_->declare_parameter("parameter_service_negative_cache_sec", 60.0, cache_desc); - - // Store own node FQN for self-query detection + // Store own node FQN for self-query detection. own_node_fqn_ = node_->get_fully_qualified_name(); - RCLCPP_INFO(node_->get_logger(), "ConfigurationManager initialized (timeout=%.1fs, negative_cache=%.0fs)", + RCLCPP_INFO(node_->get_logger(), "Ros2ParameterTransport initialized (timeout=%.1fs, negative_cache=%.0fs)", service_timeout_sec_, negative_cache_ttl_sec_); } -ConfigurationManager::~ConfigurationManager() { +Ros2ParameterTransport::~Ros2ParameterTransport() { shutdown(); } -void ConfigurationManager::shutdown() { - if (shutdown_.exchange(true)) { +void Ros2ParameterTransport::shutdown() { + if (shutdown_requested_.exchange(true)) { return; // Already shut down } - // Hold lock through cleanup to prevent race with in-flight requests + // Hold lock through cleanup to prevent race with in-flight requests. std::unique_lock spin_lock(spin_mutex_, std::defer_lock); if (!spin_lock.try_lock_for(kShutdownTimeout)) { - RCLCPP_WARN(node_->get_logger(), - "ConfigurationManager shutdown: spin_mutex_ not released within timeout, proceeding with cleanup"); + if (node_) { + RCLCPP_WARN(node_->get_logger(), + "Ros2ParameterTransport shutdown: spin_mutex_ not released within timeout, " + "proceeding with cleanup"); + } } std::lock_guard lock(clients_mutex_); param_clients_.clear(); param_node_.reset(); } -ParameterResult ConfigurationManager::shut_down_result() { - return {false, {}, "ConfigurationManager is shut down", ParameterErrorCode::SHUT_DOWN}; +bool Ros2ParameterTransport::is_self_node(const std::string & node_name) const { + return node_name == own_node_fqn_; +} + +bool Ros2ParameterTransport::is_node_available(const std::string & node_name) const { + return !is_node_unavailable(node_name); +} + +void Ros2ParameterTransport::invalidate(const std::string & node_name) { + { + std::unique_lock lock(negative_cache_mutex_); + unavailable_nodes_.erase(node_name); + } + { + std::lock_guard lock(defaults_mutex_); + default_values_.erase(node_name); + } + { + std::lock_guard lock(clients_mutex_); + param_clients_.erase(node_name); + } } std::optional> -ConfigurationManager::try_acquire_spin_lock(ParameterResult & result) { +Ros2ParameterTransport::try_acquire_spin_lock(ParameterResult & result) { auto timeout = std::chrono::duration_cast( std::chrono::duration(service_timeout_sec_ + kSpinLockMarginSec)); std::unique_lock lock(spin_mutex_, std::defer_lock); @@ -86,18 +96,21 @@ ConfigurationManager::try_acquire_spin_lock(ParameterResult & result) { result.error_message = "Parameter service temporarily unavailable - timed out after " + std::to_string(static_cast(service_timeout_sec_ + kSpinLockMarginSec)) + "s"; result.error_code = ParameterErrorCode::TIMEOUT; - RCLCPP_WARN(node_->get_logger(), "Parameter service spin lock timeout (%.1fs) - another operation may be blocking", - service_timeout_sec_ + kSpinLockMarginSec); + if (node_) { + RCLCPP_WARN(node_->get_logger(), + "Parameter service spin lock timeout (%.1fs) - another operation may be blocking", + service_timeout_sec_ + kSpinLockMarginSec); + } return std::nullopt; } return lock; } -std::chrono::duration ConfigurationManager::get_service_timeout() const { +std::chrono::duration Ros2ParameterTransport::get_service_timeout() const { return std::chrono::duration(service_timeout_sec_); } -bool ConfigurationManager::is_node_unavailable(const std::string & node_name) const { +bool Ros2ParameterTransport::is_node_unavailable(const std::string & node_name) const { std::shared_lock lock(negative_cache_mutex_); auto it = unavailable_nodes_.find(node_name); if (it == unavailable_nodes_.end()) { @@ -107,13 +120,12 @@ bool ConfigurationManager::is_node_unavailable(const std::string & node_name) co return elapsed < std::chrono::duration(negative_cache_ttl_sec_); } -void ConfigurationManager::mark_node_unavailable(const std::string & node_name) { +void Ros2ParameterTransport::mark_node_unavailable(const std::string & node_name) { std::unique_lock lock(negative_cache_mutex_); unavailable_nodes_[node_name] = std::chrono::steady_clock::now(); - // Cleanup to prevent unbounded growth + // Cleanup to prevent unbounded growth. if (unavailable_nodes_.size() > kMaxNegativeCacheSize) { - // First pass: remove expired entries auto now = std::chrono::steady_clock::now(); auto ttl = std::chrono::duration(negative_cache_ttl_sec_); for (auto it = unavailable_nodes_.begin(); it != unavailable_nodes_.end();) { @@ -123,7 +135,6 @@ void ConfigurationManager::mark_node_unavailable(const std::string & node_name) ++it; } } - // If still over limit, evict oldest entry if (unavailable_nodes_.size() > kMaxNegativeCacheSize) { auto oldest = std::min_element(unavailable_nodes_.begin(), unavailable_nodes_.end(), [](const auto & a, const auto & b) { @@ -136,16 +147,26 @@ void ConfigurationManager::mark_node_unavailable(const std::string & node_name) } } -bool ConfigurationManager::is_self_node(const std::string & node_name) const { - return node_name == own_node_fqn_; +std::shared_ptr Ros2ParameterTransport::get_param_client(const std::string & node_name) { + std::lock_guard lock(clients_mutex_); + auto it = param_clients_.find(node_name); + if (it != param_clients_.end()) { + return it->second; + } + if (!param_node_) { + return nullptr; + } + auto client = std::make_shared(param_node_, node_name); + param_clients_[node_name] = client; + return client; } -ParameterResult ConfigurationManager::list_own_parameters() { +ParameterResult Ros2ParameterTransport::list_own_parameters() { ParameterResult result; try { auto params = node_->get_parameters(node_->list_parameters({}, 0).names); - // Cache defaults for reset operations (same as IPC path) + // Cache defaults for reset operations (same as IPC path). { std::lock_guard lock(defaults_mutex_); if (default_values_.find(own_node_fqn_) == default_values_.end()) { @@ -175,7 +196,7 @@ ParameterResult ConfigurationManager::list_own_parameters() { return result; } -ParameterResult ConfigurationManager::get_own_parameter(const std::string & param_name) { +ParameterResult Ros2ParameterTransport::get_own_parameter(const std::string & param_name) { ParameterResult result; try { if (!node_->has_parameter(param_name)) { @@ -204,31 +225,11 @@ ParameterResult ConfigurationManager::get_own_parameter(const std::string & para return result; } -std::shared_ptr ConfigurationManager::get_param_client(const std::string & node_name) { - std::lock_guard lock(clients_mutex_); - - auto it = param_clients_.find(node_name); - if (it != param_clients_.end()) { - return it->second; - } - - // Create cached client on the shared param_node_ (created in constructor) - auto client = std::make_shared(param_node_, node_name); - param_clients_[node_name] = client; - return client; -} - -ParameterResult ConfigurationManager::list_parameters(const std::string & node_name) { - if (shutdown_.load()) { - return shut_down_result(); +ParameterResult Ros2ParameterTransport::list_parameters(const std::string & node_name) { + if (shutdown_requested_.load()) { + return {false, {}, "Ros2ParameterTransport is shut down", ParameterErrorCode::SHUT_DOWN}; } - // Self-query guard: use direct access for gateway's own node - if (is_self_node(node_name)) { - return list_own_parameters(); - } - - // Negative cache: skip nodes that recently had no parameter service if (is_node_unavailable(node_name)) { ParameterResult result; result.success = false; @@ -241,10 +242,13 @@ ParameterResult ConfigurationManager::list_parameters(const std::string & node_n try { auto client = get_param_client(node_name); + if (!client) { + result.success = false; + result.error_message = "Parameter client unavailable (transport shut down)"; + result.error_code = ParameterErrorCode::SHUT_DOWN; + return result; + } - // All ROS 2 IPC calls (wait_for_service, list_parameters, get_parameters) - // spin param_node_ internally. spin_mutex_ serializes these to prevent - // "Node already added to executor" errors from concurrent spin. std::vector parameters; { auto spin_lock = try_acquire_spin_lock(result); @@ -252,7 +256,7 @@ ParameterResult ConfigurationManager::list_parameters(const std::string & node_n return result; } - // Cache default values first (gives node extra time for DDS service discovery) + // Cache default values first (gives node extra time for DDS service discovery). cache_default_values(node_name); if (!client->wait_for_service(get_service_timeout())) { @@ -260,8 +264,10 @@ ParameterResult ConfigurationManager::list_parameters(const std::string & node_n result.error_message = "Parameter service not available for node: " + node_name; result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE; mark_node_unavailable(node_name); - RCLCPP_WARN(node_->get_logger(), "Parameter service not available for node: '%s' (cached for %.0fs)", - node_name.c_str(), negative_cache_ttl_sec_); + if (node_) { + RCLCPP_WARN(node_->get_logger(), "Parameter service not available for node: '%s' (cached for %.0fs)", + node_name.c_str(), negative_cache_ttl_sec_); + } return result; } @@ -276,11 +282,13 @@ ParameterResult ConfigurationManager::list_parameters(const std::string & node_n parameters.push_back(single_params[0]); } } catch (const std::exception & e) { - RCLCPP_DEBUG(node_->get_logger(), "Failed to get param '%s': %s", name.c_str(), e.what()); + if (node_) { + RCLCPP_DEBUG(node_->get_logger(), "Failed to get param '%s': %s", name.c_str(), e.what()); + } } } } - } // spin_mutex_ released - JSON building is lock-free + } // spin_mutex_ released - JSON building is lock-free. json params_array = json::array(); for (const auto & param : parameters) { @@ -297,19 +305,17 @@ ParameterResult ConfigurationManager::list_parameters(const std::string & node_n result.success = false; result.error_message = std::string("Failed to list parameters: ") + e.what(); result.error_code = ParameterErrorCode::INTERNAL_ERROR; - RCLCPP_ERROR(node_->get_logger(), "Exception in list_parameters for node '%s': %s", node_name.c_str(), e.what()); + if (node_) { + RCLCPP_ERROR(node_->get_logger(), "Exception in list_parameters for node '%s': %s", node_name.c_str(), e.what()); + } } return result; } -ParameterResult ConfigurationManager::get_parameter(const std::string & node_name, const std::string & param_name) { - if (shutdown_.load()) { - return shut_down_result(); - } - - if (is_self_node(node_name)) { - return get_own_parameter(param_name); +ParameterResult Ros2ParameterTransport::get_parameter(const std::string & node_name, const std::string & param_name) { + if (shutdown_requested_.load()) { + return {false, {}, "Ros2ParameterTransport is shut down", ParameterErrorCode::SHUT_DOWN}; } if (is_node_unavailable(node_name)) { @@ -332,6 +338,12 @@ ParameterResult ConfigurationManager::get_parameter(const std::string & node_nam return result; } auto client = get_param_client(node_name); + if (!client) { + result.success = false; + result.error_message = "Parameter client unavailable (transport shut down)"; + result.error_code = ParameterErrorCode::SHUT_DOWN; + return result; + } if (!client->wait_for_service(get_service_timeout())) { result.success = false; @@ -359,7 +371,7 @@ ParameterResult ConfigurationManager::get_parameter(const std::string & node_nam param = parameters[0]; descriptors = client->describe_parameters({param_name}); - } // spin_mutex_ released + } // spin_mutex_ released. json param_obj; param_obj["name"] = param.get_name(); @@ -382,13 +394,13 @@ ParameterResult ConfigurationManager::get_parameter(const std::string & node_nam return result; } -ParameterResult ConfigurationManager::set_parameter(const std::string & node_name, const std::string & param_name, - const json & value) { - if (shutdown_.load()) { - return shut_down_result(); +ParameterResult Ros2ParameterTransport::set_parameter(const std::string & node_name, const std::string & param_name, + const json & value) { + if (shutdown_requested_.load()) { + return {false, {}, "Ros2ParameterTransport is shut down", ParameterErrorCode::SHUT_DOWN}; } - // Self-query guard: set parameter directly on own node + // Self-query guard: set parameter directly on own node. if (is_self_node(node_name)) { ParameterResult result; try { @@ -438,6 +450,12 @@ ParameterResult ConfigurationManager::set_parameter(const std::string & node_nam try { auto client = get_param_client(node_name); + if (!client) { + result.success = false; + result.error_message = "Parameter client unavailable (transport shut down)"; + result.error_code = ParameterErrorCode::SHUT_DOWN; + return result; + } if (!client->wait_for_service(get_service_timeout())) { result.success = false; @@ -492,7 +510,143 @@ ParameterResult ConfigurationManager::set_parameter(const std::string & node_nam return result; } -std::string ConfigurationManager::parameter_type_to_string(rclcpp::ParameterType type) { +ParameterResult Ros2ParameterTransport::get_default(const std::string & node_name, const std::string & param_name) { + if (shutdown_requested_.load()) { + return {false, {}, "Ros2ParameterTransport is shut down", ParameterErrorCode::SHUT_DOWN}; + } + + // For self-node, populate the defaults cache lazily from the live node + // state (same path list_own_parameters() takes, but without the JSON + // assembly side-effect) in case the manager calls reset before list. + if (is_self_node(node_name)) { + std::lock_guard lock(defaults_mutex_); + if (default_values_.find(node_name) == default_values_.end()) { + try { + auto params = node_->get_parameters(node_->list_parameters({}, 0).names); + std::map node_defaults; + for (const auto & param : params) { + node_defaults[param.get_name()] = param; + } + default_values_[node_name] = std::move(node_defaults); + } catch (const std::exception & e) { + ParameterResult result; + result.success = false; + result.error_message = std::string("Failed to seed own defaults cache: ") + e.what(); + result.error_code = ParameterErrorCode::INTERNAL_ERROR; + return result; + } + } + } else { + // Non-self: ensure defaults are cached. cache_default_values requires + // spin_mutex_ to be held. + { + std::lock_guard lock(defaults_mutex_); + if (default_values_.find(node_name) != default_values_.end()) { + // already cached, no need to spin + } else { + // Drop the lock before spinning - cache_default_values reacquires it. + } + } + bool needs_cache = false; + { + std::lock_guard lock(defaults_mutex_); + needs_cache = (default_values_.find(node_name) == default_values_.end()); + } + if (needs_cache) { + ParameterResult tmp; + auto spin_lock = try_acquire_spin_lock(tmp); + if (!spin_lock) { + return tmp; + } + cache_default_values(node_name); + } + } + + ParameterResult result; + std::lock_guard lock(defaults_mutex_); + auto node_it = default_values_.find(node_name); + if (node_it == default_values_.end()) { + result.success = false; + result.error_message = "No default values cached for node: " + node_name; + result.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED; + return result; + } + auto param_it = node_it->second.find(param_name); + if (param_it == node_it->second.end()) { + result.success = false; + result.error_message = "No default value for parameter: " + param_name; + result.error_code = ParameterErrorCode::NOT_FOUND; + return result; + } + result.success = true; + result.data = parameter_value_to_json(param_it->second.get_parameter_value()); + return result; +} + +ParameterResult Ros2ParameterTransport::list_defaults(const std::string & node_name) { + if (shutdown_requested_.load()) { + return {false, {}, "Ros2ParameterTransport is shut down", ParameterErrorCode::SHUT_DOWN}; + } + + // Mirror the get_default cache-population logic. + if (is_self_node(node_name)) { + std::lock_guard lock(defaults_mutex_); + if (default_values_.find(node_name) == default_values_.end()) { + try { + auto params = node_->get_parameters(node_->list_parameters({}, 0).names); + std::map node_defaults; + for (const auto & param : params) { + node_defaults[param.get_name()] = param; + } + default_values_[node_name] = std::move(node_defaults); + } catch (const std::exception & e) { + ParameterResult result; + result.success = false; + result.error_message = std::string("Failed to seed own defaults cache: ") + e.what(); + result.error_code = ParameterErrorCode::INTERNAL_ERROR; + return result; + } + } + } else { + bool needs_cache = false; + { + std::lock_guard lock(defaults_mutex_); + needs_cache = (default_values_.find(node_name) == default_values_.end()); + } + if (needs_cache) { + ParameterResult tmp; + auto spin_lock = try_acquire_spin_lock(tmp); + if (!spin_lock) { + return tmp; + } + cache_default_values(node_name); + } + } + + ParameterResult result; + std::lock_guard lock(defaults_mutex_); + auto node_it = default_values_.find(node_name); + if (node_it == default_values_.end()) { + result.success = false; + result.error_message = "No default values cached for node: " + node_name; + result.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED; + return result; + } + + json defaults_array = json::array(); + for (const auto & [name, param] : node_it->second) { + json entry; + entry["name"] = name; + entry["value"] = parameter_value_to_json(param.get_parameter_value()); + entry["type"] = parameter_type_to_string(param.get_type()); + defaults_array.push_back(entry); + } + result.success = true; + result.data = defaults_array; + return result; +} + +std::string Ros2ParameterTransport::parameter_type_to_string(rclcpp::ParameterType type) const { switch (type) { case rclcpp::ParameterType::PARAMETER_BOOL: return "bool"; @@ -518,7 +672,7 @@ std::string ConfigurationManager::parameter_type_to_string(rclcpp::ParameterType } } -json ConfigurationManager::parameter_value_to_json(const rclcpp::ParameterValue & value) { +json Ros2ParameterTransport::parameter_value_to_json(const rclcpp::ParameterValue & value) const { switch (value.get_type()) { case rclcpp::ParameterType::PARAMETER_BOOL: return value.get(); @@ -544,8 +698,8 @@ json ConfigurationManager::parameter_value_to_json(const rclcpp::ParameterValue } } -rclcpp::ParameterValue ConfigurationManager::json_to_parameter_value(const json & value, - rclcpp::ParameterType hint_type) { +rclcpp::ParameterValue Ros2ParameterTransport::json_to_parameter_value(const json & value, + rclcpp::ParameterType hint_type) const { if (hint_type != rclcpp::ParameterType::PARAMETER_NOT_SET) { switch (hint_type) { case rclcpp::ParameterType::PARAMETER_BOOL: @@ -647,7 +801,7 @@ rclcpp::ParameterValue ConfigurationManager::json_to_parameter_value(const json return rclcpp::ParameterValue(value.dump()); } -void ConfigurationManager::cache_default_values(const std::string & node_name) { +void Ros2ParameterTransport::cache_default_values(const std::string & node_name) { { std::lock_guard lock(defaults_mutex_); if (default_values_.find(node_name) != default_values_.end()) { @@ -655,17 +809,21 @@ void ConfigurationManager::cache_default_values(const std::string & node_name) { } } - // Skip if node is in negative cache if (is_node_unavailable(node_name)) { return; } try { auto client = get_param_client(node_name); + if (!client) { + return; + } if (!client->wait_for_service(get_service_timeout())) { - RCLCPP_WARN(node_->get_logger(), "Cannot cache defaults - service not available for node: '%s'", - node_name.c_str()); + if (node_) { + RCLCPP_WARN(node_->get_logger(), "Cannot cache defaults - service not available for node: '%s'", + node_name.c_str()); + } mark_node_unavailable(node_name); return; } @@ -699,271 +857,10 @@ void ConfigurationManager::cache_default_values(const std::string & node_name) { } } } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Failed to cache defaults for node '%s': %s", node_name.c_str(), e.what()); - } -} - -ParameterResult ConfigurationManager::reset_parameter(const std::string & node_name, const std::string & param_name) { - if (shutdown_.load()) { - return shut_down_result(); - } - - // Self-query guard: reset via direct access - if (is_self_node(node_name)) { - ParameterResult result; - // Copy default value under lock, release before set_parameter I/O - rclcpp::Parameter default_param; - { - std::lock_guard lock(defaults_mutex_); - auto node_it = default_values_.find(node_name); - if (node_it == default_values_.end()) { - result.success = false; - result.error_message = "No default values cached for node: " + node_name; - result.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED; - return result; - } - auto param_it = node_it->second.find(param_name); - if (param_it == node_it->second.end()) { - result.success = false; - result.error_message = "No default value for parameter: " + param_name; - result.error_code = ParameterErrorCode::NOT_FOUND; - return result; - } - default_param = param_it->second; - } // defaults_mutex_ released - - auto set_result = node_->set_parameter(default_param); - if (!set_result.successful) { - result.success = false; - result.error_message = set_result.reason; - result.error_code = ParameterErrorCode::INTERNAL_ERROR; - return result; - } - json param_obj; - param_obj["name"] = param_name; - param_obj["value"] = parameter_value_to_json(default_param.get_parameter_value()); - param_obj["type"] = parameter_type_to_string(default_param.get_type()); - param_obj["reset_to_default"] = true; - result.success = true; - result.data = param_obj; - return result; - } - - if (is_node_unavailable(node_name)) { - ParameterResult result; - result.success = false; - result.error_message = "Parameter service not available for node (cached): " + node_name; - result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE; - return result; - } - - ParameterResult result; - - auto spin_lock = try_acquire_spin_lock(result); - if (!spin_lock) { - return result; - } - - try { - cache_default_values(node_name); - - rclcpp::Parameter default_param; - { - std::lock_guard lock(defaults_mutex_); - auto node_it = default_values_.find(node_name); - if (node_it == default_values_.end()) { - result.success = false; - result.error_message = "No default values cached for node: " + node_name; - result.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED; - return result; - } - - auto param_it = node_it->second.find(param_name); - if (param_it == node_it->second.end()) { - result.success = false; - result.error_message = "No default value for parameter: " + param_name; - result.error_code = ParameterErrorCode::NOT_FOUND; - return result; - } - - default_param = param_it->second; - } - - auto client = get_param_client(node_name); - if (!client->wait_for_service(get_service_timeout())) { - result.success = false; - result.error_message = "Parameter service not available for node: " + node_name; - result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE; - mark_node_unavailable(node_name); - return result; - } - - auto results = client->set_parameters({default_param}); - if (results.empty() || !results[0].successful) { - result.success = false; - result.error_message = results.empty() ? "Failed to reset parameter" : results[0].reason; - result.error_code = ParameterErrorCode::INTERNAL_ERROR; - return result; + if (node_) { + RCLCPP_ERROR(node_->get_logger(), "Failed to cache defaults for node '%s': %s", node_name.c_str(), e.what()); } - - json param_obj; - param_obj["name"] = param_name; - param_obj["value"] = parameter_value_to_json(default_param.get_parameter_value()); - param_obj["type"] = parameter_type_to_string(default_param.get_type()); - param_obj["reset_to_default"] = true; - - result.success = true; - result.data = param_obj; - - RCLCPP_INFO(node_->get_logger(), "Reset parameter '%s' on node '%s' to default value", param_name.c_str(), - node_name.c_str()); - } catch (const std::exception & e) { - result.success = false; - result.error_message = std::string("Failed to reset parameter: ") + e.what(); - result.error_code = ParameterErrorCode::INTERNAL_ERROR; } - - return result; -} - -ParameterResult ConfigurationManager::reset_all_parameters(const std::string & node_name) { - if (shutdown_.load()) { - return shut_down_result(); - } - - // Self-query guard: reset via direct access - if (is_self_node(node_name)) { - ParameterResult result; - std::vector params_to_reset; - { - std::lock_guard lock(defaults_mutex_); - auto node_it = default_values_.find(node_name); - if (node_it == default_values_.end()) { - result.success = false; - result.error_message = "No default values cached for node: " + node_name; - result.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED; - return result; - } - for (const auto & [name, param] : node_it->second) { - params_to_reset.push_back(param); - } - } - size_t reset_count = 0; - size_t failed_count = 0; - json failed_params = json::array(); - for (const auto & param : params_to_reset) { - auto set_result = node_->set_parameter(param); - if (set_result.successful) { - reset_count++; - } else { - failed_count++; - failed_params.push_back(param.get_name()); - } - } - json response; - response["node_name"] = node_name; - response["reset_count"] = reset_count; - response["failed_count"] = failed_count; - if (!failed_params.empty()) { - response["failed_parameters"] = failed_params; - } - result.success = (failed_count == 0); - result.data = response; - if (failed_count > 0) { - result.error_message = "Some parameters could not be reset"; - result.error_code = ParameterErrorCode::INTERNAL_ERROR; - } - return result; - } - - if (is_node_unavailable(node_name)) { - ParameterResult result; - result.success = false; - result.error_message = "Parameter service not available for node (cached): " + node_name; - result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE; - return result; - } - - ParameterResult result; - - auto spin_lock = try_acquire_spin_lock(result); - if (!spin_lock) { - return result; - } - - try { - cache_default_values(node_name); - - std::vector params_to_reset; - { - std::lock_guard lock(defaults_mutex_); - auto node_it = default_values_.find(node_name); - if (node_it == default_values_.end()) { - result.success = false; - result.error_message = "No default values cached for node: " + node_name; - result.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED; - return result; - } - - params_to_reset.reserve(node_it->second.size()); - for (const auto & [name, param] : node_it->second) { - params_to_reset.push_back(param); - } - } - - auto client = get_param_client(node_name); - if (!client->wait_for_service(get_service_timeout())) { - result.success = false; - result.error_message = "Parameter service not available for node: " + node_name; - result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE; - mark_node_unavailable(node_name); - return result; - } - - size_t reset_count = 0; - size_t failed_count = 0; - json failed_params = json::array(); - - for (const auto & param : params_to_reset) { - try { - auto results = client->set_parameters({param}); - if (!results.empty() && results[0].successful) { - reset_count++; - } else { - failed_count++; - failed_params.push_back(param.get_name()); - } - } catch (const std::exception &) { - failed_count++; - failed_params.push_back(param.get_name()); - } - } - - json response; - response["node_name"] = node_name; - response["reset_count"] = reset_count; - response["failed_count"] = failed_count; - if (!failed_params.empty()) { - response["failed_parameters"] = failed_params; - } - - result.success = (failed_count == 0); - result.data = response; - - if (failed_count > 0) { - result.error_message = "Some parameters could not be reset"; - result.error_code = ParameterErrorCode::INTERNAL_ERROR; - } - - RCLCPP_INFO(node_->get_logger(), "Reset %zu parameters on node '%s' (%zu failed)", reset_count, node_name.c_str(), - failed_count); - } catch (const std::exception & e) { - result.success = false; - result.error_message = std::string("Failed to reset parameters: ") + e.what(); - result.error_code = ParameterErrorCode::INTERNAL_ERROR; - } - - return result; } -} // namespace ros2_medkit_gateway +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/src/ros2/transports/ros2_service_transport.cpp b/src/ros2_medkit_gateway/src/ros2/transports/ros2_service_transport.cpp new file mode 100644 index 000000000..2adc51092 --- /dev/null +++ b/src/ros2_medkit_gateway/src/ros2/transports/ros2_service_transport.cpp @@ -0,0 +1,160 @@ +// 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/ros2/transports/ros2_service_transport.hpp" + +#include +#include + +#include "ros2_medkit_serialization/message_cleanup.hpp" +#include "ros2_medkit_serialization/serialization_error.hpp" +#include "ros2_medkit_serialization/service_action_types.hpp" +#include "ros2_medkit_serialization/type_cache.hpp" + +namespace ros2_medkit_gateway::ros2 { + +Ros2ServiceTransport::Ros2ServiceTransport(rclcpp::Node * node) + : node_(node), serializer_(std::make_shared()) { + RCLCPP_INFO(node_->get_logger(), "Ros2ServiceTransport initialised (native serialization)"); +} + +Ros2ServiceTransport::~Ros2ServiceTransport() { + // Drop all cached clients so any outstanding promise references release + // before rclcpp shuts the underlying executor down. The compat shim + // (Humble) terminates if a future still references shared state at + // destruction; the wiring contract guarantees this destructor runs while + // the executor is still spinnable. + std::unique_lock lock(clients_mutex_); + clients_.clear(); +} + +std::string Ros2ServiceTransport::make_client_key(const std::string & service_path, const std::string & service_type) { + return service_path + "|" + service_type; +} + +compat::GenericServiceClient::SharedPtr Ros2ServiceTransport::get_or_create_client(const std::string & service_path, + const std::string & service_type) { + const std::string key = make_client_key(service_path, service_type); + + { + std::shared_lock lock(clients_mutex_); + auto it = clients_.find(key); + if (it != clients_.end()) { + return it->second; + } + } + + std::unique_lock lock(clients_mutex_); + auto it = clients_.find(key); + if (it != clients_.end()) { + return it->second; + } + + auto client = compat::create_generic_service_client(node_, service_path, service_type); + clients_[key] = client; + + RCLCPP_DEBUG(node_->get_logger(), "Created generic client for %s (%s)", service_path.c_str(), service_type.c_str()); + + return client; +} + +ServiceCallResult Ros2ServiceTransport::call(const std::string & service_path, const std::string & service_type, + const json & request, std::chrono::duration timeout) { + using ros2_medkit_serialization::ServiceActionTypes; + ServiceCallResult result; + + try { + auto client = get_or_create_client(service_path, service_type); + + if (!client->wait_for_service(std::chrono::seconds(5))) { + result.success = false; + result.error_message = "Service not available: " + service_path; + return result; + } + + std::string request_type = ServiceActionTypes::get_service_request_type(service_type); + std::string response_type = ServiceActionTypes::get_service_response_type(service_type); + + json request_data = request.empty() || request.is_null() ? json::object() : request; + + try { + json defaults = serializer_->get_defaults(request_type); + for (auto it = request_data.begin(); it != request_data.end(); ++it) { + defaults[it.key()] = it.value(); + } + request_data = defaults; + } catch (const ros2_medkit_serialization::TypeNotFoundError &) { + // If we can't get defaults, just use provided data. + } + + RosMessage_Cpp ros_request = serializer_->from_json(request_type, request_data); + + RCLCPP_INFO(node_->get_logger(), "Calling service: %s (type: %s)", service_path.c_str(), service_type.c_str()); + + auto future_and_id = client->async_send_request(ros_request.data); + + const auto timeout_secs = + std::chrono::milliseconds{static_cast(std::max(timeout.count(), 0.0) * 1000.0)}; + auto future_status = future_and_id.wait_for(timeout_secs); + + if (future_status != std::future_status::ready) { + client->remove_pending_request(future_and_id.request_id); + ros2_medkit_serialization::destroy_ros_message(&ros_request); + result.success = false; + result.error_message = + "Service call timed out (" + std::to_string(static_cast(timeout.count())) + "s): " + service_path; + return result; + } + + ros2_medkit_serialization::destroy_ros_message(&ros_request); + + auto response_ptr = future_and_id.get(); + if (response_ptr != nullptr) { + try { + auto type_info = ros2_medkit_serialization::TypeCache::instance().get_message_type_info(response_type); + if (type_info != nullptr) { + result.response = serializer_->to_json(type_info, response_ptr.get()); + result.success = true; + RCLCPP_DEBUG(node_->get_logger(), "Service call succeeded: %s", result.response.dump().c_str()); + } else { + result.success = false; + result.error_message = "Unknown response type: " + response_type; + } + } catch (const std::exception & e) { + result.success = false; + result.error_message = std::string("Failed to deserialize response: ") + e.what(); + } + } else { + result.success = false; + result.error_message = "Service returned null response"; + } + + } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + RCLCPP_ERROR(node_->get_logger(), "Type not found for service '%s': %s", service_path.c_str(), e.what()); + result.success = false; + result.error_message = std::string("Unknown service type: ") + e.what(); + } catch (const ros2_medkit_serialization::SerializationError & e) { + RCLCPP_ERROR(node_->get_logger(), "Serialization failed for service '%s': %s", service_path.c_str(), e.what()); + result.success = false; + result.error_message = std::string("Invalid request format: ") + e.what(); + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Service call failed for '%s': %s", service_path.c_str(), e.what()); + result.success = false; + result.error_message = e.what(); + } + + return result; +} + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_subscription_transport.cpp b/src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_subscription_transport.cpp new file mode 100644 index 000000000..43aa67fa8 --- /dev/null +++ b/src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_subscription_transport.cpp @@ -0,0 +1,56 @@ +// 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/ros2/transports/ros2_topic_subscription_transport.hpp" + +#include +#include + +namespace ros2_medkit_gateway::ros2 { + +Ros2TopicSubscriptionTransport::Ros2TopicSubscriptionTransport(TriggerTopicSubscriber * subscriber) + : subscriber_(subscriber) { + if (!subscriber_) { + throw std::invalid_argument("Ros2TopicSubscriptionTransport requires a valid subscriber pointer"); + } +} + +std::unique_ptr Ros2TopicSubscriptionTransport::subscribe(const std::string & topic_path, + const std::string & msg_type, + SampleCallback callback) { + auto key = allocate_key(); + subscriber_->subscribe(topic_path, msg_type, key, std::move(callback)); + return std::make_unique(subscriber_, key); +} + +std::string Ros2TopicSubscriptionTransport::allocate_key() { + // Monotonic synthetic id; the underlying subscriber treats each key as + // an opaque identifier so collisions between adapter instances are + // structurally impossible (each adapter wraps one subscriber). + uint64_t id = next_key_.fetch_add(1, std::memory_order_relaxed); + return "trig_handle_" + std::to_string(id); +} + +Ros2TopicSubscriptionTransport::Handle::Handle(TriggerTopicSubscriber * subscriber, std::string key) + : subscriber_(subscriber), key_(std::move(key)) { +} + +Ros2TopicSubscriptionTransport::Handle::~Handle() { + // Subscriber outlives the handle by construction (GatewayNode tear-down + // order); calling unsubscribe() here drains in-flight rclcpp callbacks + // before the user callback is released. + subscriber_->unsubscribe(key_); +} + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_transport.cpp b/src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_transport.cpp new file mode 100644 index 000000000..3897aabe7 --- /dev/null +++ b/src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_transport.cpp @@ -0,0 +1,177 @@ +// 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/ros2/transports/ros2_topic_transport.hpp" + +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/core/data/topic_data_provider.hpp" +#include "ros2_medkit_gateway/core/exceptions.hpp" +#include "ros2_medkit_serialization/serialization_error.hpp" + +namespace ros2_medkit_gateway::ros2 { + +Ros2TopicTransport::Ros2TopicTransport(rclcpp::Node * node, double default_sample_timeout_sec) + : node_(node) + , serializer_(std::make_shared()) + , type_introspection_(std::make_unique("")) + , default_sample_timeout_sec_(default_sample_timeout_sec) { + if (default_sample_timeout_sec_ < 0.1 || default_sample_timeout_sec_ > 30.0) { + RCLCPP_WARN(node_->get_logger(), + "topic_sample_timeout_sec (%.2f) out of valid range (0.1-30.0), using default: 1.0", + default_sample_timeout_sec_); + default_sample_timeout_sec_ = 1.0; + } + + RCLCPP_INFO(node_->get_logger(), + "Ros2TopicTransport initialised (native_publishing=enabled, " + "topic_sample_timeout=%.2fs).", + default_sample_timeout_sec_); +} + +void Ros2TopicTransport::set_data_provider(TopicDataProvider * provider) { + data_provider_ = provider; +} + +rclcpp::GenericPublisher::SharedPtr Ros2TopicTransport::get_or_create_publisher(const std::string & topic_path, + const std::string & msg_type) { + const std::string key = topic_path + "|" + msg_type; + + // Try read lock first (fast path). + { + std::shared_lock lock(publishers_mutex_); + auto it = publishers_.find(key); + if (it != publishers_.end()) { + return it->second; + } + } + + // Need to create - take exclusive lock. + std::unique_lock lock(publishers_mutex_); + + // Double-check (another thread might have created it). + auto it = publishers_.find(key); + if (it != publishers_.end()) { + return it->second; + } + + // Create new publisher with default QoS (reliable, keep last 10). + auto publisher = node_->create_generic_publisher(topic_path, msg_type, rclcpp::QoS(10)); + publishers_[key] = publisher; + + RCLCPP_DEBUG(node_->get_logger(), "Created generic publisher for %s (%s)", topic_path.c_str(), msg_type.c_str()); + + return publisher; +} + +json Ros2TopicTransport::publish(const std::string & topic_path, const std::string & msg_type, const json & data, + std::chrono::duration /* timeout */) { + try { + auto publisher = get_or_create_publisher(topic_path, msg_type); + rclcpp::SerializedMessage serialized_msg = serializer_->serialize(msg_type, data); + publisher->publish(serialized_msg); + + RCLCPP_INFO(node_->get_logger(), "Published to topic '%s' with type '%s' (native)", topic_path.c_str(), + msg_type.c_str()); + + return json{{"topic", topic_path}, + {"type", msg_type}, + {"status", "published"}, + {"timestamp", std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()}}; + + } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + RCLCPP_ERROR(node_->get_logger(), "Unknown message type '%s': %s", msg_type.c_str(), e.what()); + throw std::runtime_error("Unknown message type '" + msg_type + "': " + e.what()); + + } catch (const ros2_medkit_serialization::SerializationError & e) { + RCLCPP_ERROR(node_->get_logger(), "Failed to serialize message for '%s': %s", topic_path.c_str(), e.what()); + throw std::runtime_error("Failed to serialize message for '" + topic_path + "': " + e.what()); + + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Failed to publish to topic '%s': %s", topic_path.c_str(), e.what()); + throw std::runtime_error("Failed to publish to topic '" + topic_path + "': " + e.what()); + } +} + +TopicSample Ros2TopicTransport::sample(const std::string & topic_name, std::chrono::duration timeout) { + TopicSample out; + out.topic = topic_name; + + if (!data_provider_) { + RCLCPP_ERROR(node_->get_logger(), "TopicDataProvider not configured - sampling '%s' is unavailable", + topic_name.c_str()); + throw TopicNotAvailableException(topic_name); + } + + const double timeout_sec = (timeout.count() > 0.0) ? timeout.count() : default_sample_timeout_sec_; + const auto timeout_ms = std::chrono::milliseconds{static_cast(std::max(timeout_sec, 0.0) * 1000.0)}; + + auto r = data_provider_->sample(topic_name, timeout_ms); + if (!r) { + const auto & err = r.error(); + RCLCPP_WARN(node_->get_logger(), "TopicDataProvider::sample('%s') failed: %s [%d]", topic_name.c_str(), + err.message.c_str(), err.http_status); + if (err.http_status == 404) { + throw TopicNotAvailableException(topic_name); + } + throw ProviderErrorException(err); + } + + const TopicSampleResult & res = *r; + + if (res.message_type.empty() && !res.has_data) { + throw TopicNotAvailableException(topic_name); + } + + out.success = true; + out.publisher_count = static_cast(res.publisher_count); + out.subscriber_count = static_cast(res.subscriber_count); + out.type = res.message_type; + + if (res.has_data && res.data) { + out.status = "data"; + out.data = *res.data; + } else { + out.status = "metadata_only"; + } + + if (!res.message_type.empty()) { + try { + ros2_medkit_serialization::TopicTypeInfo info = type_introspection_->get_type_info(res.message_type); + out.type_info = json{{"schema", info.schema}, {"default_value", info.default_value}}; + } catch (const std::exception & e) { + RCLCPP_DEBUG(node_->get_logger(), "Could not get type info for '%s': %s", res.message_type.c_str(), e.what()); + } + } + + return out; +} + +std::pair Ros2TopicTransport::count_publishers_subscribers(const std::string & topic_name) const { + try { + return {node_->count_publishers(topic_name), node_->count_subscribers(topic_name)}; + } catch (const std::exception & e) { + RCLCPP_DEBUG(node_->get_logger(), "count_publishers/subscribers failed for '%s': %s", topic_name.c_str(), e.what()); + return {0, 0}; + } +} + +} // namespace ros2_medkit_gateway::ros2 diff --git a/src/ros2_medkit_gateway/src/ros2/trigger_topic_subscriber.cpp b/src/ros2_medkit_gateway/src/ros2/trigger_topic_subscriber.cpp new file mode 100644 index 000000000..cf5e318c5 --- /dev/null +++ b/src/ros2_medkit_gateway/src/ros2/trigger_topic_subscriber.cpp @@ -0,0 +1,244 @@ +// 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/ros2/trigger_topic_subscriber.hpp" + +#include +#include +#include + +#include +#include + +#include "ros2_medkit_serialization/serialization_error.hpp" + +namespace ros2_medkit_gateway { + +TriggerTopicSubscriber::TriggerTopicSubscriber(rclcpp::Node * node) + : node_(node), serializer_(std::make_shared()) { + if (!node_) { + throw std::invalid_argument("TriggerTopicSubscriber requires a valid node pointer"); + } + + retry_timer_ = node_->create_wall_timer(std::chrono::seconds(kRetryIntervalSec), [this]() { + retry_pending_subscriptions(); + }); + + RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber initialized"); +} + +TriggerTopicSubscriber::~TriggerTopicSubscriber() { + shutdown(); +} + +void TriggerTopicSubscriber::subscribe(const std::string & topic_name, const std::string & msg_type, + const std::string & handle_key, SampleCallback callback) { + if (shutdown_flag_.load()) { + return; + } + + std::lock_guard lock(mutex_); + + // Idempotent: drop any prior registration for this handle so the new one + // takes over cleanly (both live and pending paths). + live_.erase(handle_key); + pending_.erase(handle_key); + + std::string resolved_type = msg_type; + if (resolved_type.empty()) { + auto topic_names_and_types = node_->get_topic_names_and_types(); + auto it = topic_names_and_types.find(topic_name); + if (it != topic_names_and_types.end() && !it->second.empty()) { + resolved_type = it->second[0]; + } + } + + if (resolved_type.empty()) { + RCLCPP_WARN(node_->get_logger(), + "TriggerTopicSubscriber: cannot determine type for topic '%s' " + "(no publishers yet), queuing handle '%s' for retry", + topic_name.c_str(), handle_key.c_str()); + PendingEntry entry; + entry.topic_name = topic_name; + entry.callback = std::move(callback); + entry.created_at = std::chrono::steady_clock::now(); + pending_[handle_key] = std::move(entry); + return; + } + + create_subscription_internal(handle_key, topic_name, resolved_type, std::move(callback)); +} + +void TriggerTopicSubscriber::create_subscription_internal(const std::string & handle_key, + const std::string & topic_name, const std::string & msg_type, + SampleCallback callback) { + rclcpp::QoS qos = rclcpp::SensorDataQoS(); + + // Snapshot the handle key + topic + type by value so the callback never + // dereferences any expired strings; the user callback is looked up under + // mutex_ to avoid firing after unsubscribe()/shutdown() returns. + // NOLINTNEXTLINE(performance-unnecessary-value-param) - GenericSubscription requires value type in callback + auto cb = [this, handle_key, topic_name, msg_type](std::shared_ptr msg) { + if (shutdown_flag_.load()) { + return; + } + + SampleCallback user_cb; + { + std::lock_guard cb_lock(mutex_); + auto it = live_.find(handle_key); + if (it == live_.end()) { + return; + } + user_cb = it->second.callback; + } + if (!user_cb) { + return; + } + + try { + nlohmann::json data_json = serializer_->deserialize(msg_type, *msg); + user_cb(data_json); + } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "TriggerTopicSubscriber: unknown type '%s' for topic '%s': %s", msg_type.c_str(), + topic_name.c_str(), e.what()); + } catch (const ros2_medkit_serialization::SerializationError & e) { + RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "TriggerTopicSubscriber: deserialization failed for '%s': %s", topic_name.c_str(), e.what()); + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, + "TriggerTopicSubscriber: error processing message from '%s': %s", topic_name.c_str(), + e.what()); + } + }; + + try { + auto subscription = node_->create_generic_subscription(topic_name, msg_type, qos, cb); + + LiveEntry entry; + entry.subscription = std::move(subscription); + entry.topic_name = topic_name; + entry.msg_type = msg_type; + entry.callback = std::move(callback); + live_[handle_key] = std::move(entry); + + RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber: subscribed handle '%s' to '%s' (type=%s)", + handle_key.c_str(), topic_name.c_str(), msg_type.c_str()); + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), + "TriggerTopicSubscriber: failed to create subscription for '%s' (handle '%s'): %s", topic_name.c_str(), + handle_key.c_str(), e.what()); + } +} + +void TriggerTopicSubscriber::unsubscribe(const std::string & handle_key) { + std::lock_guard lock(mutex_); + + if (pending_.erase(handle_key) > 0) { + RCLCPP_DEBUG(node_->get_logger(), "TriggerTopicSubscriber: removed pending handle '%s'", handle_key.c_str()); + return; + } + + auto it = live_.find(handle_key); + if (it == live_.end()) { + return; + } + + // Reset the subscription before erasing the map entry: rclcpp guarantees + // that GenericSubscription destruction waits for in-flight callbacks to + // drain, so the captured handle_key cannot resolve to a stale entry after + // this returns. + it->second.subscription.reset(); + it->second.callback = nullptr; + live_.erase(it); + + RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber: unsubscribed handle '%s'", handle_key.c_str()); +} + +void TriggerTopicSubscriber::shutdown() { + if (shutdown_flag_.exchange(true)) { + return; + } + + if (retry_timer_) { + retry_timer_->cancel(); + retry_timer_.reset(); + } + + std::lock_guard lock(mutex_); + pending_.clear(); + // Reset every subscription explicitly so rclcpp drains in-flight callbacks + // before we tear down user callbacks (the subscription-destructor pattern). + for (auto & [_, entry] : live_) { + entry.subscription.reset(); + entry.callback = nullptr; + } + live_.clear(); + RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber: shutdown complete"); +} + +void TriggerTopicSubscriber::set_retry_callback(RetryCallback cb) { + retry_callback_ = std::move(cb); +} + +void TriggerTopicSubscriber::retry_pending_subscriptions() { + // Run the manager-side retry hook first so that any newly resolvable + // unresolved triggers get a chance to call subscribe() with a real + // topic name before the per-handle retry below. + if (retry_callback_) { + retry_callback_(); + } + + if (shutdown_flag_.load()) { + return; + } + + std::lock_guard lock(mutex_); + if (pending_.empty()) { + return; + } + + auto now = std::chrono::steady_clock::now(); + std::vector resolved_keys; + std::vector expired_keys; + + auto topic_names_and_types = node_->get_topic_names_and_types(); + + for (auto & [handle_key, entry] : pending_) { + if (now - entry.created_at > std::chrono::seconds(kPendingTimeoutSec)) { + RCLCPP_ERROR(node_->get_logger(), + "TriggerTopicSubscriber: handle '%s' (topic '%s') type not available after %ds, giving up", + handle_key.c_str(), entry.topic_name.c_str(), kPendingTimeoutSec); + expired_keys.push_back(handle_key); + continue; + } + + auto type_it = topic_names_and_types.find(entry.topic_name); + if (type_it != topic_names_and_types.end() && !type_it->second.empty()) { + create_subscription_internal(handle_key, entry.topic_name, type_it->second[0], std::move(entry.callback)); + resolved_keys.push_back(handle_key); + } + } + + for (const auto & key : resolved_keys) { + RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber: resolved pending handle '%s'", key.c_str()); + pending_.erase(key); + } + for (const auto & key : expired_keys) { + pending_.erase(key); + } +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/trigger_fault_subscriber.cpp b/src/ros2_medkit_gateway/src/trigger_fault_subscriber.cpp index ff05b4551..31a334f9a 100644 --- a/src/ros2_medkit_gateway/src/trigger_fault_subscriber.cpp +++ b/src/ros2_medkit_gateway/src/trigger_fault_subscriber.cpp @@ -14,8 +14,8 @@ #include "ros2_medkit_gateway/trigger_fault_subscriber.hpp" -#include "ros2_medkit_gateway/fault_manager.hpp" #include "ros2_medkit_gateway/fault_manager_paths.hpp" +#include "ros2_medkit_gateway/ros2/conversions/fault_msg_conversions.hpp" namespace ros2_medkit_gateway { @@ -40,8 +40,8 @@ void TriggerFaultSubscriber::set_node_to_entity_resolver(NodeToEntityFn resolver } void TriggerFaultSubscriber::on_fault_event(const ros2_medkit_msgs::msg::FaultEvent::ConstSharedPtr & msg) { - // Convert fault to JSON using the same method as SSEFaultHandler - nlohmann::json fault_json = FaultManager::fault_to_json(msg->fault); + // Convert fault to JSON using the same helper as SSEFaultHandler + nlohmann::json fault_json = ros2::conversions::fault_to_json(msg->fault); fault_json["event_type"] = msg->event_type; // Derive entity_id from reporting_sources (first source, if available). diff --git a/src/ros2_medkit_gateway/src/trigger_topic_subscriber.cpp b/src/ros2_medkit_gateway/src/trigger_topic_subscriber.cpp deleted file mode 100644 index 7f518ea37..000000000 --- a/src/ros2_medkit_gateway/src/trigger_topic_subscriber.cpp +++ /dev/null @@ -1,260 +0,0 @@ -// 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/trigger_topic_subscriber.hpp" - -#include -#include - -#include "ros2_medkit_serialization/serialization_error.hpp" - -namespace ros2_medkit_gateway { - -TriggerTopicSubscriber::TriggerTopicSubscriber(rclcpp::Node * node, ResourceChangeNotifier & notifier) - : node_(node), notifier_(notifier), serializer_(std::make_shared()) { - if (!node_) { - throw std::invalid_argument("TriggerTopicSubscriber requires a valid node pointer"); - } - - // Create a wall timer that retries pending subscriptions every kRetryIntervalSec seconds. - retry_timer_ = node_->create_wall_timer(std::chrono::seconds(kRetryIntervalSec), [this]() { - retry_pending_subscriptions(); - }); - - RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber initialized"); -} - -TriggerTopicSubscriber::~TriggerTopicSubscriber() { - shutdown(); -} - -void TriggerTopicSubscriber::subscribe(const std::string & topic_name, const std::string & resource_path, - const std::string & entity_id) { - if (shutdown_flag_.load()) { - return; - } - - std::lock_guard lock(mutex_); - - // If already subscribed, add entity_id to the existing subscription - auto it = subscriptions_.find(topic_name); - if (it != subscriptions_.end()) { - it->second.entity_ids.insert(entity_id); - RCLCPP_DEBUG(node_->get_logger(), - "TriggerTopicSubscriber: added entity '%s' to existing subscription for '%s' " - "(entity_ids count=%zu)", - entity_id.c_str(), topic_name.c_str(), it->second.entity_ids.size()); - return; - } - - // If already pending, add entity_id to the pending entry - auto pit = pending_.find(topic_name); - if (pit != pending_.end()) { - pit->second.entity_ids.insert(entity_id); - RCLCPP_DEBUG(node_->get_logger(), "TriggerTopicSubscriber: added entity '%s' to pending subscription for '%s'", - entity_id.c_str(), topic_name.c_str()); - return; - } - - // Determine topic type from the ROS 2 graph - auto topic_names_and_types = node_->get_topic_names_and_types(); - auto type_it = topic_names_and_types.find(topic_name); - if (type_it == topic_names_and_types.end() || type_it->second.empty()) { - RCLCPP_WARN(node_->get_logger(), - "TriggerTopicSubscriber: cannot determine type for topic '%s' " - "(no publishers yet), queuing for retry", - topic_name.c_str()); - auto & pending = pending_[topic_name]; - pending.resource_path = resource_path; - pending.entity_ids.insert(entity_id); - pending.created_at = std::chrono::steady_clock::now(); - return; - } - - const std::string & msg_type = type_it->second[0]; - create_subscription_internal(topic_name, msg_type, resource_path, {entity_id}); -} - -void TriggerTopicSubscriber::create_subscription_internal(const std::string & topic_name, const std::string & msg_type, - const std::string & resource_path, - const std::unordered_set & entity_ids) { - // Create a GenericSubscription that deserializes and forwards to notifier. - // Use SensorDataQoS (best effort) as a default for sensor-style topics. - rclcpp::QoS qos = rclcpp::SensorDataQoS(); - - // Capture topic_name and msg_type by value for the callback. - // Entity IDs are read from subscriptions_ under lock in the callback. - // NOLINTNEXTLINE(performance-unnecessary-value-param) - GenericSubscription requires value type in callback - auto callback = [this, topic_name, msg_type](std::shared_ptr msg) { - if (shutdown_flag_.load()) { - return; - } - - try { - // Deserialize CDR data to JSON - nlohmann::json data_json = serializer_->deserialize(msg_type, *msg); - - // Read current entity_ids and resource_path under lock - std::unordered_set current_entity_ids; - std::string current_resource_path; - { - std::lock_guard cb_lock(mutex_); - auto sub_it = subscriptions_.find(topic_name); - if (sub_it == subscriptions_.end()) { - return; - } - current_entity_ids = sub_it->second.entity_ids; - current_resource_path = sub_it->second.resource_path; - } - - // Emit one notification per entity_id, using resource_path (not topic_name) - for (const auto & eid : current_entity_ids) { - notifier_.notify("data", eid, current_resource_path, data_json, ChangeType::UPDATED); - } - - } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { - RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, - "TriggerTopicSubscriber: unknown type '%s' for topic '%s': %s", msg_type.c_str(), - topic_name.c_str(), e.what()); - } catch (const ros2_medkit_serialization::SerializationError & e) { - RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, - "TriggerTopicSubscriber: deserialization failed for '%s': %s", topic_name.c_str(), e.what()); - } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, - "TriggerTopicSubscriber: error processing message from '%s': %s", topic_name.c_str(), - e.what()); - } - }; - - try { - auto subscription = node_->create_generic_subscription(topic_name, msg_type, qos, callback); - - SubscriptionEntry entry; - entry.subscription = std::move(subscription); - entry.resource_path = resource_path; - entry.entity_ids = entity_ids; - subscriptions_[topic_name] = std::move(entry); - - RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber: subscribed to '%s' (type=%s, entities=%zu, resource=%s)", - topic_name.c_str(), msg_type.c_str(), entity_ids.size(), resource_path.c_str()); - } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "TriggerTopicSubscriber: failed to create subscription for '%s': %s", - topic_name.c_str(), e.what()); - } -} - -void TriggerTopicSubscriber::unsubscribe(const std::string & topic_name, const std::string & entity_id) { - std::lock_guard lock(mutex_); - - // Check pending subscriptions first - auto pit = pending_.find(topic_name); - if (pit != pending_.end()) { - pit->second.entity_ids.erase(entity_id); - if (pit->second.entity_ids.empty()) { - RCLCPP_INFO(node_->get_logger(), - "TriggerTopicSubscriber: removed pending subscription for '%s' (no entities remaining)", - topic_name.c_str()); - pending_.erase(pit); - } - return; - } - - auto it = subscriptions_.find(topic_name); - if (it == subscriptions_.end()) { - return; - } - - it->second.entity_ids.erase(entity_id); - - if (it->second.entity_ids.empty()) { - RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber: unsubscribed from '%s' (no entities remaining)", - topic_name.c_str()); - subscriptions_.erase(it); - } else { - RCLCPP_DEBUG(node_->get_logger(), "TriggerTopicSubscriber: removed entity '%s' from '%s' (entity_ids count=%zu)", - entity_id.c_str(), topic_name.c_str(), it->second.entity_ids.size()); - } -} - -void TriggerTopicSubscriber::shutdown() { - if (shutdown_flag_.exchange(true)) { - return; // Already shut down - } - - if (retry_timer_) { - retry_timer_->cancel(); - retry_timer_.reset(); - } - - std::lock_guard lock(mutex_); - pending_.clear(); - subscriptions_.clear(); - RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber: shutdown complete"); -} - -void TriggerTopicSubscriber::set_retry_callback(RetryCallback cb) { - retry_callback_ = std::move(cb); -} - -void TriggerTopicSubscriber::retry_pending_subscriptions() { - // Also retry unresolved triggers in TriggerManager (resource_path -> topic_name) - if (retry_callback_) { - retry_callback_(); - } - - if (shutdown_flag_.load()) { - return; - } - - std::lock_guard lock(mutex_); - - if (pending_.empty()) { - return; - } - - auto now = std::chrono::steady_clock::now(); - std::vector resolved; - std::vector expired; - - // Query the ROS 2 graph once for all pending topics - auto topic_names_and_types = node_->get_topic_names_and_types(); - - for (auto & [topic_name, pending] : pending_) { - // Check timeout - if (now - pending.created_at > std::chrono::seconds(kPendingTimeoutSec)) { - RCLCPP_ERROR(node_->get_logger(), "TriggerTopicSubscriber: topic '%s' type not available after %ds, giving up", - topic_name.c_str(), kPendingTimeoutSec); - expired.push_back(topic_name); - continue; - } - - // Try to resolve topic type - auto type_it = topic_names_and_types.find(topic_name); - if (type_it != topic_names_and_types.end() && !type_it->second.empty()) { - // Topic type is now available - create the subscription - create_subscription_internal(topic_name, type_it->second[0], pending.resource_path, pending.entity_ids); - resolved.push_back(topic_name); - } - } - - for (const auto & name : resolved) { - RCLCPP_INFO(node_->get_logger(), "TriggerTopicSubscriber: resolved pending subscription for '%s'", name.c_str()); - pending_.erase(name); - } - for (const auto & name : expired) { - pending_.erase(name); - } -} - -} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/test/test_configuration_manager.cpp b/src/ros2_medkit_gateway/test/test_configuration_manager.cpp index a6f2ce21d..4e387a360 100644 --- a/src/ros2_medkit_gateway/test/test_configuration_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_configuration_manager.cpp @@ -20,6 +20,7 @@ #include #include "ros2_medkit_gateway/configuration_manager.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_parameter_transport.hpp" using namespace ros2_medkit_gateway; @@ -38,9 +39,10 @@ class TestConfigurationManager : public ::testing::Test { // Use short timeout for tests to avoid long waits on nonexistent nodes rclcpp::NodeOptions options; options.allow_undeclared_parameters(false); - options.parameter_overrides({rclcpp::Parameter("parameter_service_timeout_sec", 0.1)}); node_ = std::make_shared("test_config_manager_node", options); - config_manager_ = std::make_unique(node_.get()); + // Short timeout (0.1s) for tests to avoid long waits on nonexistent nodes. + parameter_transport_ = std::make_shared(node_.get(), 0.1, 60.0); + config_manager_ = std::make_unique(parameter_transport_); // Create and start executor in separate thread to avoid deadlocks executor_ = std::make_shared(); @@ -60,11 +62,13 @@ class TestConfigurationManager : public ::testing::Test { } executor_->remove_node(node_); config_manager_.reset(); + parameter_transport_.reset(); node_.reset(); executor_.reset(); } std::shared_ptr node_; + std::shared_ptr parameter_transport_; std::unique_ptr config_manager_; std::shared_ptr executor_; std::thread spin_thread_; @@ -347,6 +351,7 @@ TEST_F(TestConfigurationManager, test_concurrent_queries_no_crash) { // (spin_mutex_ serializes ROS 2 IPC to prevent executor conflicts). std::vector results(3); std::vector threads; + threads.reserve(3); for (int i = 0; i < 3; ++i) { threads.emplace_back([this, i, &results]() { results[static_cast(i)] = config_manager_->list_parameters("/concurrent_node_" + std::to_string(i)); @@ -422,6 +427,7 @@ TEST_F(TestConfigurationManager, test_concurrent_parameter_access) { // Access from multiple threads should be safe std::vector threads; + threads.reserve(3); std::atomic success_count{0}; for (int i = 0; i < 3; ++i) { @@ -461,6 +467,7 @@ TEST_F(TestConfigurationManager, test_concurrent_parameter_operations_no_executo constexpr int kOpsPerThread = 5; std::vector threads; + threads.reserve(kNumThreads); std::atomic success_count{0}; std::atomic exception_count{0}; std::atomic start_flag{false}; diff --git a/src/ros2_medkit_gateway/test/test_configuration_manager_routing.cpp b/src/ros2_medkit_gateway/test/test_configuration_manager_routing.cpp new file mode 100644 index 000000000..43ba57aef --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_configuration_manager_routing.cpp @@ -0,0 +1,323 @@ +// 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 + +#include +#include +#include + +#include "ros2_medkit_gateway/core/configuration/parameter_types.hpp" +#include "ros2_medkit_gateway/core/managers/configuration_manager.hpp" +#include "ros2_medkit_gateway/core/transports/parameter_transport.hpp" + +namespace ros2_medkit_gateway { +namespace { + +class MockParameterTransport : public ParameterTransport { + public: + MockParameterTransport() = default; + ~MockParameterTransport() override = default; + MockParameterTransport(const MockParameterTransport &) = delete; + MockParameterTransport & operator=(const MockParameterTransport &) = delete; + MockParameterTransport(MockParameterTransport &&) = delete; + MockParameterTransport & operator=(MockParameterTransport &&) = delete; + + bool is_self_node(const std::string & node_name) const override { + return node_name == self_node_; + } + + ParameterResult list_parameters(const std::string & node_name) override { + last_list_node_ = node_name; + ++list_calls_; + ParameterResult r; + r.success = list_success_; + r.data = list_data_; + r.error_message = list_error_; + r.error_code = list_error_code_; + return r; + } + + ParameterResult get_parameter(const std::string & node_name, const std::string & param_name) override { + last_get_node_ = node_name; + last_get_name_ = param_name; + ++get_calls_; + ParameterResult r; + r.success = get_success_; + r.data = get_data_; + r.error_message = get_error_; + r.error_code = get_error_code_; + return r; + } + + ParameterResult set_parameter(const std::string & node_name, const std::string & param_name, + const json & value) override { + last_set_node_ = node_name; + last_set_name_ = param_name; + last_set_value_ = value; + ++set_calls_; + set_history_.push_back({node_name, param_name, value}); + ParameterResult r; + auto override_it = set_failures_.find(param_name); + if (override_it != set_failures_.end()) { + r.success = false; + r.error_message = override_it->second; + r.error_code = ParameterErrorCode::INVALID_VALUE; + return r; + } + r.success = set_success_; + if (set_success_) { + json obj; + obj["name"] = param_name; + obj["value"] = value; + obj["type"] = "string"; + r.data = obj; + } else { + r.error_message = set_error_; + r.error_code = set_error_code_; + } + return r; + } + + ParameterResult list_own_parameters() override { + ++own_list_calls_; + ParameterResult r; + r.success = true; + r.data = json::array({json{{"name", "self_param"}, {"value", 1}, {"type", "int"}}}); + return r; + } + + ParameterResult get_own_parameter(const std::string & param_name) override { + last_own_get_name_ = param_name; + ++own_get_calls_; + ParameterResult r; + r.success = true; + r.data = json{{"name", param_name}, {"value", 7}, {"type", "int"}}; + return r; + } + + ParameterResult get_default(const std::string & node_name, const std::string & param_name) override { + last_get_default_node_ = node_name; + last_get_default_name_ = param_name; + ++get_default_calls_; + ParameterResult r; + auto node_it = defaults_.find(node_name); + if (node_it == defaults_.end()) { + r.success = false; + r.error_message = "no defaults"; + r.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED; + return r; + } + auto val_it = node_it->second.find(param_name); + if (val_it == node_it->second.end()) { + r.success = false; + r.error_message = "no default for param"; + r.error_code = ParameterErrorCode::NOT_FOUND; + return r; + } + r.success = true; + r.data = val_it->second; + return r; + } + + ParameterResult list_defaults(const std::string & node_name) override { + last_list_defaults_node_ = node_name; + ++list_defaults_calls_; + ParameterResult r; + auto node_it = defaults_.find(node_name); + if (node_it == defaults_.end()) { + r.success = false; + r.error_message = "no defaults"; + r.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED; + return r; + } + json arr = json::array(); + for (const auto & [name, value] : node_it->second) { + json entry; + entry["name"] = name; + entry["value"] = value; + entry["type"] = "string"; + arr.push_back(entry); + } + r.success = true; + r.data = arr; + return r; + } + + bool is_node_available(const std::string & /*node_name*/) const override { + return true; + } + + void invalidate(const std::string & node_name) override { + invalidated_.push_back(node_name); + } + + void shutdown() override { + ++shutdown_calls_; + } + + // Mock state knobs --------------------------------------------------------- + std::string self_node_ = "/self"; + bool list_success_ = true; + json list_data_ = json::array(); + std::string list_error_; + ParameterErrorCode list_error_code_ = ParameterErrorCode::NONE; + + bool get_success_ = true; + json get_data_ = json::object(); + std::string get_error_; + ParameterErrorCode get_error_code_ = ParameterErrorCode::NONE; + + bool set_success_ = true; + std::string set_error_; + ParameterErrorCode set_error_code_ = ParameterErrorCode::NONE; + std::map set_failures_; // param_name -> reason + + std::map> defaults_; + + // Recording state ---------------------------------------------------------- + struct SetCall { + std::string node; + std::string param; + json value; + }; + + std::string last_list_node_; + std::string last_get_node_, last_get_name_; + std::string last_set_node_, last_set_name_; + json last_set_value_; + std::string last_own_get_name_; + std::string last_get_default_node_, last_get_default_name_; + std::string last_list_defaults_node_; + std::vector set_history_; + std::vector invalidated_; + + int list_calls_ = 0; + int get_calls_ = 0; + int set_calls_ = 0; + int own_list_calls_ = 0; + int own_get_calls_ = 0; + int get_default_calls_ = 0; + int list_defaults_calls_ = 0; + int shutdown_calls_ = 0; +}; + +} // namespace + +TEST(ConfigurationManagerRoutingTest, ListParametersDelegatesToTransportForRemoteNode) { + auto mock = std::make_shared(); + mock->list_data_ = json::array({json{{"name", "speed"}, {"value", 42}, {"type", "int"}}}); + ConfigurationManager mgr(mock); + auto r = mgr.list_parameters("/remote/node"); + EXPECT_TRUE(r.success); + EXPECT_EQ(mock->last_list_node_, "/remote/node"); + EXPECT_EQ(mock->list_calls_, 1); + EXPECT_EQ(mock->own_list_calls_, 0); + ASSERT_TRUE(r.data.is_array()); + ASSERT_EQ(r.data.size(), 1u); + EXPECT_EQ(r.data[0]["name"], "speed"); +} + +TEST(ConfigurationManagerRoutingTest, ListParametersShortCircuitsForSelfNode) { + auto mock = std::make_shared(); + mock->self_node_ = "/gateway"; + ConfigurationManager mgr(mock); + auto r = mgr.list_parameters("/gateway"); + EXPECT_TRUE(r.success); + EXPECT_EQ(mock->own_list_calls_, 1); + EXPECT_EQ(mock->list_calls_, 0); // no IPC delegation +} + +TEST(ConfigurationManagerRoutingTest, GetParameterShortCircuitsForSelfNode) { + auto mock = std::make_shared(); + mock->self_node_ = "/gateway"; + ConfigurationManager mgr(mock); + auto r = mgr.get_parameter("/gateway", "log_level"); + EXPECT_TRUE(r.success); + EXPECT_EQ(mock->own_get_calls_, 1); + EXPECT_EQ(mock->last_own_get_name_, "log_level"); + EXPECT_EQ(mock->get_calls_, 0); +} + +TEST(ConfigurationManagerRoutingTest, SetParameterDelegatesAndPropagatesError) { + auto mock = std::make_shared(); + mock->set_success_ = false; + mock->set_error_ = "invalid value"; + mock->set_error_code_ = ParameterErrorCode::INVALID_VALUE; + ConfigurationManager mgr(mock); + auto r = mgr.set_parameter("/node", "p", 1); + EXPECT_FALSE(r.success); + EXPECT_EQ(r.error_code, ParameterErrorCode::INVALID_VALUE); + EXPECT_EQ(r.error_message, "invalid value"); + EXPECT_EQ(mock->last_set_node_, "/node"); + EXPECT_EQ(mock->last_set_name_, "p"); +} + +TEST(ConfigurationManagerRoutingTest, ResetParameterUsesCachedDefaultThenSet) { + auto mock = std::make_shared(); + mock->defaults_["/node"]["p"] = 99; + ConfigurationManager mgr(mock); + auto r = mgr.reset_parameter("/node", "p"); + ASSERT_TRUE(r.success); + EXPECT_EQ(mock->get_default_calls_, 1); + EXPECT_EQ(mock->last_get_default_node_, "/node"); + EXPECT_EQ(mock->last_get_default_name_, "p"); + EXPECT_EQ(mock->set_calls_, 1); + EXPECT_EQ(mock->last_set_value_, 99); + ASSERT_TRUE(r.data.is_object()); + EXPECT_EQ(r.data["reset_to_default"], true); +} + +TEST(ConfigurationManagerRoutingTest, ResetParameterPropagatesNoDefaults) { + auto mock = std::make_shared(); + ConfigurationManager mgr(mock); + auto r = mgr.reset_parameter("/node", "p"); + EXPECT_FALSE(r.success); + EXPECT_EQ(r.error_code, ParameterErrorCode::NO_DEFAULTS_CACHED); + EXPECT_EQ(mock->set_calls_, 0); // never reached set +} + +TEST(ConfigurationManagerRoutingTest, ResetAllParametersFanOutsAndAggregates) { + auto mock = std::make_shared(); + mock->defaults_["/node"]["a"] = 1; + mock->defaults_["/node"]["b"] = 2; + mock->defaults_["/node"]["c"] = 3; + mock->set_failures_["b"] = "read-only"; // one of three fails + ConfigurationManager mgr(mock); + auto r = mgr.reset_all_parameters("/node"); + EXPECT_FALSE(r.success); // partial failure + EXPECT_EQ(mock->list_defaults_calls_, 1); + EXPECT_EQ(mock->set_calls_, 3); + ASSERT_TRUE(r.data.is_object()); + EXPECT_EQ(r.data["reset_count"], 2u); + EXPECT_EQ(r.data["failed_count"], 1u); + ASSERT_TRUE(r.data["failed_parameters"].is_array()); + EXPECT_EQ(r.data["failed_parameters"][0], "b"); +} + +TEST(ConfigurationManagerRoutingTest, ShutdownIsIdempotentAndForwardsToTransport) { + auto mock = std::make_shared(); + ConfigurationManager mgr(mock); + mgr.shutdown(); + mgr.shutdown(); + mgr.shutdown(); + EXPECT_EQ(mock->shutdown_calls_, 1); // forwarded once + // Subsequent operations short-circuit with SHUT_DOWN. + auto r = mgr.list_parameters("/node"); + EXPECT_FALSE(r.success); + EXPECT_EQ(r.error_code, ParameterErrorCode::SHUT_DOWN); + EXPECT_EQ(mock->list_calls_, 0); +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/test/test_data_access_manager.cpp b/src/ros2_medkit_gateway/test/test_data_access_manager.cpp index c8fa508b4..7b3874add 100644 --- a/src/ros2_medkit_gateway/test/test_data_access_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_data_access_manager.cpp @@ -22,84 +22,15 @@ #include #include "ros2_medkit_gateway/core/exceptions.hpp" -#include "ros2_medkit_gateway/core/type_introspection.hpp" #include "ros2_medkit_gateway/data/ros2_topic_data_provider.hpp" #include "ros2_medkit_gateway/data_access_manager.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp" #include "ros2_medkit_gateway/ros2_common/ros2_subscription_executor.hpp" #include "ros2_medkit_serialization/json_serializer.hpp" using namespace ros2_medkit_gateway; using namespace std::chrono_literals; -// ============================================================================= -// TypeIntrospection Tests -// ============================================================================= - -class TypeIntrospectionTest : public ::testing::Test { - protected: - void SetUp() override { - introspection_ = std::make_unique(""); - } - - std::unique_ptr introspection_; -}; - -TEST_F(TypeIntrospectionTest, get_type_template_for_valid_type) { - auto template_json = introspection_->get_type_template("std_msgs/msg/String"); - - EXPECT_TRUE(template_json.is_object()); - EXPECT_TRUE(template_json.contains("data")); -} - -TEST_F(TypeIntrospectionTest, get_type_template_for_float32) { - auto template_json = introspection_->get_type_template("std_msgs/msg/Float32"); - - EXPECT_TRUE(template_json.is_object()); - EXPECT_TRUE(template_json.contains("data")); -} - -TEST_F(TypeIntrospectionTest, get_type_template_for_unknown_type_throws) { - EXPECT_THROW(introspection_->get_type_template("nonexistent_pkg/msg/NonExistent"), std::runtime_error); -} - -TEST_F(TypeIntrospectionTest, get_type_schema_for_valid_type) { - auto schema = introspection_->get_type_schema("std_msgs/msg/String"); - - EXPECT_TRUE(schema.is_object()); -} - -TEST_F(TypeIntrospectionTest, get_type_schema_for_unknown_type_throws) { - EXPECT_THROW(introspection_->get_type_schema("nonexistent_pkg/msg/NonExistent"), std::runtime_error); -} - -TEST_F(TypeIntrospectionTest, get_type_info_returns_complete_info) { - auto info = introspection_->get_type_info("std_msgs/msg/String"); - - EXPECT_EQ(info.name, "std_msgs/msg/String"); - EXPECT_TRUE(info.default_value.is_object()); - EXPECT_TRUE(info.schema.is_object()); -} - -TEST_F(TypeIntrospectionTest, get_type_info_caches_results) { - // First call - auto info1 = introspection_->get_type_info("std_msgs/msg/Float32"); - // Second call should return cached result - auto info2 = introspection_->get_type_info("std_msgs/msg/Float32"); - - EXPECT_EQ(info1.name, info2.name); - EXPECT_EQ(info1.default_value, info2.default_value); - EXPECT_EQ(info1.schema, info2.schema); -} - -TEST_F(TypeIntrospectionTest, get_type_info_for_unknown_type_returns_empty) { - // For unknown types, get_type_info should return empty objects instead of throwing - auto info = introspection_->get_type_info("nonexistent_pkg/msg/NonExistent"); - - EXPECT_EQ(info.name, "nonexistent_pkg/msg/NonExistent"); - EXPECT_TRUE(info.default_value.is_object()); - EXPECT_TRUE(info.schema.is_object()); -} - // ============================================================================= // DataAccessManager Tests // ============================================================================= @@ -115,19 +46,19 @@ class DataAccessManagerTest : public ::testing::Test { } void SetUp() override { - rclcpp::NodeOptions options; - // Use short timeout for faster tests - options.parameter_overrides({rclcpp::Parameter("topic_sample_timeout_sec", 0.5)}); - node_ = std::make_shared("test_data_access_node", options); - data_manager_ = std::make_unique(node_.get()); + node_ = std::make_shared("test_data_access_node"); + transport_ = std::make_shared(node_.get(), 0.5); + data_manager_ = std::make_unique(transport_, 0.5); } void TearDown() override { data_manager_.reset(); + transport_.reset(); node_.reset(); } std::shared_ptr node_; + std::shared_ptr transport_; std::unique_ptr data_manager_; }; @@ -147,9 +78,14 @@ TEST_F(DataAccessManagerTest, get_topic_sample_timeout_returns_configured_value) EXPECT_NEAR(timeout, 0.5, 0.01); } -TEST_F(DataAccessManagerTest, sample_nonexistent_topic_throws) { - EXPECT_THROW(data_manager_->get_topic_sample_with_fallback("/nonexistent_topic_xyz_123", 0.1), - TopicNotAvailableException); +TEST_F(DataAccessManagerTest, sample_nonexistent_topic_returns_metadata_only) { + // With no publishers present, the manager short-circuits to a metadata-only + // response without engaging the transport. This mirrors the fast-path + // contract documented in the manager and exercised by the routing tests. + auto result = data_manager_->get_topic_sample_with_fallback("/nonexistent_topic_xyz_123", 0.1); + EXPECT_EQ(result["status"], "metadata_only"); + EXPECT_EQ(result["topic"], "/nonexistent_topic_xyz_123"); + EXPECT_EQ(result["publisher_count"].get(), 0u); } TEST_F(DataAccessManagerTest, publish_to_topic_creates_publisher) { @@ -204,9 +140,7 @@ class DataAccessManagerWithPublisherTest : public ::testing::Test { } void SetUp() override { - rclcpp::NodeOptions options; - options.parameter_overrides({rclcpp::Parameter("topic_sample_timeout_sec", 1.0)}); - node_ = std::make_shared("test_data_access_pub_node", options); + node_ = std::make_shared("test_data_access_pub_node"); // Publisher lives on a node that the main executor does not spin so // its create / destroy does not race rcutils_hash_map_* iterations // from the spin thread (TSan). @@ -218,12 +152,14 @@ class DataAccessManagerWithPublisherTest : public ::testing::Test { executor_ = std::make_shared(); executor_->add_node(node_); - data_manager_ = std::make_unique(node_.get()); + transport_ = std::make_shared(node_.get(), 1.0); + data_manager_ = std::make_unique(transport_, 1.0); sub_exec_ = std::make_shared(node_); serializer_ = std::make_shared(); topic_data_provider_ = std::make_shared(sub_exec_, serializer_); data_manager_->set_topic_data_provider(topic_data_provider_.get()); + transport_->set_data_provider(topic_data_provider_.get()); // Create a publisher for test topic publisher_ = publisher_node_->create_publisher("/test_sample_topic", 10); @@ -243,9 +179,13 @@ class DataAccessManagerWithPublisherTest : public ::testing::Test { spin_thread_.join(); } publisher_.reset(); + if (transport_) { + transport_->set_data_provider(nullptr); + } topic_data_provider_.reset(); sub_exec_.reset(); data_manager_.reset(); + transport_.reset(); executor_.reset(); publisher_node_.reset(); node_.reset(); @@ -254,6 +194,7 @@ class DataAccessManagerWithPublisherTest : public ::testing::Test { std::shared_ptr node_; std::shared_ptr publisher_node_; std::shared_ptr executor_; + std::shared_ptr transport_; std::unique_ptr data_manager_; std::shared_ptr sub_exec_; std::shared_ptr serializer_; @@ -285,8 +226,11 @@ TEST_F(DataAccessManagerWithPublisherTest, sample_topic_returns_type_info) { EXPECT_EQ(result["type"], "std_msgs/msg/String"); } -TEST_F(DataAccessManagerWithPublisherTest, sample_nonexistent_topic_throws_exception) { - EXPECT_THROW(data_manager_->get_topic_sample_native("/nonexistent_topic_abc", 0.1), TopicNotAvailableException); +TEST_F(DataAccessManagerWithPublisherTest, sample_nonexistent_topic_returns_metadata_only) { + auto result = data_manager_->get_topic_sample_native("/nonexistent_topic_abc", 0.1); + EXPECT_EQ(result["status"], "metadata_only"); + EXPECT_EQ(result["topic"], "/nonexistent_topic_abc"); + EXPECT_EQ(result["publisher_count"].get(), 0u); } TEST_F(DataAccessManagerWithPublisherTest, get_topic_sample_native_returns_metadata) { @@ -329,12 +273,10 @@ class DataAccessManagerParameterTest : public ::testing::Test { }; TEST_F(DataAccessManagerParameterTest, invalid_timeout_uses_default) { - rclcpp::NodeOptions options; - options.parameter_overrides({rclcpp::Parameter("topic_sample_timeout_sec", 100.0)}); // Out of range - auto node = std::make_shared("test_param_node2", options); - - auto manager = std::make_unique(node.get()); - // Should use default of 1.0 + auto node = std::make_shared("test_param_node2"); + // Out-of-range timeout should be clamped to the safe default (1.0). + auto transport = std::make_shared(node.get(), 100.0); + auto manager = std::make_unique(transport, 100.0); EXPECT_NEAR(manager->get_topic_sample_timeout(), 1.0, 0.01); } diff --git a/src/ros2_medkit_gateway/test/test_data_access_manager_routing.cpp b/src/ros2_medkit_gateway/test/test_data_access_manager_routing.cpp new file mode 100644 index 000000000..df5ef0e24 --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_data_access_manager_routing.cpp @@ -0,0 +1,127 @@ +// 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 + +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/core/managers/data_access_manager.hpp" +#include "ros2_medkit_gateway/core/transports/topic_transport.hpp" + +namespace ros2_medkit_gateway { +namespace { + +class MockTopicTransport : public TopicTransport { + public: + MockTopicTransport() = default; + ~MockTopicTransport() override = default; + MockTopicTransport(const MockTopicTransport &) = delete; + MockTopicTransport & operator=(const MockTopicTransport &) = delete; + MockTopicTransport(MockTopicTransport &&) = delete; + MockTopicTransport & operator=(MockTopicTransport &&) = delete; + + json publish(const std::string & topic_path, const std::string & msg_type, const json & data, + std::chrono::duration timeout) override { + last_publish_topic_ = topic_path; + last_publish_type_ = msg_type; + last_publish_data_ = data; + last_publish_timeout_ = timeout.count(); + return json{{"status", "published"}, {"topic", topic_path}}; + } + + TopicSample sample(const std::string & topic_name, std::chrono::duration timeout) override { + last_sample_topic_ = topic_name; + last_sample_timeout_ = timeout.count(); + TopicSample r; + r.success = sample_publishers_ > 0; + r.status = (sample_publishers_ > 0) ? "data" : "metadata_only"; + r.topic = topic_name; + r.type = "std_msgs/msg/Float32"; + r.publisher_count = sample_publishers_; + if (r.success) { + r.data = json{{"value", 42.0}}; + } + return r; + } + + std::pair count_publishers_subscribers(const std::string & /*topic*/) const override { + return {sample_publishers_, 0}; + } + + ros2_medkit_serialization::TypeIntrospection * get_type_introspection() const override { + return nullptr; + } + + uint64_t sample_publishers_ = 1; + std::string last_publish_topic_; + std::string last_publish_type_; + json last_publish_data_; + double last_publish_timeout_ = 0.0; + std::string last_sample_topic_; + double last_sample_timeout_ = 0.0; +}; + +} // namespace + +TEST(DataAccessManagerRoutingTest, PublishDelegatesToTransport) { + auto mock = std::make_shared(); + DataAccessManager mgr(mock, 5.0); + auto out = mgr.publish_to_topic("/foo", "std_msgs/msg/Float32", json{{"data", 1.0}}, 2.0); + EXPECT_EQ(mock->last_publish_topic_, "/foo"); + EXPECT_EQ(mock->last_publish_type_, "std_msgs/msg/Float32"); + EXPECT_EQ(mock->last_publish_data_["data"], 1.0); + EXPECT_DOUBLE_EQ(mock->last_publish_timeout_, 2.0); + EXPECT_EQ(out["status"], "published"); +} + +TEST(DataAccessManagerRoutingTest, SampleWithFallbackUsesDefaultTimeout) { + auto mock = std::make_shared(); + DataAccessManager mgr(mock, 7.5); + mgr.get_topic_sample_with_fallback("/bar", -1.0); + EXPECT_DOUBLE_EQ(mock->last_sample_timeout_, 7.5); +} + +TEST(DataAccessManagerRoutingTest, SampleWithFallbackUsesProvidedTimeout) { + auto mock = std::make_shared(); + DataAccessManager mgr(mock, 7.5); + mgr.get_topic_sample_with_fallback("/baz", 3.0); + EXPECT_DOUBLE_EQ(mock->last_sample_timeout_, 3.0); +} + +TEST(DataAccessManagerRoutingTest, NativeSampleWithoutPublishersReturnsMetadataWithoutTransportCall) { + auto mock = std::make_shared(); + mock->sample_publishers_ = 0; + DataAccessManager mgr(mock, 5.0); + auto out = mgr.get_topic_sample_native("/empty", 1.0); + EXPECT_EQ(out["status"], "metadata_only"); + EXPECT_EQ(out["topic"], "/empty"); + EXPECT_EQ(out["publisher_count"].get(), 0u); + EXPECT_TRUE(mock->last_sample_topic_.empty()); // sample() must not be called +} + +TEST(DataAccessManagerRoutingTest, NativeSampleWithPublishersDelegatesAndIncludesData) { + auto mock = std::make_shared(); + mock->sample_publishers_ = 2; + DataAccessManager mgr(mock, 5.0); + auto out = mgr.get_topic_sample_native("/has_pub", 1.0); + EXPECT_EQ(out["status"], "data"); + EXPECT_EQ(out["data"]["value"], 42.0); + EXPECT_EQ(mock->last_sample_topic_, "/has_pub"); +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/test/test_fault_handlers.cpp b/src/ros2_medkit_gateway/test/test_fault_handlers.cpp index f01ea6b9c..1b4b56395 100644 --- a/src/ros2_medkit_gateway/test/test_fault_handlers.cpp +++ b/src/ros2_medkit_gateway/test/test_fault_handlers.cpp @@ -17,6 +17,7 @@ #include #include "ros2_medkit_gateway/http/handlers/fault_handlers.hpp" +#include "ros2_medkit_gateway/ros2/conversions/fault_msg_conversions.hpp" #include "ros2_medkit_msgs/msg/environment_data.hpp" #include "ros2_medkit_msgs/msg/extended_data_records.hpp" #include "ros2_medkit_msgs/msg/fault.hpp" @@ -24,12 +25,20 @@ using json = nlohmann::json; using ros2_medkit_gateway::handlers::FaultHandlers; +namespace conversions = ros2_medkit_gateway::ros2::conversions; + +// The handler now consumes JSON shaped by the transport adapter. These tests +// drive that contract end-to-end by using the same conversions module the +// adapter uses to translate ros2_medkit_msgs into JSON, then call the handler +// to produce the final SOVD response. class FaultHandlersTest : public ::testing::Test { protected: - void SetUp() override { + static json fault_json(const ros2_medkit_msgs::msg::Fault & f) { + return conversions::fault_to_json(f); } - void TearDown() override { + static json env_json(const ros2_medkit_msgs::msg::EnvironmentData & e) { + return conversions::environment_data_to_json(e); } }; @@ -47,7 +56,8 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseBasicFields) { env_data.extended_data_records.first_occurrence_ns = 1707044400000000000; env_data.extended_data_records.last_occurrence_ns = 1707044460000000000; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/motor_controller"); + auto response = + FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/motor_controller"); // Verify item structure EXPECT_EQ(response["item"]["code"], "TEST_FAULT"); @@ -84,7 +94,7 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseWithFreezeFrame) { freeze_frame.captured_at_ns = 1707044400000000000; env_data.snapshots.push_back(freeze_frame); - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/motor"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/motor"); auto & snap = response["environment_data"]["snapshots"][0]; EXPECT_EQ(snap["type"], "freeze_frame"); @@ -104,12 +114,14 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseWithRosbag) { ros2_medkit_msgs::msg::Snapshot rosbag; rosbag.type = "rosbag"; rosbag.name = "fault_recording"; + rosbag.bulk_data_id = "ROSBAG_FAULT"; rosbag.size_bytes = 1234567; rosbag.duration_sec = 6.0; rosbag.format = "mcap"; env_data.snapshots.push_back(rosbag); - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/motor_controller"); + auto response = + FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/motor_controller"); auto & snap = response["environment_data"]["snapshots"][0]; EXPECT_EQ(snap["type"], "rosbag"); @@ -130,7 +142,8 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseNestedEntityPath) { rosbag.bulk_data_id = "NESTED_FAULT"; env_data.snapshots.push_back(rosbag); - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/areas/perception/subareas/lidar"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), + "/areas/perception/subareas/lidar"); auto & snap = response["environment_data"]["snapshots"][0]; EXPECT_EQ(snap["bulk_data_uri"], "/areas/perception/subareas/lidar/bulk-data/rosbags/NESTED_FAULT"); @@ -144,7 +157,7 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseStatusCleared) { ros2_medkit_msgs::msg::EnvironmentData env_data; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); auto status = response["item"]["status"]; EXPECT_EQ(status["aggregatedStatus"], "cleared"); @@ -161,7 +174,7 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseStatusPassive) { ros2_medkit_msgs::msg::EnvironmentData env_data; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); auto status = response["item"]["status"]; EXPECT_EQ(status["aggregatedStatus"], "passive"); @@ -176,42 +189,42 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseSeverityLabels) { { ros2_medkit_msgs::msg::Fault fault; fault.severity = 0; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); EXPECT_EQ(response["x-medkit"]["severity_label"], "DEBUG"); } // Test INFO (1) { ros2_medkit_msgs::msg::Fault fault; fault.severity = 1; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); EXPECT_EQ(response["x-medkit"]["severity_label"], "INFO"); } // Test WARN (2) { ros2_medkit_msgs::msg::Fault fault; fault.severity = 2; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); EXPECT_EQ(response["x-medkit"]["severity_label"], "WARN"); } // Test ERROR (3) { ros2_medkit_msgs::msg::Fault fault; fault.severity = 3; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); EXPECT_EQ(response["x-medkit"]["severity_label"], "ERROR"); } // Test FATAL (4) { ros2_medkit_msgs::msg::Fault fault; fault.severity = 4; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); EXPECT_EQ(response["x-medkit"]["severity_label"], "FATAL"); } // Test UNKNOWN (255) { ros2_medkit_msgs::msg::Fault fault; fault.severity = 255; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); EXPECT_EQ(response["x-medkit"]["severity_label"], "UNKNOWN"); } } @@ -230,7 +243,7 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseWithInvalidJson) { freeze_frame.message_type = "std_msgs/msg/String"; env_data.snapshots.push_back(freeze_frame); - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); auto & snap = response["environment_data"]["snapshots"][0]; EXPECT_EQ(snap["data"], "not valid json {"); // Raw data returned @@ -246,7 +259,7 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseExtendedDataRecords) { env_data.extended_data_records.first_occurrence_ns = 1770458400000000000; // 2026-02-08 env_data.extended_data_records.last_occurrence_ns = 1770458460000000000; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); auto & edr = response["environment_data"]["extended_data_records"]; std::string first = edr["first_occurrence"].get(); @@ -254,9 +267,9 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseExtendedDataRecords) { // Verify ISO 8601 format with milliseconds and Z suffix EXPECT_TRUE(first.find("2026") != std::string::npos); - EXPECT_TRUE(first.find("Z") != std::string::npos); - EXPECT_TRUE(first.find("T") != std::string::npos); - EXPECT_TRUE(last.find("Z") != std::string::npos); + EXPECT_TRUE(first.find('Z') != std::string::npos); + EXPECT_TRUE(first.find('T') != std::string::npos); + EXPECT_TRUE(last.find('Z') != std::string::npos); } // @verifies REQ_INTEROP_013 @@ -273,7 +286,7 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponsePrimaryValueExtraction) { env_data.snapshots.clear(); env_data.snapshots.push_back(snap); - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); EXPECT_DOUBLE_EQ(response["environment_data"]["snapshots"][0]["data"].get(), 42.5); } @@ -287,7 +300,7 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponsePrimaryValueExtraction) { env_data.snapshots.clear(); env_data.snapshots.push_back(snap); - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); auto data = response["environment_data"]["snapshots"][0]["data"]; EXPECT_EQ(data["foo"], "bar"); EXPECT_EQ(data["baz"], 123); @@ -302,7 +315,7 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseMultipleSources) { ros2_medkit_msgs::msg::EnvironmentData env_data; - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/apps/test"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/apps/test"); auto sources = response["x-medkit"]["reporting_sources"]; ASSERT_EQ(sources.size(), 3); @@ -335,7 +348,7 @@ TEST_F(FaultHandlersTest, BuildSovdFaultResponseMixedSnapshots) { rosbag.format = "mcap"; env_data.snapshots.push_back(rosbag); - auto response = FaultHandlers::build_sovd_fault_response(fault, env_data, "/components/motor"); + auto response = FaultHandlers::build_sovd_fault_response(fault_json(fault), env_json(env_data), "/components/motor"); ASSERT_EQ(response["environment_data"]["snapshots"].size(), 2); diff --git a/src/ros2_medkit_gateway/test/test_fault_manager.cpp b/src/ros2_medkit_gateway/test/test_fault_manager.cpp index 1126110bc..a7e65764c 100644 --- a/src/ros2_medkit_gateway/test/test_fault_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_fault_manager.cpp @@ -27,6 +27,7 @@ #include "ros2_medkit_gateway/core/resource_change_notifier.hpp" #include "ros2_medkit_gateway/fault_manager.hpp" #include "ros2_medkit_gateway/fault_manager_paths.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_fault_service_transport.hpp" #include "ros2_medkit_gateway/trigger_fault_subscriber.hpp" #include "ros2_medkit_msgs/msg/fault_event.hpp" #include "ros2_medkit_msgs/srv/get_rosbag.hpp" @@ -125,7 +126,7 @@ TEST(FaultManagerPathsTest, BuildPathsFromNamespaceString) { // @verifies REQ_INTEROP_088 TEST_F(FaultManagerTest, GetSnapshotsServiceNotAvailable) { - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); // Don't create a service, so it will timeout auto result = fault_manager.get_snapshots("TEST_FAULT"); @@ -154,7 +155,7 @@ TEST_F(FaultManagerTest, GetSnapshotsSuccessWithValidJson) { }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); auto result = fault_manager.get_snapshots("MOTOR_OVERHEAT"); @@ -187,7 +188,7 @@ TEST_F(FaultManagerTest, GetSnapshotsUsesConfiguredFaultManagerNamespace) { }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); auto result = fault_manager.get_snapshots("NAMESPACED_FAULT"); @@ -216,7 +217,7 @@ TEST_F(FaultManagerTest, InvalidFaultManagerNamespaceFallsBackToRootServicePath) }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); auto result = fault_manager.get_snapshots("INVALID_NAMESPACE_FAULT"); @@ -237,7 +238,7 @@ TEST_F(FaultManagerTest, GetSnapshotsSuccessWithTopicFilter) { }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); fault_manager.get_snapshots("TEST_FAULT", "/specific_topic"); @@ -254,7 +255,7 @@ TEST_F(FaultManagerTest, GetSnapshotsErrorResponse) { }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); auto result = fault_manager.get_snapshots("NONEXISTENT_FAULT"); @@ -272,7 +273,7 @@ TEST_F(FaultManagerTest, GetSnapshotsInvalidJsonFallback) { }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); auto result = fault_manager.get_snapshots("TEST_FAULT"); @@ -292,7 +293,7 @@ TEST_F(FaultManagerTest, GetSnapshotsEmptyResponse) { }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); auto result = fault_manager.get_snapshots("TEST_FAULT"); @@ -305,7 +306,7 @@ TEST_F(FaultManagerTest, GetSnapshotsEmptyResponse) { // @verifies REQ_INTEROP_088 TEST_F(FaultManagerTest, GetRosbagServiceNotAvailable) { - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); // Don't create a service, so it will timeout auto result = fault_manager.get_rosbag("TEST_FAULT"); @@ -330,7 +331,7 @@ TEST_F(FaultManagerTest, GetRosbagSuccess) { }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); auto result = fault_manager.get_rosbag("TEST_ROSBAG_FAULT"); @@ -363,7 +364,7 @@ TEST_F(FaultManagerTest, GetRosbagUsesConfiguredFaultManagerNamespace) { }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); auto result = fault_manager.get_rosbag("NAMESPACED_ROSBAG"); @@ -576,7 +577,7 @@ TEST_F(FaultManagerTest, GetRosbagNotFound) { }); start_spinning(); - FaultManager fault_manager(node_.get()); + FaultManager fault_manager(std::make_shared(node_.get())); auto result = fault_manager.get_rosbag("NONEXISTENT_FAULT"); diff --git a/src/ros2_medkit_gateway/test/test_fault_manager_routing.cpp b/src/ros2_medkit_gateway/test/test_fault_manager_routing.cpp new file mode 100644 index 000000000..59fd6c5db --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_fault_manager_routing.cpp @@ -0,0 +1,368 @@ +// 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 + +#include +#include +#include + +#include "ros2_medkit_gateway/core/faults/fault_types.hpp" +#include "ros2_medkit_gateway/core/managers/fault_manager.hpp" +#include "ros2_medkit_gateway/core/transports/fault_service_transport.hpp" + +namespace ros2_medkit_gateway { +namespace { + +class MockFaultServiceTransport : public FaultServiceTransport { + public: + MockFaultServiceTransport() = default; + ~MockFaultServiceTransport() override = default; + MockFaultServiceTransport(const MockFaultServiceTransport &) = delete; + MockFaultServiceTransport & operator=(const MockFaultServiceTransport &) = delete; + MockFaultServiceTransport(MockFaultServiceTransport &&) = delete; + MockFaultServiceTransport & operator=(MockFaultServiceTransport &&) = delete; + + FaultResult report_fault(const std::string & fault_code, uint8_t severity, const std::string & description, + const std::string & source_id) override { + last_report_code_ = fault_code; + last_report_severity_ = severity; + last_report_description_ = description; + last_report_source_ = source_id; + ++report_calls_; + FaultResult r; + r.success = report_success_; + r.data = report_data_; + r.error_message = report_error_; + return r; + } + + FaultResult list_faults(const std::string & source_id, bool include_prefailed, bool include_confirmed, + bool include_cleared, bool include_healed, bool include_muted, + bool include_clusters) override { + last_list_source_ = source_id; + last_include_prefailed_ = include_prefailed; + last_include_confirmed_ = include_confirmed; + last_include_cleared_ = include_cleared; + last_include_healed_ = include_healed; + last_include_muted_ = include_muted; + last_include_clusters_ = include_clusters; + ++list_calls_; + FaultResult r; + r.success = list_success_; + r.data = list_data_; + r.error_message = list_error_; + return r; + } + + FaultWithEnvJsonResult get_fault_with_env(const std::string & fault_code, const std::string & source_id) override { + last_get_env_code_ = fault_code; + last_get_env_source_ = source_id; + ++get_env_calls_; + FaultWithEnvJsonResult r; + r.success = get_env_success_; + r.data = get_env_data_; + r.error_message = get_env_error_; + return r; + } + + FaultResult get_fault(const std::string & fault_code, const std::string & source_id) override { + last_get_code_ = fault_code; + last_get_source_ = source_id; + ++get_calls_; + FaultResult r; + r.success = get_success_; + r.data = get_data_; + r.error_message = get_error_; + return r; + } + + FaultResult clear_fault(const std::string & fault_code) override { + last_clear_code_ = fault_code; + ++clear_calls_; + FaultResult r; + r.success = clear_success_; + r.data = clear_data_; + r.error_message = clear_error_; + return r; + } + + FaultResult get_snapshots(const std::string & fault_code, const std::string & topic) override { + last_snapshots_code_ = fault_code; + last_snapshots_topic_ = topic; + ++snapshots_calls_; + FaultResult r; + r.success = snapshots_success_; + r.data = snapshots_data_; + r.error_message = snapshots_error_; + return r; + } + + FaultResult get_rosbag(const std::string & fault_code) override { + last_rosbag_code_ = fault_code; + ++rosbag_calls_; + FaultResult r; + r.success = rosbag_success_; + r.data = rosbag_data_; + r.error_message = rosbag_error_; + return r; + } + + FaultResult list_rosbags(const std::string & entity_fqn) override { + last_list_rosbags_entity_ = entity_fqn; + ++list_rosbags_calls_; + FaultResult r; + r.success = list_rosbags_success_; + r.data = list_rosbags_data_; + r.error_message = list_rosbags_error_; + return r; + } + + bool wait_for_services(std::chrono::duration timeout) override { + last_wait_timeout_ = timeout.count(); + ++wait_calls_; + return wait_result_; + } + + bool is_available() const override { + ++availability_calls_; + return is_available_; + } + + // Routing knobs / observation fields --------------------------------------- + bool report_success_ = true; + json report_data_ = json{{"accepted", true}}; + std::string report_error_; + std::string last_report_code_; + uint8_t last_report_severity_ = 0; + std::string last_report_description_; + std::string last_report_source_; + int report_calls_ = 0; + + bool list_success_ = true; + json list_data_ = json{{"faults", json::array()}, {"count", 0}}; + std::string list_error_; + std::string last_list_source_; + bool last_include_prefailed_ = false; + bool last_include_confirmed_ = false; + bool last_include_cleared_ = false; + bool last_include_healed_ = false; + bool last_include_muted_ = false; + bool last_include_clusters_ = false; + int list_calls_ = 0; + + bool get_env_success_ = true; + json get_env_data_; + std::string get_env_error_; + std::string last_get_env_code_; + std::string last_get_env_source_; + int get_env_calls_ = 0; + + bool get_success_ = true; + json get_data_; + std::string get_error_; + std::string last_get_code_; + std::string last_get_source_; + int get_calls_ = 0; + + bool clear_success_ = true; + json clear_data_ = json{{"success", true}, {"message", "ok"}}; + std::string clear_error_; + std::string last_clear_code_; + int clear_calls_ = 0; + + bool snapshots_success_ = true; + json snapshots_data_ = json::object(); + std::string snapshots_error_; + std::string last_snapshots_code_; + std::string last_snapshots_topic_; + int snapshots_calls_ = 0; + + bool rosbag_success_ = true; + json rosbag_data_ = json::object(); + std::string rosbag_error_; + std::string last_rosbag_code_; + int rosbag_calls_ = 0; + + bool list_rosbags_success_ = true; + json list_rosbags_data_ = json{{"rosbags", json::array()}}; + std::string list_rosbags_error_; + std::string last_list_rosbags_entity_; + int list_rosbags_calls_ = 0; + + bool wait_result_ = true; + double last_wait_timeout_ = 0.0; + int wait_calls_ = 0; + + bool is_available_ = true; + mutable int availability_calls_ = 0; +}; + +} // namespace + +TEST(FaultManagerRoutingTest, ReportFaultDelegatesAllArgsToTransport) { + auto mock = std::make_shared(); + FaultManager mgr(mock); + + auto r = mgr.report_fault("MOTOR_OVERHEAT", 3u, "engine reported overheat", "/powertrain/engine"); + + EXPECT_TRUE(r.success); + EXPECT_EQ(mock->report_calls_, 1); + EXPECT_EQ(mock->last_report_code_, "MOTOR_OVERHEAT"); + EXPECT_EQ(mock->last_report_severity_, 3u); + EXPECT_EQ(mock->last_report_description_, "engine reported overheat"); + EXPECT_EQ(mock->last_report_source_, "/powertrain/engine"); +} + +TEST(FaultManagerRoutingTest, ReportFaultPropagatesErrorMessage) { + auto mock = std::make_shared(); + mock->report_success_ = false; + mock->report_error_ = "ReportFault service not available"; + FaultManager mgr(mock); + + auto r = mgr.report_fault("X", 0u, "", ""); + + EXPECT_FALSE(r.success); + EXPECT_EQ(r.error_message, "ReportFault service not available"); +} + +TEST(FaultManagerRoutingTest, ListFaultsForwardsAllStatusFlags) { + auto mock = std::make_shared(); + FaultManager mgr(mock); + + auto r = mgr.list_faults("/cell_a", /*include_prefailed=*/true, /*include_confirmed=*/true, + /*include_cleared=*/true, /*include_healed=*/true, /*include_muted=*/true, + /*include_clusters=*/true); + + EXPECT_TRUE(r.success); + EXPECT_EQ(mock->list_calls_, 1); + EXPECT_EQ(mock->last_list_source_, "/cell_a"); + EXPECT_TRUE(mock->last_include_prefailed_); + EXPECT_TRUE(mock->last_include_confirmed_); + EXPECT_TRUE(mock->last_include_cleared_); + EXPECT_TRUE(mock->last_include_healed_); + EXPECT_TRUE(mock->last_include_muted_); + EXPECT_TRUE(mock->last_include_clusters_); +} + +TEST(FaultManagerRoutingTest, GetFaultWithEnvReturnsTransportJsonShape) { + auto mock = std::make_shared(); + // Shape the transport returns: { "fault": {...}, "environment_data": {...} } + // (rosbag snapshot intentionally omits bulk_data_uri - the handler adds it.) + mock->get_env_data_ = {{"fault", + {{"fault_code", "F1"}, + {"severity", 2}, + {"severity_label", "WARN"}, + {"status", "CONFIRMED"}, + {"description", "test"}}}, + {"environment_data", + {{"extended_data_records", {{"first_occurrence", "2026-01-01T00:00:00.000Z"}}}, + {"snapshots", json::array({{{"type", "rosbag"}, + {"name", "F1.bag"}, + {"fault_code", "F1"}, + {"size_bytes", 4096}, + {"duration_sec", 5.0}, + {"format", "mcap"}}})}}}}; + FaultManager mgr(mock); + + auto r = mgr.get_fault_with_env("F1", "/cell_a"); + + EXPECT_TRUE(r.success); + EXPECT_EQ(mock->last_get_env_code_, "F1"); + EXPECT_EQ(mock->last_get_env_source_, "/cell_a"); + ASSERT_TRUE(r.data.contains("fault")); + ASSERT_TRUE(r.data.contains("environment_data")); + EXPECT_EQ(r.data["fault"]["fault_code"], "F1"); + ASSERT_TRUE(r.data["environment_data"]["snapshots"].is_array()); + ASSERT_EQ(r.data["environment_data"]["snapshots"].size(), 1u); + // Critical: the transport must NOT have populated bulk_data_uri at this stage. + EXPECT_FALSE(r.data["environment_data"]["snapshots"][0].contains("bulk_data_uri")); +} + +TEST(FaultManagerRoutingTest, GetFaultWithEnvPropagatesErrorMessage) { + auto mock = std::make_shared(); + mock->get_env_success_ = false; + mock->get_env_error_ = "Fault not found"; + FaultManager mgr(mock); + + auto r = mgr.get_fault_with_env("MISSING"); + + EXPECT_FALSE(r.success); + EXPECT_EQ(r.error_message, "Fault not found"); + EXPECT_TRUE(r.data.empty()); +} + +TEST(FaultManagerRoutingTest, ClearFaultDelegatesAndReturnsAutoClearedCodes) { + auto mock = std::make_shared(); + mock->clear_data_ = {{"success", true}, {"message", "ok"}, {"auto_cleared_codes", json::array({"S1", "S2"})}}; + FaultManager mgr(mock); + + auto r = mgr.clear_fault("ROOT"); + + EXPECT_TRUE(r.success); + EXPECT_EQ(mock->last_clear_code_, "ROOT"); + ASSERT_TRUE(r.data.contains("auto_cleared_codes")); + EXPECT_EQ(r.data["auto_cleared_codes"].size(), 2u); +} + +TEST(FaultManagerRoutingTest, GetSnapshotsRoutesTopicFilter) { + auto mock = std::make_shared(); + mock->snapshots_data_ = {{"topics", json::object()}}; + FaultManager mgr(mock); + + auto r = mgr.get_snapshots("F1", "/joint_states"); + + EXPECT_TRUE(r.success); + EXPECT_EQ(mock->last_snapshots_code_, "F1"); + EXPECT_EQ(mock->last_snapshots_topic_, "/joint_states"); +} + +TEST(FaultManagerRoutingTest, GetRosbagPropagatesErrorMessage) { + auto mock = std::make_shared(); + mock->rosbag_success_ = false; + mock->rosbag_error_ = "No rosbag file available for fault"; + FaultManager mgr(mock); + + auto r = mgr.get_rosbag("F1"); + + EXPECT_FALSE(r.success); + EXPECT_EQ(mock->last_rosbag_code_, "F1"); + EXPECT_EQ(r.error_message, "No rosbag file available for fault"); +} + +TEST(FaultManagerRoutingTest, ListRosbagsForwardsEntityFqn) { + auto mock = std::make_shared(); + FaultManager mgr(mock); + + auto r = mgr.list_rosbags("/sensors/lidar"); + + EXPECT_TRUE(r.success); + EXPECT_EQ(mock->last_list_rosbags_entity_, "/sensors/lidar"); +} + +TEST(FaultManagerRoutingTest, IsAvailableAndWaitForwardToTransport) { + auto mock = std::make_shared(); + mock->is_available_ = true; + mock->wait_result_ = true; + FaultManager mgr(mock); + + EXPECT_TRUE(mgr.is_available()); + EXPECT_GE(mock->availability_calls_, 1); + + EXPECT_TRUE(mgr.wait_for_services(std::chrono::duration(2.5))); + EXPECT_EQ(mock->wait_calls_, 1); + EXPECT_DOUBLE_EQ(mock->last_wait_timeout_, 2.5); +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp index 106ba80b3..93cc79f3d 100644 --- a/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp +++ b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp @@ -22,6 +22,13 @@ #include "ros2_medkit_gateway/core/discovery/models/area.hpp" #include "ros2_medkit_gateway/core/discovery/models/component.hpp" #include "ros2_medkit_gateway/core/discovery/models/function.hpp" +#include "ros2_medkit_gateway/core/discovery/service_action_resolver.hpp" +#include "ros2_medkit_gateway/core/managers/configuration_manager.hpp" +#include "ros2_medkit_gateway/core/managers/data_access_manager.hpp" +#include "ros2_medkit_gateway/core/managers/fault_manager.hpp" +#include "ros2_medkit_gateway/core/managers/log_manager.hpp" +#include "ros2_medkit_gateway/core/managers/operation_manager.hpp" +#include "ros2_medkit_gateway/core/managers/trigger_manager.hpp" #include "ros2_medkit_gateway/core/providers/data_provider.hpp" #include "ros2_medkit_gateway/core/providers/fault_provider.hpp" #include "ros2_medkit_gateway/core/providers/host_info_provider.hpp" @@ -30,6 +37,13 @@ #include "ros2_medkit_gateway/core/providers/operation_provider.hpp" #include "ros2_medkit_gateway/core/providers/script_provider.hpp" #include "ros2_medkit_gateway/core/providers/update_provider.hpp" +#include "ros2_medkit_gateway/core/transports/action_transport.hpp" +#include "ros2_medkit_gateway/core/transports/fault_service_transport.hpp" +#include "ros2_medkit_gateway/core/transports/log_source.hpp" +#include "ros2_medkit_gateway/core/transports/parameter_transport.hpp" +#include "ros2_medkit_gateway/core/transports/service_transport.hpp" +#include "ros2_medkit_gateway/core/transports/topic_subscription_transport.hpp" +#include "ros2_medkit_gateway/core/transports/topic_transport.hpp" #include @@ -41,23 +55,45 @@ namespace { // above are not flagged as unused. The translation unit must still link // against gateway_core alone, which proves the neutral-layer contract. +using ros2_medkit_gateway::ActionTransport; using ros2_medkit_gateway::App; using ros2_medkit_gateway::Area; using ros2_medkit_gateway::Component; +using ros2_medkit_gateway::ConfigurationManager; +using ros2_medkit_gateway::DataAccessManager; using ros2_medkit_gateway::DataProvider; +using ros2_medkit_gateway::FaultManager; using ros2_medkit_gateway::FaultProvider; +using ros2_medkit_gateway::FaultServiceTransport; using ros2_medkit_gateway::Function; using ros2_medkit_gateway::HostInfoProvider; using ros2_medkit_gateway::IntrospectionProvider; +using ros2_medkit_gateway::LogManager; using ros2_medkit_gateway::LogProvider; +using ros2_medkit_gateway::LogSource; +using ros2_medkit_gateway::OperationManager; using ros2_medkit_gateway::OperationProvider; +using ros2_medkit_gateway::ParameterTransport; using ros2_medkit_gateway::ScriptProvider; +using ros2_medkit_gateway::ServiceActionResolver; +using ros2_medkit_gateway::ServiceTransport; +using ros2_medkit_gateway::TopicSubscriptionHandle; +using ros2_medkit_gateway::TopicSubscriptionTransport; +using ros2_medkit_gateway::TopicTransport; +using ros2_medkit_gateway::TriggerManager; using ros2_medkit_gateway::UpdateProvider; static_assert(sizeof(Area) > 0); static_assert(sizeof(Component) > 0); static_assert(sizeof(App) > 0); static_assert(sizeof(Function) > 0); +static_assert(sizeof(ConfigurationManager) > 0); +static_assert(sizeof(DataAccessManager) > 0); +static_assert(sizeof(FaultManager) > 0); +static_assert(sizeof(LogManager) > 0); +static_assert(sizeof(OperationManager) > 0); +static_assert(sizeof(TriggerManager) > 0); +static_assert(std::is_abstract_v); static_assert(std::is_abstract_v); static_assert(std::is_abstract_v); static_assert(std::is_abstract_v); @@ -66,6 +102,16 @@ static_assert(std::is_abstract_v); static_assert(std::is_abstract_v); static_assert(std::is_abstract_v); static_assert(sizeof(HostInfoProvider) > 0); +static_assert(std::is_abstract_v); +static_assert(std::is_abstract_v); +static_assert(std::is_abstract_v); +static_assert(std::is_abstract_v); +static_assert(std::is_abstract_v); +static_assert(std::is_abstract_v); +static_assert(std::is_abstract_v); +// TopicSubscriptionHandle is a polymorphic RAII base (virtual destructor only) +// rather than an abstract port - subclasses add the resource being managed. +static_assert(sizeof(TopicSubscriptionHandle) > 0); } // namespace diff --git a/src/ros2_medkit_gateway/test/test_log_manager.cpp b/src/ros2_medkit_gateway/test/test_log_manager.cpp index 839a57093..763b552a8 100644 --- a/src/ros2_medkit_gateway/test/test_log_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_log_manager.cpp @@ -26,6 +26,7 @@ #include "ros2_medkit_gateway/core/providers/log_provider.hpp" #include "ros2_medkit_gateway/core/resource_change_notifier.hpp" #include "ros2_medkit_gateway/log_manager.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_log_source.hpp" using json = nlohmann::json; using ros2_medkit_gateway::LogConfig; @@ -168,7 +169,8 @@ class LogManagerBufferTest : public ::testing::Test { rclcpp::init(0, nullptr); node_ = std::make_shared("test_log_manager"); // Small buffer size of 3 for easy eviction testing - mgr_ = std::make_unique(node_.get(), nullptr, /*max_buffer_size=*/3); + mgr_ = std::make_unique(std::make_shared(node_.get()), + nullptr, /*max_buffer_size=*/3); } void TearDown() override { @@ -594,7 +596,8 @@ TEST_F(LogManagerIngestionTest, ManagesIngestionDelegatesToPlugin) { auto * raw = plugin.get(); plugin_mgr_->add_plugin(std::move(plugin)); - mgr_ = std::make_unique(node_.get(), plugin_mgr_.get(), 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), + plugin_mgr_.get(), 10); auto result = mgr_->get_logs({"/my_node"}, false, "", "", "entity1"); EXPECT_TRUE(raw->get_logs_called); @@ -611,7 +614,8 @@ TEST_F(LogManagerIngestionTest, ManagesIngestionUpdateConfigDelegatesToPlugin) { auto * raw = plugin.get(); plugin_mgr_->add_plugin(std::move(plugin)); - mgr_ = std::make_unique(node_.get(), plugin_mgr_.get(), 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), + plugin_mgr_.get(), 10); auto err = mgr_->update_config("entity1", std::string("error"), std::nullopt); EXPECT_TRUE(err.empty()); @@ -628,7 +632,8 @@ TEST_F(LogManagerIngestionTest, ManagesIngestionGetConfigDelegatesToPlugin) { raw->configs["entity1"] = LogConfig{"warning", 50}; plugin_mgr_->add_plugin(std::move(plugin)); - mgr_ = std::make_unique(node_.get(), plugin_mgr_.get(), 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), + plugin_mgr_.get(), 10); auto cfg = mgr_->get_config("entity1"); EXPECT_TRUE(raw->get_config_called); @@ -645,7 +650,8 @@ TEST_F(LogManagerIngestionTest, ManagesIngestionLocalBufferBypassed) { plugin->canned_entries.clear(); plugin_mgr_->add_plugin(std::move(plugin)); - mgr_ = std::make_unique(node_.get(), plugin_mgr_.get(), 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), + plugin_mgr_.get(), 10); // Inject entries into local buffer - these should be invisible because plugin owns queries mgr_->inject_entry_for_testing(make_entry(1, "my_node")); @@ -664,7 +670,8 @@ TEST_F(LogManagerIngestionTest, DefaultManagesIngestionPreservesCurrentBehavior) auto * raw = plugin.get(); plugin_mgr_->add_plugin(std::move(plugin)); - mgr_ = std::make_unique(node_.get(), plugin_mgr_.get(), 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), + plugin_mgr_.get(), 10); auto result = mgr_->get_logs({"/node1"}, false, "", "", ""); // Passive plugin still receives get_logs() delegation @@ -678,7 +685,8 @@ TEST_F(LogManagerIngestionTest, DefaultManagesIngestionPreservesCurrentBehavior) // @verifies REQ_INTEROP_061 TEST_F(LogManagerIngestionTest, NoPluginPreservesDefaultBehavior) { // No plugin manager at all - ring buffer works as before - mgr_ = std::make_unique(node_.get(), nullptr, 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), nullptr, + 10); mgr_->inject_entry_for_testing(make_entry(1, "my_node")); mgr_->inject_entry_for_testing(make_entry(2, "my_node")); @@ -697,7 +705,8 @@ TEST_F(LogManagerIngestionTest, ManagesIngestionStillValidatesBeforeDelegation) auto * raw = plugin.get(); plugin_mgr_->add_plugin(std::move(plugin)); - mgr_ = std::make_unique(node_.get(), plugin_mgr_.get(), 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), + plugin_mgr_.get(), 10); // LogManager validates severity before delegating - "verbose" is invalid auto err = mgr_->update_config("e", std::string("verbose"), std::nullopt); @@ -714,7 +723,8 @@ TEST_F(LogManagerIngestionTest, ManagesIngestionStillValidatesBeforeDelegation) TEST_F(LogManagerIngestionTest, PluginGetLogsThrowReturnsError) { plugin_mgr_ = std::make_unique(); plugin_mgr_->add_plugin(std::make_unique()); - mgr_ = std::make_unique(node_.get(), plugin_mgr_.get(), 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), + plugin_mgr_.get(), 10); auto result = mgr_->get_logs({"/node"}, false, "", "", ""); ASSERT_FALSE(result.has_value()); @@ -725,7 +735,8 @@ TEST_F(LogManagerIngestionTest, PluginGetLogsThrowReturnsError) { TEST_F(LogManagerIngestionTest, PluginGetConfigThrowReturnsError) { plugin_mgr_ = std::make_unique(); plugin_mgr_->add_plugin(std::make_unique()); - mgr_ = std::make_unique(node_.get(), plugin_mgr_.get(), 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), + plugin_mgr_.get(), 10); auto result = mgr_->get_config("entity1"); ASSERT_FALSE(result.has_value()); @@ -743,7 +754,8 @@ class LogManagerResolverTest : public ::testing::Test { rclcpp::init(0, nullptr); node_ = std::make_shared("test_log_resolver"); notifier_ = std::make_unique(); - mgr_ = std::make_unique(node_.get(), nullptr, 10); + mgr_ = std::make_unique(std::make_shared(node_.get()), + nullptr, 10); mgr_->set_notifier(notifier_.get()); // Subscribe to notifier to capture entity IDs @@ -883,7 +895,8 @@ TEST_F(LogManagerBufferTest, BufferCapDropsNewNodesWhenFull) { // Buffer cap = size * 10 = 30 distinct nodes. // Create a fresh manager with buffer size 5 so cap = 50. mgr_.reset(); - mgr_ = std::make_unique(node_.get(), nullptr, /*max_buffer_size=*/5); + mgr_ = std::make_unique(std::make_shared(node_.get()), nullptr, + /*max_buffer_size=*/5); // Fill to the cap: 50 distinct nodes for (int i = 0; i < 50; ++i) { diff --git a/src/ros2_medkit_gateway/test/test_log_manager_routing.cpp b/src/ros2_medkit_gateway/test/test_log_manager_routing.cpp new file mode 100644 index 000000000..cda99585a --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_log_manager_routing.cpp @@ -0,0 +1,312 @@ +// 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. + +// LogManager routing test: links exclusively against gateway_core + GTest. +// No rclcpp/ament dependencies on the link line, proving that the manager +// body lives in the middleware-neutral build layer. The /rosout subscription +// and the rcl_interfaces::msg::Log -> LogEntry conversion are provided by the +// Ros2LogSource adapter; here we exercise the manager against a mock source +// that emits LogEntry directly. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/core/log_types.hpp" +#include "ros2_medkit_gateway/core/managers/log_manager.hpp" +#include "ros2_medkit_gateway/core/providers/log_provider.hpp" +#include "ros2_medkit_gateway/core/providers/log_provider_registry.hpp" +#include "ros2_medkit_gateway/core/transports/log_source.hpp" + +namespace ros2_medkit_gateway { +namespace { + +/// Mock LogSource that records start()/stop() calls and exposes a manual +/// emit() helper so tests can route LogEntry instances through the manager +/// without any middleware dependency. +class MockLogSource : public LogSource { + public: + MockLogSource() = default; + ~MockLogSource() override = default; + MockLogSource(const MockLogSource &) = delete; + MockLogSource & operator=(const MockLogSource &) = delete; + MockLogSource(MockLogSource &&) = delete; + MockLogSource & operator=(MockLogSource &&) = delete; + + void start(EntryCallback callback) override { + ++start_calls_; + callback_ = std::move(callback); + } + + void stop() override { + ++stop_calls_; + callback_ = nullptr; + } + + /// Test-side helper: emit a LogEntry as if rclcpp had just delivered one. + /// Returns false if the manager never started the source (in which case + /// the entry would never have been delivered in the real adapter either). + bool emit(const LogEntry & entry) { + if (!callback_) { + return false; + } + callback_(entry); + return true; + } + + int start_calls() const { + return start_calls_; + } + + int stop_calls() const { + return stop_calls_; + } + + private: + EntryCallback callback_; + int start_calls_ = 0; + int stop_calls_ = 0; +}; + +/// Helper: construct a LogEntry without an id (the manager assigns one). +LogEntry make_entry(const std::string & name, uint8_t level, const std::string & msg) { + LogEntry e{}; + e.id = 0; + e.stamp_sec = 1000; + e.stamp_nanosec = 0; + e.level = level; + e.name = name; + e.msg = msg; + return e; +} + +/// LogProvider that fully owns ingestion (manages_ingestion() == true). +class MockIngestionProvider : public LogProvider { + public: + bool manages_ingestion() const override { + return true; + } + + std::vector get_logs(const std::vector & /*node_fqns*/, bool /*prefix_match*/, + const std::string & /*min_severity*/, const std::string & /*context_filter*/, + const std::string & /*entity_id*/) override { + return {}; + } + + LogConfig get_config(const std::string & /*entity_id*/) const override { + return LogConfig{}; + } + + std::string update_config(const std::string & /*entity_id*/, const std::optional & /*severity_filter*/, + const std::optional & /*max_entries*/) override { + return ""; + } +}; + +/// Observer-mode LogProvider: counts on_log_entry() calls and remembers the +/// most recent entry so the routing test can assert the manager forwards. +class MockObserverProvider : public LogProvider { + public: + std::vector get_logs(const std::vector & /*node_fqns*/, bool /*prefix_match*/, + const std::string & /*min_severity*/, const std::string & /*context_filter*/, + const std::string & /*entity_id*/) override { + return {}; + } + + LogConfig get_config(const std::string & /*entity_id*/) const override { + return LogConfig{}; + } + + std::string update_config(const std::string & /*entity_id*/, const std::optional & /*severity_filter*/, + const std::optional & /*max_entries*/) override { + return ""; + } + + bool on_log_entry(const LogEntry & entry) override { + ++calls_; + last_name_ = entry.name; + last_msg_ = entry.msg; + return false; + } + + int calls() const { + return calls_; + } + + const std::string & last_name() const { + return last_name_; + } + + const std::string & last_msg() const { + return last_msg_; + } + + private: + int calls_ = 0; + std::string last_name_; + std::string last_msg_; +}; + +/// Trivial LogProviderRegistry adapter: holds a primary provider + a flat +/// observer list. Mirrors what PluginManager exposes through the registry +/// port without pulling in the rclcpp-coupled implementation. +class MockProviderRegistry : public LogProviderRegistry { + public: + void set_primary(LogProvider * primary) { + primary_ = primary; + } + + void add_observer(LogProvider * observer) { + observers_.push_back(observer); + } + + LogProvider * primary_log_provider() const override { + return primary_; + } + + std::vector log_observers() const override { + return observers_; + } + + private: + LogProvider * primary_ = nullptr; + std::vector observers_; +}; + +} // namespace + +// ============================================================ +// Routing tests +// ============================================================ + +TEST(LogManagerRouting, SourceEmittedEntryRoutesToBuffer) { + auto source = std::make_shared(); + LogManager manager(source, /*plugin_mgr=*/nullptr, /*max_buffer_size=*/10); + EXPECT_EQ(source->start_calls(), 1); + + ASSERT_TRUE(source->emit(make_entry("my_node", /*level=*/20, "hello"))); + + auto result = manager.get_logs({"/my_node"}, /*prefix_match=*/false, "", "", ""); + ASSERT_TRUE(result.has_value()); + ASSERT_EQ(result->size(), 1u); + EXPECT_EQ((*result)[0]["context"]["node"], "my_node"); + EXPECT_EQ((*result)[0]["message"], "hello"); + // Manager assigns ids monotonically starting at 1. + EXPECT_EQ((*result)[0]["id"], "log_1"); +} + +TEST(LogManagerRouting, ManagesIngestionTrueSkipsSourceStart) { + MockIngestionProvider ingestion; + MockProviderRegistry registry; + registry.set_primary(&ingestion); + + auto source = std::make_shared(); + LogManager manager(source, ®istry, /*max_buffer_size=*/10); + + // Primary LogProvider declares manages_ingestion() -> manager must NOT have + // started the source (preserving today's "no /rosout subscription created" + // behaviour for full-ingestion plugins). + EXPECT_EQ(source->start_calls(), 0); +} + +TEST(LogManagerRouting, ManagesIngestionFalseStartsSource) { + MockObserverProvider observer; + MockProviderRegistry registry; + registry.set_primary(&observer); + registry.add_observer(&observer); + + auto source = std::make_shared(); + LogManager manager(source, ®istry, /*max_buffer_size=*/10); + + // Primary LogProvider is observer-mode (default manages_ingestion()=false) + // so the manager must call start() exactly once. + EXPECT_EQ(source->start_calls(), 1); +} + +TEST(LogManagerRouting, PluginObserverFiresOnEachEntry) { + MockObserverProvider observer1; + MockObserverProvider observer2; + MockProviderRegistry registry; + registry.add_observer(&observer1); + registry.add_observer(&observer2); + + auto source = std::make_shared(); + LogManager manager(source, ®istry, /*max_buffer_size=*/10); + + ASSERT_TRUE(source->emit(make_entry("alpha", /*level=*/30, "first"))); + ASSERT_TRUE(source->emit(make_entry("beta", /*level=*/40, "second"))); + + EXPECT_EQ(observer1.calls(), 2); + EXPECT_EQ(observer2.calls(), 2); + EXPECT_EQ(observer1.last_name(), "beta"); + EXPECT_EQ(observer1.last_msg(), "second"); + EXPECT_EQ(observer2.last_name(), "beta"); + EXPECT_EQ(observer2.last_msg(), "second"); +} + +TEST(LogManagerRouting, RingBufferOverflowEvictsOldest) { + auto source = std::make_shared(); + // Tiny ring buffer to exercise eviction quickly. + LogManager manager(source, /*plugin_mgr=*/nullptr, /*max_buffer_size=*/3); + + for (int i = 0; i < 5; ++i) { + ASSERT_TRUE(source->emit(make_entry("n", /*level=*/20, "msg" + std::to_string(i)))); + } + + auto result = manager.get_logs({"/n"}, /*prefix_match=*/false, "", "", ""); + ASSERT_TRUE(result.has_value()); + ASSERT_EQ(result->size(), 3u); + // Manager assigns ids 1..5; oldest 2 must be gone (3, 4, 5 remain). + EXPECT_EQ((*result)[0]["id"], "log_3"); + EXPECT_EQ((*result)[1]["id"], "log_4"); + EXPECT_EQ((*result)[2]["id"], "log_5"); +} + +TEST(LogManagerRouting, SeverityFilterApplied) { + auto source = std::make_shared(); + LogManager manager(source, /*plugin_mgr=*/nullptr, /*max_buffer_size=*/10); + + ASSERT_TRUE(source->emit(make_entry("n", /*level=*/10, "debug-msg"))); // debug + ASSERT_TRUE(source->emit(make_entry("n", /*level=*/20, "info-msg"))); // info + ASSERT_TRUE(source->emit(make_entry("n", /*level=*/30, "warning-msg"))); // warning + ASSERT_TRUE(source->emit(make_entry("n", /*level=*/40, "error-msg"))); // error + + // min_severity=warning -> only warning + error returned. + auto result = manager.get_logs({"/n"}, /*prefix_match=*/false, "warning", "", ""); + ASSERT_TRUE(result.has_value()); + ASSERT_EQ(result->size(), 2u); + EXPECT_EQ((*result)[0]["severity"], "warning"); + EXPECT_EQ((*result)[1]["severity"], "error"); +} + +TEST(LogManagerRouting, ShutdownStopsSource) { + auto source = std::make_shared(); + { + LogManager manager(source, /*plugin_mgr=*/nullptr, /*max_buffer_size=*/10); + EXPECT_EQ(source->start_calls(), 1); + EXPECT_EQ(source->stop_calls(), 0); + } // manager destructs here + + // Manager destructor must call source->stop() so the adapter can release + // its rclcpp subscription deterministically. + EXPECT_GE(source->stop_calls(), 1); +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/test/test_operation_handlers.cpp b/src/ros2_medkit_gateway/test/test_operation_handlers.cpp index 95eb0cf8d..6b2072d11 100644 --- a/src/ros2_medkit_gateway/test/test_operation_handlers.cpp +++ b/src/ros2_medkit_gateway/test/test_operation_handlers.cpp @@ -108,12 +108,17 @@ class TestLongCalibrationActionServer : public rclcpp::Node { using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; TestLongCalibrationActionServer() : rclcpp::Node("test_long_calibration_action", "/powertrain/engine") { - using namespace std::placeholders; - action_server_ = rclcpp_action::create_server( - this, "long_calibration", std::bind(&TestLongCalibrationActionServer::handle_goal, this, _1, _2), - std::bind(&TestLongCalibrationActionServer::handle_cancel, this, _1), - std::bind(&TestLongCalibrationActionServer::handle_accepted, this, _1)); + this, "long_calibration", + [this](const rclcpp_action::GoalUUID & uuid, const std::shared_ptr & goal) { + return handle_goal(uuid, goal); + }, + [this](const std::shared_ptr & goal_handle) { + return handle_cancel(goal_handle); + }, + [this](const std::shared_ptr & goal_handle) { + handle_accepted(goal_handle); + }); } void prepare_shutdown() { @@ -125,26 +130,26 @@ class TestLongCalibrationActionServer : public rclcpp::Node { } private: - rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &, - std::shared_ptr goal) { + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & /*uuid*/, + const std::shared_ptr & goal) { if (goal->order > 50) { return rclcpp_action::GoalResponse::REJECT; } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } - rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr) { + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr & /*goal_handle*/) { return rclcpp_action::CancelResponse::ACCEPT; } - void handle_accepted(const std::shared_ptr goal_handle) { + void handle_accepted(const std::shared_ptr & goal_handle) { if (execution_thread_.joinable()) { execution_thread_.join(); } execution_thread_ = std::thread(&TestLongCalibrationActionServer::execute, this, goal_handle); } - void execute(const std::shared_ptr goal_handle) { + void execute(const std::shared_ptr & goal_handle) { auto feedback = std::make_shared(); auto result = std::make_shared(); const auto goal = goal_handle->get_goal(); @@ -261,8 +266,8 @@ class OperationHandlersFixtureTest : public ::testing::Test { service_node_ = std::make_shared("test_calibrate_service", "/powertrain/engine"); trigger_service_ = service_node_->create_service( - "calibrate", [](const std::shared_ptr, - std::shared_ptr response) { + "calibrate", [](const std::shared_ptr & /*request*/, + const std::shared_ptr & response) { response->success = true; response->message = "calibration complete"; }); @@ -277,8 +282,6 @@ class OperationHandlersFixtureTest : public ::testing::Test { executor_->spin(); }); - seed_component_cache(); - ctx_ = std::make_unique(gateway_node_.get(), cors_, auth_, tls_, nullptr); handlers_ = std::make_unique(*ctx_); @@ -287,6 +290,15 @@ class OperationHandlersFixtureTest : public ::testing::Test { // (_cancel_goal, _get_result, _status). On slow CI runners under // parallel load, 200ms was insufficient. std::this_thread::sleep_for(1s); + + // Seed the component cache AFTER discovery has settled. The gateway's + // graph-event-driven `refresh_cache()` fires whenever the executor + // observes a graph change (adding service_node_ / action_server_node_ + // produces several such events), and each refresh pass calls + // `cache.update_all(...)` from the gateway's own discovery view - + // which would otherwise wipe a pre-spin seed. Seeding here guarantees + // the test's manually-injected entities are the latest write. + seed_component_cache(); } void TearDown() override { diff --git a/src/ros2_medkit_gateway/test/test_operation_manager.cpp b/src/ros2_medkit_gateway/test/test_operation_manager.cpp index 26f1b4be5..3d30a4bba 100644 --- a/src/ros2_medkit_gateway/test/test_operation_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_operation_manager.cpp @@ -19,6 +19,8 @@ #include "ros2_medkit_gateway/discovery/discovery_manager.hpp" #include "ros2_medkit_gateway/operation_manager.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_action_transport.hpp" +#include "ros2_medkit_gateway/ros2/transports/ros2_service_transport.hpp" using namespace ros2_medkit_gateway; @@ -33,22 +35,27 @@ class TestOperationManager : public ::testing::Test { } void SetUp() override { - // Use short timeout for tests to avoid long waits on nonexistent services - rclcpp::NodeOptions options; - options.parameter_overrides({rclcpp::Parameter("service_call_timeout_sec", static_cast(1))}); - node_ = std::make_shared("test_operation_manager_node", options); + // Use short timeout for tests to avoid long waits on nonexistent services. + node_ = std::make_shared("test_operation_manager_node"); discovery_manager_ = std::make_unique(node_.get()); - operation_manager_ = std::make_unique(node_.get(), discovery_manager_.get()); + service_transport_ = std::make_shared(node_.get()); + action_transport_ = std::make_shared(node_.get()); + operation_manager_ = std::make_unique(service_transport_, action_transport_, + discovery_manager_.get(), /*timeout=*/1); } void TearDown() override { operation_manager_.reset(); discovery_manager_.reset(); + service_transport_.reset(); + action_transport_.reset(); node_.reset(); } std::shared_ptr node_; std::unique_ptr discovery_manager_; + std::shared_ptr service_transport_; + std::shared_ptr action_transport_; std::unique_ptr operation_manager_; }; diff --git a/src/ros2_medkit_gateway/test/test_operation_manager_routing.cpp b/src/ros2_medkit_gateway/test/test_operation_manager_routing.cpp new file mode 100644 index 000000000..b199c074d --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_operation_manager_routing.cpp @@ -0,0 +1,389 @@ +// 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 + +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/core/discovery/service_action_resolver.hpp" +#include "ros2_medkit_gateway/core/managers/operation_manager.hpp" +#include "ros2_medkit_gateway/core/operations/operation_types.hpp" +#include "ros2_medkit_gateway/core/transports/action_transport.hpp" +#include "ros2_medkit_gateway/core/transports/service_transport.hpp" + +namespace ros2_medkit_gateway { +namespace { + +class MockServiceTransport : public ServiceTransport { + public: + MockServiceTransport() = default; + ~MockServiceTransport() override = default; + MockServiceTransport(const MockServiceTransport &) = delete; + MockServiceTransport & operator=(const MockServiceTransport &) = delete; + MockServiceTransport(MockServiceTransport &&) = delete; + MockServiceTransport & operator=(MockServiceTransport &&) = delete; + + ServiceCallResult call(const std::string & service_path, const std::string & service_type, const json & request, + std::chrono::duration timeout) override { + last_service_path_ = service_path; + last_service_type_ = service_type; + last_request_ = request; + last_timeout_ = timeout.count(); + call_count_ += 1; + ServiceCallResult r; + r.success = success_; + r.response = response_; + r.error_message = error_message_; + return r; + } + + std::string last_service_path_; + std::string last_service_type_; + json last_request_; + double last_timeout_ = 0.0; + int call_count_ = 0; + bool success_ = true; + json response_ = json{{"result", "ok"}}; + std::string error_message_; +}; + +class MockActionTransport : public ActionTransport { + public: + MockActionTransport() = default; + ~MockActionTransport() override = default; + MockActionTransport(const MockActionTransport &) = delete; + MockActionTransport & operator=(const MockActionTransport &) = delete; + MockActionTransport(MockActionTransport &&) = delete; + MockActionTransport & operator=(MockActionTransport &&) = delete; + + ActionSendGoalResult send_goal(const std::string & action_path, const std::string & action_type, const json & goal, + std::chrono::duration timeout) override { + last_send_path_ = action_path; + last_send_type_ = action_type; + last_send_goal_ = goal; + last_send_timeout_ = timeout.count(); + send_calls_ += 1; + ActionSendGoalResult r; + r.success = send_success_; + r.goal_accepted = send_accepted_; + r.goal_id = send_goal_id_; + r.error_message = send_error_; + return r; + } + + ActionCancelResult cancel_goal(const std::string & action_path, const std::string & goal_id, + std::chrono::duration timeout) override { + last_cancel_path_ = action_path; + last_cancel_goal_id_ = goal_id; + last_cancel_timeout_ = timeout.count(); + cancel_calls_ += 1; + ActionCancelResult r; + r.success = cancel_success_; + r.return_code = cancel_return_code_; + r.error_message = cancel_error_; + return r; + } + + ActionGetResultResult get_result(const std::string & action_path, const std::string & action_type, + const std::string & goal_id, std::chrono::duration timeout) override { + last_get_path_ = action_path; + last_get_type_ = action_type; + last_get_goal_id_ = goal_id; + last_get_timeout_ = timeout.count(); + get_calls_ += 1; + ActionGetResultResult r; + r.success = get_success_; + r.status = get_status_; + r.result = get_payload_; + r.error_message = get_error_; + return r; + } + + void subscribe_status(const std::string & action_path, StatusCallback callback) override { + subscribed_paths_.push_back(action_path); + callbacks_[action_path] = std::move(callback); + } + + void unsubscribe_status(const std::string & action_path) override { + unsubscribed_paths_.push_back(action_path); + callbacks_.erase(action_path); + } + + /// Test helper to fire a status update on a previously subscribed path. + void fire_status(const std::string & action_path, const std::string & goal_id, ActionGoalStatus status) { + auto it = callbacks_.find(action_path); + if (it != callbacks_.end() && it->second) { + it->second(action_path, goal_id, status); + } + } + + // Send-goal knobs. + bool send_success_ = true; + bool send_accepted_ = true; + std::string send_goal_id_ = "0123456789abcdef0123456789abcdef"; + std::string send_error_; + int send_calls_ = 0; + std::string last_send_path_; + std::string last_send_type_; + json last_send_goal_; + double last_send_timeout_ = 0.0; + + // Cancel knobs. + bool cancel_success_ = true; + int8_t cancel_return_code_ = 0; + std::string cancel_error_; + int cancel_calls_ = 0; + std::string last_cancel_path_; + std::string last_cancel_goal_id_; + double last_cancel_timeout_ = 0.0; + + // Get-result knobs. + bool get_success_ = true; + ActionGoalStatus get_status_ = ActionGoalStatus::SUCCEEDED; + json get_payload_ = json{{"value", 42}}; + std::string get_error_; + int get_calls_ = 0; + std::string last_get_path_; + std::string last_get_type_; + std::string last_get_goal_id_; + double last_get_timeout_ = 0.0; + + std::vector subscribed_paths_; + std::vector unsubscribed_paths_; + std::map callbacks_; +}; + +class MockResolver : public ServiceActionResolver { + public: + MockResolver() = default; + std::optional find_service(const std::string & /*entity_id*/, + const std::string & /*operation_id*/) const override { + return service_; + } + std::optional find_action(const std::string & /*entity_id*/, + const std::string & /*operation_id*/) const override { + return action_; + } + + std::optional service_; + std::optional action_; +}; + +} // namespace + +TEST(OperationManagerRoutingTest, CallServiceDelegatesToServiceTransport) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/3); + auto out = mgr.call_service("/foo/srv", "std_srvs/srv/Trigger", json{{"x", 1}}); + EXPECT_TRUE(out.success); + EXPECT_EQ(svc->last_service_path_, "/foo/srv"); + EXPECT_EQ(svc->last_service_type_, "std_srvs/srv/Trigger"); + EXPECT_EQ(svc->last_request_["x"], 1); + EXPECT_DOUBLE_EQ(svc->last_timeout_, 3.0); + EXPECT_EQ(svc->call_count_, 1); +} + +TEST(OperationManagerRoutingTest, CallComponentServiceUsesResolverWhenTypeMissing) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + MockResolver resolver; + ServiceInfo info; + info.full_path = "/cmp/op"; + info.type = "std_srvs/srv/Trigger"; + resolver.service_ = info; + OperationManager mgr(svc, act, &resolver, /*timeout=*/2); + auto out = mgr.call_component_service("/cmp", "op", std::nullopt, json{}); + EXPECT_TRUE(out.success); + EXPECT_EQ(svc->last_service_path_, "/cmp/op"); + EXPECT_EQ(svc->last_service_type_, "std_srvs/srv/Trigger"); +} + +TEST(OperationManagerRoutingTest, CallComponentServiceRejectsActionType) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/1); + auto out = mgr.call_component_service("/cmp", "op", std::optional{"example_interfaces/action/Fibonacci"}, + json{}); + EXPECT_FALSE(out.success); + EXPECT_NE(out.error_message.find("not a service type"), std::string::npos); + EXPECT_EQ(svc->call_count_, 0); +} + +TEST(OperationManagerRoutingTest, SendActionGoalRoutesAndTracksGoal) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/4); + auto out = mgr.send_action_goal("/path/act", "example_interfaces/action/Fibonacci", json{{"order", 5}}, "engine"); + EXPECT_TRUE(out.success); + EXPECT_TRUE(out.goal_accepted); + EXPECT_EQ(act->last_send_path_, "/path/act"); + EXPECT_EQ(act->last_send_type_, "example_interfaces/action/Fibonacci"); + EXPECT_EQ(act->last_send_goal_["order"], 5); + EXPECT_DOUBLE_EQ(act->last_send_timeout_, 4.0); + + // Manager tracks the goal returned by the transport. + auto tracked = mgr.get_tracked_goal(out.goal_id); + ASSERT_TRUE(tracked.has_value()); + EXPECT_EQ(tracked->action_path, "/path/act"); + EXPECT_EQ(tracked->entity_id, "engine"); + EXPECT_EQ(tracked->status, ActionGoalStatus::EXECUTING); + + // And subscribed to status updates on that path. + ASSERT_EQ(act->subscribed_paths_.size(), 1u); + EXPECT_EQ(act->subscribed_paths_[0], "/path/act"); +} + +TEST(OperationManagerRoutingTest, StatusCallbackUpdatesTrackedGoal) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/4); + auto out = mgr.send_action_goal("/p/a", "example_interfaces/action/Fibonacci", json{}, "engine"); + ASSERT_TRUE(out.success); + + // Drive a status update through the transport's wired callback. + act->fire_status("/p/a", out.goal_id, ActionGoalStatus::SUCCEEDED); + + auto tracked = mgr.get_tracked_goal(out.goal_id); + ASSERT_TRUE(tracked.has_value()); + EXPECT_EQ(tracked->status, ActionGoalStatus::SUCCEEDED); +} + +TEST(OperationManagerRoutingTest, CancelActionGoalRoutesToActionTransport) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/2); + auto sent = mgr.send_action_goal("/p/a", "example_interfaces/action/Fibonacci", json{}, ""); + ASSERT_TRUE(sent.success); + + auto out = mgr.cancel_action_goal("/p/a", sent.goal_id); + EXPECT_TRUE(out.success); + EXPECT_EQ(act->cancel_calls_, 1); + EXPECT_EQ(act->last_cancel_path_, "/p/a"); + EXPECT_EQ(act->last_cancel_goal_id_, sent.goal_id); + + auto tracked = mgr.get_tracked_goal(sent.goal_id); + ASSERT_TRUE(tracked.has_value()); + EXPECT_EQ(tracked->status, ActionGoalStatus::CANCELING); +} + +TEST(OperationManagerRoutingTest, CancelActionGoalRejectsInvalidUuid) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/2); + auto out = mgr.cancel_action_goal("/p/a", "not_a_uuid"); + EXPECT_FALSE(out.success); + EXPECT_NE(out.error_message.find("Invalid goal_id format"), std::string::npos); + EXPECT_EQ(act->cancel_calls_, 0); +} + +TEST(OperationManagerRoutingTest, GetActionResultRoutesAndUpdatesTracking) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/5); + auto sent = mgr.send_action_goal("/p/a", "example_interfaces/action/Fibonacci", json{}, ""); + ASSERT_TRUE(sent.success); + + act->get_status_ = ActionGoalStatus::SUCCEEDED; + act->get_payload_ = json{{"answer", 42}}; + auto out = mgr.get_action_result("/p/a", "example_interfaces/action/Fibonacci", sent.goal_id); + EXPECT_TRUE(out.success); + EXPECT_EQ(out.status, ActionGoalStatus::SUCCEEDED); + EXPECT_EQ(out.result["answer"], 42); + EXPECT_EQ(act->get_calls_, 1); + EXPECT_EQ(act->last_get_path_, "/p/a"); + EXPECT_EQ(act->last_get_goal_id_, sent.goal_id); + EXPECT_DOUBLE_EQ(act->last_get_timeout_, 5.0); + + auto tracked = mgr.get_tracked_goal(sent.goal_id); + ASSERT_TRUE(tracked.has_value()); + EXPECT_EQ(tracked->status, ActionGoalStatus::SUCCEEDED); +} + +TEST(OperationManagerRoutingTest, CleanupOldGoalsRemovesCompletedAndUnsubscribes) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/2); + auto sent = mgr.send_action_goal("/p/a", "example_interfaces/action/Fibonacci", json{}, ""); + ASSERT_TRUE(sent.success); + + // Mark the goal as terminal. + mgr.update_goal_status(sent.goal_id, ActionGoalStatus::SUCCEEDED); + + // The cleanup compares `now - last_update > max_age`, so the negative max_age + // forces the comparison to succeed without sleeping. + mgr.cleanup_old_goals(std::chrono::seconds(-1)); + + EXPECT_TRUE(mgr.list_tracked_goals().empty()); + ASSERT_EQ(act->unsubscribed_paths_.size(), 1u); + EXPECT_EQ(act->unsubscribed_paths_[0], "/p/a"); +} + +TEST(OperationManagerRoutingTest, ShutdownClearsTrackedGoals) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/2); + auto sent = mgr.send_action_goal("/p/a", "example_interfaces/action/Fibonacci", json{}, ""); + ASSERT_TRUE(sent.success); + ASSERT_FALSE(mgr.list_tracked_goals().empty()); + + mgr.shutdown(); + EXPECT_TRUE(mgr.list_tracked_goals().empty()); + // Shutdown asks the transport to drop the subscription. + EXPECT_FALSE(act->unsubscribed_paths_.empty()); + // Idempotency: a second shutdown is a no-op. + EXPECT_NO_THROW(mgr.shutdown()); +} + +TEST(OperationManagerRoutingTest, StaticUuidValidatorWorksWithoutRos) { + EXPECT_TRUE(OperationManager::is_valid_uuid_hex("0123456789abcdef0123456789abcdef")); + EXPECT_FALSE(OperationManager::is_valid_uuid_hex("0123")); + EXPECT_FALSE(OperationManager::is_valid_uuid_hex("0123456789abcdef0123456789abcdez")); +} + +TEST(OperationManagerRoutingTest, StaticTypeValidatorsWorkWithoutRos) { + EXPECT_TRUE(OperationManager::is_valid_message_type("std_srvs/srv/Trigger")); + EXPECT_TRUE(OperationManager::is_service_type("std_srvs/srv/Trigger")); + EXPECT_FALSE(OperationManager::is_action_type("std_srvs/srv/Trigger")); + EXPECT_TRUE(OperationManager::is_action_type("example_interfaces/action/Fibonacci")); + EXPECT_FALSE(OperationManager::is_service_type("example_interfaces/action/Fibonacci")); +} + +TEST(OperationManagerRoutingTest, GetLatestGoalForActionPicksMostRecent) { + auto svc = std::make_shared(); + auto act = std::make_shared(); + OperationManager mgr(svc, act, nullptr, /*timeout=*/2); + + act->send_goal_id_ = "00000000000000000000000000000001"; + auto first = mgr.send_action_goal("/p/a", "example_interfaces/action/Fibonacci", json{}, ""); + ASSERT_TRUE(first.success); + // Sleep briefly so created_at differs - chrono::system_clock::now() is + // monotonic enough at millisecond granularity for this assertion. + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + act->send_goal_id_ = "00000000000000000000000000000002"; + auto second = mgr.send_action_goal("/p/a", "example_interfaces/action/Fibonacci", json{}, ""); + ASSERT_TRUE(second.success); + + auto latest = mgr.get_latest_goal_for_action("/p/a"); + ASSERT_TRUE(latest.has_value()); + EXPECT_EQ(latest->goal_id, second.goal_id); +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/test/test_runtime_discovery.cpp b/src/ros2_medkit_gateway/test/test_runtime_discovery.cpp index 62b0795ce..6f758f898 100644 --- a/src/ros2_medkit_gateway/test/test_runtime_discovery.cpp +++ b/src/ros2_medkit_gateway/test/test_runtime_discovery.cpp @@ -19,9 +19,9 @@ #include #include -#include "ros2_medkit_gateway/discovery/runtime_discovery.hpp" +#include "ros2_medkit_gateway/ros2/providers/ros2_runtime_introspection.hpp" -using ros2_medkit_gateway::discovery::RuntimeDiscoveryStrategy; +using ros2_medkit_gateway::ros2::Ros2RuntimeIntrospection; // ============================================================================= // Global rclcpp lifecycle - runs once for ALL test suites in this binary @@ -40,7 +40,7 @@ class RclcppEnvironment : public ::testing::Environment { ::testing::Environment * const rclcpp_env = ::testing::AddGlobalTestEnvironment(new RclcppEnvironment); // ============================================================================= -// RuntimeDiscoveryStrategy - Function from namespace tests +// Ros2RuntimeIntrospection - Function from namespace tests // ============================================================================= class RuntimeDiscoveryTest : public ::testing::Test { @@ -49,7 +49,7 @@ class RuntimeDiscoveryTest : public ::testing::Test { // Create node in a namespace to test namespace grouping rclcpp::NodeOptions options; node_ = std::make_shared("test_node", "/test_ns", options); - strategy_ = std::make_unique(node_.get()); + strategy_ = std::make_unique(node_.get()); } void TearDown() override { @@ -58,26 +58,19 @@ class RuntimeDiscoveryTest : public ::testing::Test { } std::shared_ptr node_; - std::unique_ptr strategy_; + std::unique_ptr strategy_; }; // ----------------------------------------------------------------------------- -// discover_areas() - always returns empty (Areas come from manifest only) +// introspect() - never produces Areas or Components (they come from the +// manifest layer or HostInfoProvider). // ----------------------------------------------------------------------------- -TEST_F(RuntimeDiscoveryTest, DiscoverAreas_AlwaysReturnsEmpty) { - auto areas = strategy_->discover_areas(); - EXPECT_TRUE(areas.empty()) << "Areas should always be empty - Areas come from manifest only"; -} - -// ----------------------------------------------------------------------------- -// discover_components() - always returns empty (Components come from -// HostInfoProvider or manifest) -// ----------------------------------------------------------------------------- - -TEST_F(RuntimeDiscoveryTest, DiscoverComponents_AlwaysReturnsEmpty) { - auto components = strategy_->discover_components(); - EXPECT_TRUE(components.empty()) +TEST_F(RuntimeDiscoveryTest, Introspect_NeverProducesAreasOrComponents) { + ros2_medkit_gateway::IntrospectionInput input; + auto result = strategy_->introspect(input); + EXPECT_TRUE(result.new_entities.areas.empty()) << "Areas should always be empty - Areas come from manifest only"; + EXPECT_TRUE(result.new_entities.components.empty()) << "Components should always be empty - Components come from HostInfoProvider or manifest"; } @@ -112,7 +105,7 @@ TEST_F(RuntimeDiscoveryTest, DiscoverFunctions_DefaultCreatesFromNamespaces) { } TEST_F(RuntimeDiscoveryTest, DiscoverFunctions_ReturnsEmptyWhenDisabled) { - RuntimeDiscoveryStrategy::RuntimeConfig config; + Ros2RuntimeIntrospection::RuntimeConfig config; config.create_functions_from_namespaces = false; strategy_->set_config(config); @@ -152,7 +145,7 @@ TEST_F(RuntimeDiscoveryTest, DiscoverFunctions_OnlyCreatesNonEmptyFunctions) { // ----------------------------------------------------------------------------- TEST_F(RuntimeDiscoveryTest, DefaultConfig_HasCorrectDefaults) { - RuntimeDiscoveryStrategy::RuntimeConfig config; + Ros2RuntimeIntrospection::RuntimeConfig config; EXPECT_TRUE(config.create_functions_from_namespaces) << "create_functions_from_namespaces should default to true"; } @@ -168,7 +161,7 @@ class RuntimeDiscoveryMultiNsTest : public ::testing::Test { node_nav_ = std::make_shared("planner", "/navigation"); // Create discovery node (the one that queries the graph) node_discovery_ = std::make_shared("discovery_node"); - strategy_ = std::make_unique(node_discovery_.get()); + strategy_ = std::make_unique(node_discovery_.get()); } void TearDown() override { @@ -181,7 +174,7 @@ class RuntimeDiscoveryMultiNsTest : public ::testing::Test { std::shared_ptr node_sensors_; std::shared_ptr node_nav_; std::shared_ptr node_discovery_; - std::unique_ptr strategy_; + std::unique_ptr strategy_; }; // @verifies REQ_INTEROP_003 @@ -220,13 +213,10 @@ TEST_F(RuntimeDiscoveryMultiNsTest, DiscoverFunctions_GroupsByNamespace) { EXPECT_TRUE(found_root) << "Should create 'root' function from root namespace nodes"; } -TEST_F(RuntimeDiscoveryMultiNsTest, DiscoverAreas_AlwaysEmpty) { - auto areas = strategy_->discover_areas(); - EXPECT_TRUE(areas.empty()) << "Areas should always be empty - Areas come from manifest only"; -} - -TEST_F(RuntimeDiscoveryMultiNsTest, DiscoverComponents_AlwaysEmpty) { - auto components = strategy_->discover_components(); - EXPECT_TRUE(components.empty()) +TEST_F(RuntimeDiscoveryMultiNsTest, Introspect_AreasAndComponentsEmpty) { + ros2_medkit_gateway::IntrospectionInput input; + auto result = strategy_->introspect(input); + EXPECT_TRUE(result.new_entities.areas.empty()) << "Areas should always be empty - Areas come from manifest only"; + EXPECT_TRUE(result.new_entities.components.empty()) << "Components should always be empty - Components come from HostInfoProvider or manifest"; } diff --git a/src/ros2_medkit_gateway/test/test_trigger_manager_routing.cpp b/src/ros2_medkit_gateway/test/test_trigger_manager_routing.cpp new file mode 100644 index 000000000..13af23cff --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_trigger_manager_routing.cpp @@ -0,0 +1,323 @@ +// 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. + +// TriggerManager routing test: links exclusively against gateway_core + +// GTest. No rclcpp/ament dependencies on the link line, proving that the +// manager body lives in the middleware-neutral build layer. The +// rclcpp::GenericSubscription / type lookup / pending retry concerns belong +// to the Ros2TopicSubscriptionTransport adapter; here we exercise the +// manager against a mock transport that records subscribe/unsubscribe +// calls and lets the test emit JSON samples directly. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/core/condition_evaluator.hpp" +#include "ros2_medkit_gateway/core/managers/trigger_manager.hpp" +#include "ros2_medkit_gateway/core/resource_change_notifier.hpp" +#include "ros2_medkit_gateway/core/sqlite_trigger_store.hpp" +#include "ros2_medkit_gateway/core/transports/topic_subscription_transport.hpp" + +namespace ros2_medkit_gateway { +namespace { + +// ---------------------------------------------------------------------------- +// Mock transport +// ---------------------------------------------------------------------------- + +/// Mock TopicSubscriptionTransport recording every subscribe/unsubscribe so +/// the test can assert what the manager routes through it. Each subscribe() +/// returns a handle whose dtor flips a per-key "alive" flag, mirroring the +/// real adapter's contract. +class MockTopicSubscriptionTransport : public TopicSubscriptionTransport { + public: + struct Subscription { + std::string topic_path; + std::string msg_type; + SampleCallback callback; + std::shared_ptr> alive = std::make_shared>(true); + }; + + class Handle : public TopicSubscriptionHandle { + public: + Handle(MockTopicSubscriptionTransport * owner, std::string key) : owner_(owner), key_(std::move(key)) { + } + + ~Handle() override { + owner_->release(key_); + } + + Handle(const Handle &) = delete; + Handle & operator=(const Handle &) = delete; + Handle(Handle &&) = delete; + Handle & operator=(Handle &&) = delete; + + const std::string & key() const { + return key_; + } + + private: + MockTopicSubscriptionTransport * owner_; + std::string key_; + }; + + std::unique_ptr subscribe(const std::string & topic_path, const std::string & msg_type, + SampleCallback callback) override { + std::lock_guard lock(mutex_); + auto key = "mock_" + std::to_string(next_id_++); + Subscription entry; + entry.topic_path = topic_path; + entry.msg_type = msg_type; + entry.callback = std::move(callback); + subs_[key] = std::move(entry); + subscribe_log_.emplace_back(key, topic_path); + return std::make_unique(this, key); + } + + /// Test helper: emit a sample as if rclcpp had just delivered one. + /// Returns true when the key is still alive (callback was invoked). + bool emit(const std::string & key, const nlohmann::json & sample) { + SampleCallback cb; + { + std::lock_guard lock(mutex_); + auto it = subs_.find(key); + if (it == subs_.end() || !it->second.alive->load()) { + return false; + } + cb = it->second.callback; + } + if (!cb) { + return false; + } + cb(sample); + return true; + } + + /// Test helper: returns the topic path most recently registered. + std::string last_subscribed_topic() const { + std::lock_guard lock(mutex_); + return subscribe_log_.empty() ? std::string() : subscribe_log_.back().second; + } + + /// Test helper: count of currently-alive subscriptions. + size_t alive_count() const { + std::lock_guard lock(mutex_); + size_t n = 0; + for (const auto & [_, sub] : subs_) { + if (sub.alive->load()) { + ++n; + } + } + return n; + } + + /// Test helper: total number of subscribe() calls observed. + size_t total_subscribes() const { + std::lock_guard lock(mutex_); + return subscribe_log_.size(); + } + + /// Test helper: list of keys still alive. + std::vector alive_keys() const { + std::lock_guard lock(mutex_); + std::vector out; + for (const auto & [key, sub] : subs_) { + if (sub.alive->load()) { + out.push_back(key); + } + } + return out; + } + + private: + void release(const std::string & key) { + std::lock_guard lock(mutex_); + auto it = subs_.find(key); + if (it == subs_.end()) { + return; + } + it->second.alive->store(false); + it->second.callback = nullptr; + } + + mutable std::mutex mutex_; + std::unordered_map subs_; + std::vector> subscribe_log_; // (key, topic) + uint64_t next_id_ = 1; +}; + +// ---------------------------------------------------------------------------- +// Fixture +// ---------------------------------------------------------------------------- + +class TriggerManagerRoutingTest : public ::testing::Test { + protected: + void SetUp() override { + registry_.register_condition("OnChange", std::make_shared()); + + transport_ = std::make_shared(); + + TriggerConfig config; + config.max_triggers = 50; + config.on_restart_behavior = "reset"; + manager_ = std::make_unique(notifier_, registry_, store_, config, transport_); + } + + void TearDown() override { + if (manager_) { + manager_->shutdown(); + } + notifier_.shutdown(); + } + + TriggerCreateRequest make_data_request(const std::string & entity_id = "sensor", + const std::string & resolved_topic = "/sensor/temperature", + const std::string & resource_path = "/temperature") { + TriggerCreateRequest req; + req.entity_id = entity_id; + req.entity_type = "apps"; + req.resource_uri = "/api/v1/apps/" + entity_id + "/data" + resource_path; + req.collection = "data"; + req.resource_path = resource_path; + req.resolved_topic_name = resolved_topic; + req.path = ""; + req.condition_type = "OnChange"; + req.condition_params = nlohmann::json::object(); + req.protocol = "sse"; + req.multishot = true; + req.persistent = false; + return req; + } + + ResourceChangeNotifier notifier_; + ConditionRegistry registry_; + SqliteTriggerStore store_{":memory:"}; + std::shared_ptr transport_; + std::unique_ptr manager_; +}; + +// ---------------------------------------------------------------------------- +// Routing tests (~6 cases) +// ---------------------------------------------------------------------------- + +TEST_F(TriggerManagerRoutingTest, RegisteredTriggerSubscribesToTransport) { + auto created = manager_->create(make_data_request("sensor", "/sensor/temperature")); + ASSERT_TRUE(created.has_value()) << created.error().message; + + EXPECT_EQ(transport_->total_subscribes(), 1u); + EXPECT_EQ(transport_->alive_count(), 1u); + EXPECT_EQ(transport_->last_subscribed_topic(), "/sensor/temperature"); +} + +TEST_F(TriggerManagerRoutingTest, RemovedTriggerDestructsHandle) { + auto created = manager_->create(make_data_request()); + ASSERT_TRUE(created.has_value()); + ASSERT_EQ(transport_->alive_count(), 1u); + + ASSERT_TRUE(manager_->remove(created->id)); + EXPECT_EQ(transport_->alive_count(), 0u) + << "Removing a data trigger must drop its TopicSubscriptionHandle, which must unsubscribe"; +} + +TEST_F(TriggerManagerRoutingTest, SampleCallbackRoutesToConditionEvaluator) { + auto created = manager_->create(make_data_request("sensor", "/sensor/temperature", "/temperature")); + ASSERT_TRUE(created.has_value()); + + auto keys = transport_->alive_keys(); + ASSERT_EQ(keys.size(), 1u); + + // Route a sample through the mock transport - this must reach the + // resource-change notifier and fire the OnChange evaluator. + ASSERT_TRUE(transport_->emit(keys[0], nlohmann::json(42.0))); + ASSERT_TRUE(manager_->wait_for_event(created->id, std::chrono::milliseconds(2000))); + + auto event = manager_->consume_pending_event(created->id); + ASSERT_TRUE(event.has_value()); + EXPECT_TRUE(event->contains("payload")); + EXPECT_EQ((*event)["payload"], nlohmann::json(42.0)); +} + +TEST_F(TriggerManagerRoutingTest, OrphanedTriggerSweepUnsubscribes) { + auto alive = manager_->create(make_data_request("alive_app", "/alive_app/temperature")); + auto gone = manager_->create(make_data_request("gone_app", "/gone_app/temperature")); + ASSERT_TRUE(alive.has_value()); + ASSERT_TRUE(gone.has_value()); + ASSERT_EQ(transport_->alive_count(), 2u); + + manager_->set_entity_exists_fn([](const std::string & id, const std::string & /*type*/) { + return id != "gone_app"; + }); + manager_->sweep_orphaned_triggers(); + + EXPECT_EQ(transport_->alive_count(), 1u) << "Orphan sweep must unsubscribe gone_app's transport handle"; + EXPECT_TRUE(manager_->list("gone_app").empty()); + EXPECT_EQ(manager_->list("alive_app").size(), 1u); +} + +TEST_F(TriggerManagerRoutingTest, MultipleTriggersIndependentSubscriptions) { + auto t1 = manager_->create(make_data_request("sensor_a", "/sensor_a/temperature", "/temperature")); + auto t2 = manager_->create(make_data_request("sensor_b", "/sensor_b/temperature", "/temperature")); + ASSERT_TRUE(t1.has_value()); + ASSERT_TRUE(t2.has_value()); + + // Two distinct subscribe calls, two alive handles. + EXPECT_EQ(transport_->total_subscribes(), 2u); + EXPECT_EQ(transport_->alive_count(), 2u); + + // Removing only one drops only that handle; the other survives. + ASSERT_TRUE(manager_->remove(t1->id)); + EXPECT_EQ(transport_->alive_count(), 1u); +} + +TEST_F(TriggerManagerRoutingTest, ShutdownClearsAllSubscriptions) { + ASSERT_TRUE(manager_->create(make_data_request("a", "/a/x", "/x")).has_value()); + ASSERT_TRUE(manager_->create(make_data_request("b", "/b/x", "/x")).has_value()); + ASSERT_TRUE(manager_->create(make_data_request("c", "/c/x", "/x")).has_value()); + ASSERT_EQ(transport_->alive_count(), 3u); + + manager_->shutdown(); + EXPECT_EQ(transport_->alive_count(), 0u) << "Manager shutdown must drop every transport handle"; +} + +TEST_F(TriggerManagerRoutingTest, NonDataTriggerDoesNotSubscribe) { + // Faults / configurations / etc. don't go through the topic transport. + TriggerCreateRequest req; + req.entity_id = "sensor"; + req.entity_type = "apps"; + req.resource_uri = "/api/v1/apps/sensor/faults/fault_001"; + req.collection = "faults"; + req.resource_path = "/fault_001"; + req.resolved_topic_name = ""; + req.path = ""; + req.condition_type = "OnChange"; + req.condition_params = nlohmann::json::object(); + req.protocol = "sse"; + req.multishot = true; + req.persistent = false; + + ASSERT_TRUE(manager_->create(req).has_value()); + EXPECT_EQ(transport_->total_subscribes(), 0u) << "Non-data triggers must not register any topic subscription"; +} + +} // namespace +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_integration_tests/test/features/test_graph_event_discovery.test.py b/src/ros2_medkit_integration_tests/test/features/test_graph_event_discovery.test.py new file mode 100644 index 000000000..26a60850d --- /dev/null +++ b/src/ros2_medkit_integration_tests/test/features/test_graph_event_discovery.test.py @@ -0,0 +1,257 @@ +#!/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 graph-event-driven discovery refresh. + +The gateway runs ``refresh_cache()`` whenever rclcpp signals a graph +change (``get_graph_event()->check_and_clear()``), polled every 100 ms. +A long backstop timer (``refresh_interval_ms``, default 30 s) provides +liveness in case a graph event is ever missed. + +Scope of these tests: + +1. Two demo nodes are launched at startup; both must appear in ``/apps``. +2. A new demo node is spawned mid-test; the gateway must expose it in + ``/apps`` quickly - the load-bearing assertion that proves the + graph-event refactor is working. + +The gateway runs with a long ``refresh_interval_ms`` (30 s) so any +detection well under that window must come from the graph-event poll +rather than the safety backstop. + +Kill-detection latency is intentionally not asserted: rclcpp's +graph-event for a departing participant fires only after the executor +sees the DDS leave announcement, which depends on RMW liveliness timers +and graceful-shutdown timing. That cleanup is exercised by the existing +discovery integration tests; here we focus on the path the refactor +actually changed (the refresh trigger). +""" + +import os +import signal +import subprocess +import time +import unittest + +from ament_index_python.packages import get_package_prefix +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, + DEFAULT_DOMAIN_ID, +) +from ros2_medkit_test_utils.gateway_test_case import GatewayTestCase +from ros2_medkit_test_utils.launch_helpers import ( + create_demo_nodes, + create_gateway_node, + DEMO_NODE_REGISTRY, +) + + +# Long backstop so any sub-backstop detection must come from graph events. +BACKSTOP_INTERVAL_MS = 30000 + +# Demo nodes launched at startup. +INITIAL_NODES = ['temp_sensor', 'rpm_sensor'] + +# Demo node spawned mid-test to exercise the new-node path. +LATE_NODE_KEY = 'pressure_sensor' + +# Detection budgets. +# +# Spawn detection is bounded by: +# process exec + rclcpp init + DDS announce + 100 ms poll + refresh_cache. +# 5 s is comfortable; well under the 30 s backstop, so a pass proves the +# graph-event poll fired the refresh. +SPAWN_DETECTION_TIMEOUT = 5.0 + +# Initial discovery shares the budget with full gateway startup. +INITIAL_DETECTION_TIMEOUT = 30.0 + + +def generate_test_description(): + gateway_node = create_gateway_node( + extra_params={ + 'refresh_interval_ms': BACKSTOP_INTERVAL_MS, + }, + ) + + initial_demos = create_demo_nodes( + nodes=INITIAL_NODES, + lidar_faulty=False, + ) + + delayed = TimerAction( + period=2.0, + actions=initial_demos, + ) + + return ( + LaunchDescription([ + gateway_node, + delayed, + launch_testing.actions.ReadyToTest(), + ]), + {'gateway_node': gateway_node}, + ) + + +def _resolve_demo_executable(name): + """Resolve a demo executable to its installed absolute path. + + Avoids the cost of going through ``ros2 run``, which spins up a + Python interpreter and ament_index lookup before exec'ing the + binary - that overhead would dominate the spawn-detection budget. + """ + pkg = 'ros2_medkit_integration_tests' + prefix = get_package_prefix(pkg) + candidate = os.path.join(prefix, 'lib', pkg, name) + if not os.path.isfile(candidate): + raise FileNotFoundError(f'demo executable not found: {candidate}') + return candidate + + +def _terminate_process(proc): + if proc is None or proc.poll() is not None: + return + proc.send_signal(signal.SIGTERM) + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + proc.kill() + proc.wait(timeout=5) + + +# @verifies REQ_INTEROP_003 +class TestGraphEventDiscovery(GatewayTestCase): + """Verify graph-event-driven discovery refresh latency.""" + + @classmethod + def setUpClass(cls): + super().setUpClass() + cls._extra_proc = None + + @classmethod + def tearDownClass(cls): + # Make sure we never leak a stray demo process if a test failed + # before its own cleanup ran. + _terminate_process(cls._extra_proc) + cls._extra_proc = None + super().tearDownClass() + + @staticmethod + def _spawn_demo_process(key): + executable, ros_name, namespace = DEMO_NODE_REGISTRY[key] + + env = os.environ.copy() + env['ROS_DOMAIN_ID'] = str(DEFAULT_DOMAIN_ID) + + binary = _resolve_demo_executable(executable) + + return subprocess.Popen( + [ + binary, + '--ros-args', + '-r', f'__ns:={namespace}', + '-r', f'__node:={ros_name}', + ], + env=env, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + ) + + def test_initial_discovery_picks_up_startup_nodes(self): + """Both startup demo nodes must be visible in /apps.""" + for key in INITIAL_NODES: + _, ros_name, _ = DEMO_NODE_REGISTRY[key] + data = self.poll_endpoint_until( + '/apps', + lambda d, n=ros_name: d if any( + n in app.get('id', '') + for app in d.get('items', []) + ) else None, + timeout=INITIAL_DETECTION_TIMEOUT, + interval=0.1, + ) + app_ids = [app.get('id', '') for app in data.get('items', [])] + self.assertTrue( + any(ros_name in app_id for app_id in app_ids), + f'Expected {ros_name} in /apps, got {app_ids}', + ) + + def test_new_node_detected_via_graph_event(self): + """Spawning a node mid-run must propagate within the spawn budget. + + ``BACKSTOP_INTERVAL_MS`` is 30 s; detection within + ``SPAWN_DETECTION_TIMEOUT`` (5 s) therefore proves the refresh + was triggered by a graph event, not the safety-backstop sweep. + """ + # Make sure the initial graph is fully settled before spawning. + for key in INITIAL_NODES: + _, ros_name, _ = DEMO_NODE_REGISTRY[key] + self.poll_endpoint_until( + '/apps', + lambda d, n=ros_name: d if any( + n in app.get('id', '') + for app in d.get('items', []) + ) else None, + timeout=INITIAL_DETECTION_TIMEOUT, + interval=0.1, + ) + + _, late_ros_name, _ = DEMO_NODE_REGISTRY[LATE_NODE_KEY] + + type(self)._extra_proc = self._spawn_demo_process(LATE_NODE_KEY) + spawn_time = time.monotonic() + + try: + data = self.poll_endpoint_until( + '/apps', + lambda d: d if any( + late_ros_name in app.get('id', '') + for app in d.get('items', []) + ) else None, + timeout=SPAWN_DETECTION_TIMEOUT, + interval=0.1, + ) + elapsed = time.monotonic() - spawn_time + backstop_sec = BACKSTOP_INTERVAL_MS / 1000.0 + self.assertLess( + elapsed, backstop_sec, + f'Spawn detection took {elapsed:.3f}s, which is at or above ' + f'the {backstop_sec:.1f}s backstop - graph-event path is ' + f'NOT driving the refresh.', + ) + app_ids = [app.get('id', '') for app in data.get('items', [])] + self.assertTrue( + any(late_ros_name in app_id for app_id in app_ids), + f'Expected {late_ros_name} in /apps, got {app_ids}', + ) + finally: + _terminate_process(type(self)._extra_proc) + type(self)._extra_proc = None + + +@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_plugins/ros2_medkit_opcua/CMakeLists.txt b/src/ros2_medkit_plugins/ros2_medkit_opcua/CMakeLists.txt index 92b4e2bf7..e1dfa5a6e 100644 --- a/src/ros2_medkit_plugins/ros2_medkit_opcua/CMakeLists.txt +++ b/src/ros2_medkit_plugins/ros2_medkit_opcua/CMakeLists.txt @@ -145,12 +145,17 @@ if(BUILD_TESTING) include(ROS2MedkitTestDomain) medkit_init_test_domains(START 220 END 229) + # Each test connects to a non-existent OPC UA host and waits ~3.8s for the + # DNS / TCP failure path; with 13 tests in the suite the run requires ~90s. + # The default ament_add_gtest timeout (60s) is too tight, causing the runner + # to abort mid-suite and emit "missing_result" under heavy parallelism. ament_add_gtest(test_opcua_plugin test/test_opcua_plugin.cpp src/opcua_plugin.cpp src/opcua_client.cpp src/node_map.cpp src/opcua_poller.cpp + TIMEOUT 240 ) target_include_directories(test_opcua_plugin PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include diff --git a/src/ros2_medkit_serialization/CMakeLists.txt b/src/ros2_medkit_serialization/CMakeLists.txt index 3c2bd8b5d..e8b0b8bcb 100644 --- a/src/ros2_medkit_serialization/CMakeLists.txt +++ b/src/ros2_medkit_serialization/CMakeLists.txt @@ -38,6 +38,7 @@ find_package(nlohmann_json REQUIRED) add_library(${PROJECT_NAME} src/json_serializer.cpp src/type_cache.cpp + src/type_introspection.cpp src/service_action_types.cpp src/message_cleanup.cpp # Vendored dynmsg sources (C++ API only) @@ -145,13 +146,16 @@ if(BUILD_TESTING) "include/ros2_medkit_serialization/serialization_error.hpp" "include/ros2_medkit_serialization/service_action_types.hpp" "include/ros2_medkit_serialization/type_cache.hpp" + "include/ros2_medkit_serialization/type_introspection.hpp" "src/json_serializer.cpp" "src/message_cleanup.cpp" "src/service_action_types.cpp" "src/type_cache.cpp" + "src/type_introspection.cpp" "test/test_json_serializer.cpp" "test/test_service_action_types.cpp" "test/test_type_cache.cpp" + "test/test_type_introspection.cpp" ) ros2_medkit_clang_tidy( @@ -172,6 +176,10 @@ if(BUILD_TESTING) target_link_libraries(test_service_action_types ${PROJECT_NAME}) medkit_target_dependencies(test_service_action_types std_srvs) + ament_add_gtest(test_type_introspection test/test_type_introspection.cpp) + target_link_libraries(test_type_introspection ${PROJECT_NAME}) + medkit_target_dependencies(test_type_introspection std_msgs) + # Apply coverage flags to test targets if(ENABLE_COVERAGE) target_compile_options(test_type_cache PRIVATE --coverage -O0 -g) @@ -180,6 +188,8 @@ if(BUILD_TESTING) target_link_options(test_json_serializer PRIVATE --coverage) target_compile_options(test_service_action_types PRIVATE --coverage -O0 -g) target_link_options(test_service_action_types PRIVATE --coverage) + target_compile_options(test_type_introspection PRIVATE --coverage -O0 -g) + target_link_options(test_type_introspection PRIVATE --coverage) endif() ros2_medkit_relax_vendor_warnings() endif() diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/type_introspection.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/type_introspection.hpp similarity index 96% rename from src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/type_introspection.hpp rename to src/ros2_medkit_serialization/include/ros2_medkit_serialization/type_introspection.hpp index 0ae51c544..1c84feca2 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/type_introspection.hpp +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/type_introspection.hpp @@ -22,7 +22,7 @@ #include "ros2_medkit_serialization/json_serializer.hpp" -namespace ros2_medkit_gateway { +namespace ros2_medkit_serialization { /** * @brief Information about a ROS 2 message type including schema and template @@ -94,10 +94,10 @@ class TypeIntrospection { private: /// Native JSON serializer for type introspection - std::shared_ptr serializer_; + std::shared_ptr serializer_; std::unordered_map type_cache_; ///< Cache for type info mutable std::mutex cache_mutex_; ///< Mutex for thread-safe cache access }; -} // namespace ros2_medkit_gateway +} // namespace ros2_medkit_serialization diff --git a/src/ros2_medkit_gateway/src/type_introspection.cpp b/src/ros2_medkit_serialization/src/type_introspection.cpp similarity index 88% rename from src/ros2_medkit_gateway/src/type_introspection.cpp rename to src/ros2_medkit_serialization/src/type_introspection.cpp index ec279599f..5b85ded8e 100644 --- a/src/ros2_medkit_gateway/src/type_introspection.cpp +++ b/src/ros2_medkit_serialization/src/type_introspection.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ros2_medkit_gateway/core/type_introspection.hpp" +#include "ros2_medkit_serialization/type_introspection.hpp" #include #include @@ -20,17 +20,17 @@ #include "ros2_medkit_serialization/json_serializer.hpp" #include "ros2_medkit_serialization/serialization_error.hpp" -namespace ros2_medkit_gateway { +namespace ros2_medkit_serialization { TypeIntrospection::TypeIntrospection(const std::string & /* scripts_path */) - : serializer_(std::make_shared()) { + : serializer_(std::make_shared()) { // scripts_path is deprecated and ignored - we use native serialization now } nlohmann::json TypeIntrospection::get_type_template(const std::string & type_name) { try { return serializer_->get_defaults(type_name); - } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + } catch (const TypeNotFoundError & e) { throw std::runtime_error("Unknown type: " + type_name + " (" + e.what() + ")"); } catch (const std::exception & e) { throw std::runtime_error("Failed to get type template: " + std::string(e.what())); @@ -40,7 +40,7 @@ nlohmann::json TypeIntrospection::get_type_template(const std::string & type_nam nlohmann::json TypeIntrospection::get_type_schema(const std::string & type_name) { try { return serializer_->get_schema(type_name); - } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + } catch (const TypeNotFoundError & e) { throw std::runtime_error("Unknown type: " + type_name + " (" + e.what() + ")"); } catch (const std::exception & e) { throw std::runtime_error("Failed to get type schema: " + std::string(e.what())); @@ -85,4 +85,4 @@ TopicTypeInfo TypeIntrospection::get_type_info(const std::string & type_name) { } } -} // namespace ros2_medkit_gateway +} // namespace ros2_medkit_serialization diff --git a/src/ros2_medkit_serialization/test/test_type_introspection.cpp b/src/ros2_medkit_serialization/test/test_type_introspection.cpp new file mode 100644 index 000000000..1913915cc --- /dev/null +++ b/src/ros2_medkit_serialization/test/test_type_introspection.cpp @@ -0,0 +1,88 @@ +// 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 + +#include + +#include "ros2_medkit_serialization/type_introspection.hpp" + +namespace ros2_medkit_serialization { + +class TypeIntrospectionTest : public ::testing::Test { + protected: + void SetUp() override { + introspection_ = std::make_unique(""); + } + + std::unique_ptr introspection_; +}; + +TEST_F(TypeIntrospectionTest, get_type_template_for_valid_type) { + auto template_json = introspection_->get_type_template("std_msgs/msg/String"); + + EXPECT_TRUE(template_json.is_object()); + EXPECT_TRUE(template_json.contains("data")); +} + +TEST_F(TypeIntrospectionTest, get_type_template_for_float32) { + auto template_json = introspection_->get_type_template("std_msgs/msg/Float32"); + + EXPECT_TRUE(template_json.is_object()); + EXPECT_TRUE(template_json.contains("data")); +} + +TEST_F(TypeIntrospectionTest, get_type_template_for_unknown_type_throws) { + EXPECT_THROW(introspection_->get_type_template("nonexistent_pkg/msg/NonExistent"), std::runtime_error); +} + +TEST_F(TypeIntrospectionTest, get_type_schema_for_valid_type) { + auto schema = introspection_->get_type_schema("std_msgs/msg/String"); + + EXPECT_TRUE(schema.is_object()); +} + +TEST_F(TypeIntrospectionTest, get_type_schema_for_unknown_type_throws) { + EXPECT_THROW(introspection_->get_type_schema("nonexistent_pkg/msg/NonExistent"), std::runtime_error); +} + +TEST_F(TypeIntrospectionTest, get_type_info_returns_complete_info) { + auto info = introspection_->get_type_info("std_msgs/msg/String"); + + EXPECT_EQ(info.name, "std_msgs/msg/String"); + EXPECT_TRUE(info.default_value.is_object()); + EXPECT_TRUE(info.schema.is_object()); +} + +TEST_F(TypeIntrospectionTest, get_type_info_caches_results) { + // First call + auto info1 = introspection_->get_type_info("std_msgs/msg/Float32"); + // Second call should return cached result + auto info2 = introspection_->get_type_info("std_msgs/msg/Float32"); + + EXPECT_EQ(info1.name, info2.name); + EXPECT_EQ(info1.default_value, info2.default_value); + EXPECT_EQ(info1.schema, info2.schema); +} + +TEST_F(TypeIntrospectionTest, get_type_info_for_unknown_type_returns_empty) { + // For unknown types, get_type_info should return empty objects instead of throwing + auto info = introspection_->get_type_info("nonexistent_pkg/msg/NonExistent"); + + EXPECT_EQ(info.name, "nonexistent_pkg/msg/NonExistent"); + EXPECT_TRUE(info.default_value.is_object()); + EXPECT_TRUE(info.schema.is_object()); +} + +} // namespace ros2_medkit_serialization