From 779c6526b8076934674bedaae59d9bddfbf37636 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 08:29:24 +0200 Subject: [PATCH 01/18] refactor(gateway): relocate operation result types into core/operations Extracts ServiceCallResult, the ActionGoalStatus enum + free action_status_to_string helper, the four Action*Result structs, and ActionGoalInfo into a dedicated header under core/operations/. The types were already neutral C++ but lived behind the rclcpp-pulling operation_manager.hpp include path, which prevented the neutral build layer from consuming them. The OperationManager header forwards to the new location; the helper definition moves alongside into a small core translation unit so the symbol is provided by gateway_core. --- .../core/operations/operation_types.hpp | 83 +++++++++++++++++++ .../ros2_medkit_gateway/operation_manager.hpp | 57 +------------ .../src/core/operations/operation_types.cpp | 39 +++++++++ .../src/operation_manager.cpp | 21 ----- 4 files changed, 123 insertions(+), 77 deletions(-) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/operations/operation_types.hpp create mode 100644 src/ros2_medkit_gateway/src/core/operations/operation_types.cpp 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 00000000..e06a3240 --- /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/operation_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/operation_manager.hpp index 7e366368..29179112 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 @@ -30,6 +30,7 @@ #include "ros2_medkit_gateway/compat/generic_client_compat.hpp" #include "ros2_medkit_gateway/core/discovery/models/common.hpp" +#include "ros2_medkit_gateway/core/operations/operation_types.hpp" #include "ros2_medkit_gateway/discovery/discovery_manager.hpp" #include "ros2_medkit_serialization/json_serializer.hpp" #include "ros2_medkit_serialization/service_action_types.hpp" @@ -40,62 +41,6 @@ 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 { 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 00000000..d1022b78 --- /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/operation_manager.cpp b/src/ros2_medkit_gateway/src/operation_manager.cpp index 40e83a96..f7391da6 100644 --- a/src/ros2_medkit_gateway/src/operation_manager.cpp +++ b/src/ros2_medkit_gateway/src/operation_manager.cpp @@ -311,27 +311,6 @@ ServiceCallResult OperationManager::call_component_service(const std::string & c // ==================== 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; From 9e7ecca521d47d36842748495ba0fb0905991687 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 08:34:03 +0200 Subject: [PATCH 02/18] refactor(gateway): relocate parameter result types into core/configuration Extracts ParameterErrorCode and ParameterResult into a dedicated header under core/configuration/. The structured-error contract becomes available to the neutral build layer; the ConfigurationManager header forwards to the new location. --- .../configuration_manager.hpp | 24 +--------- .../core/configuration/parameter_types.hpp | 46 +++++++++++++++++++ 2 files changed, 48 insertions(+), 22 deletions(-) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/configuration/parameter_types.hpp 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 b83d4b96..82a85e6c 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 @@ -27,32 +27,12 @@ #include #include +#include "ros2_medkit_gateway/core/configuration/parameter_types.hpp" + 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 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 00000000..82c66432 --- /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 From 532b947a6e0db5cfb411633d6b24468fb0404570 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 08:41:00 +0200 Subject: [PATCH 03/18] refactor(gateway): relocate FaultResult into core/faults Extracts FaultResult, the JSON-bag outcome shared by seven of the eight fault-manager methods, into a dedicated header under core/faults/. The existing FaultWithEnvResult still exposes raw message types and stays inside fault_manager.hpp until the transport extraction moves the message-to-JSON conversion to the adapter layer. --- .../core/faults/fault_types.hpp | 35 +++++++++++++++++++ .../ros2_medkit_gateway/fault_manager.hpp | 9 ++--- 2 files changed, 37 insertions(+), 7 deletions(-) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/faults/fault_types.hpp 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 00000000..dd352993 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/faults/fault_types.hpp @@ -0,0 +1,35 @@ +// 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. The richer `FaultWithEnvResult`, which still exposes raw message +/// types, lives alongside the FaultManager facade until the transport +/// extraction lands. +struct FaultResult { + bool success; + json data; + std::string error_message; +}; + +} // 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 fee24eee..f76ec57f 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 @@ -31,17 +31,12 @@ #include "ros2_medkit_msgs/srv/list_rosbags.hpp" #include "ros2_medkit_msgs/srv/report_fault.hpp" +#include "ros2_medkit_gateway/core/faults/fault_types.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; From f3aa806a9ed8be3c6b128b096ae888053aada1b3 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 08:52:04 +0200 Subject: [PATCH 04/18] feat(gateway): introduce neutral Transport interfaces under core/transports Defines seven abstract ports - TopicTransport, ServiceTransport, ActionTransport, ParameterTransport, FaultServiceTransport, LogSource, TopicSubscriptionTransport - that the manager refactors will route through. Each port carries the same operations the corresponding manager performs today, expressed in pure C++ types (already-neutral result structs from core/operations, core/configuration, core/faults). The smoke test extends to assert each interface is abstract and reachable from the gateway_core build layer with no ament dependency on the link line. Adapter implementations land under src/ros2/transports/ in subsequent phases. Two name choices avoid collisions with structs that currently live in the ROS-coupled headers: * TopicTransport::sample returns TopicSample rather than TopicSampleResult, because core/data/data_types.hpp already defines a same-named struct with a different shape (per-topic batch errors, endpoint QoS) for the topic-data-provider plugin surface. * FaultServiceTransport::get_fault_with_env returns FaultWithEnvJsonResult rather than FaultWithEnvResult, because the legacy ros2_medkit_msgs-typed FaultWithEnvResult still lives next to the FaultManager facade. The two will be reconciled when the FaultManager migration lands. TopicSubscriptionHandle is a polymorphic RAII base (virtual destructor only) rather than a fully abstract port, so the smoke assertion uses sizeof > 0 instead of std::is_abstract_v. --- .../core/faults/fault_types.hpp | 12 +++ .../core/transports/action_transport.hpp | 57 ++++++++++++++ .../transports/fault_service_transport.hpp | 63 ++++++++++++++++ .../core/transports/log_source.hpp | 48 ++++++++++++ .../core/transports/parameter_transport.hpp | 74 +++++++++++++++++++ .../core/transports/service_transport.hpp | 38 ++++++++++ .../topic_subscription_transport.hpp | 60 +++++++++++++++ .../core/transports/topic_transport.hpp | 73 ++++++++++++++++++ .../test/test_gateway_core_smoke.cpp | 25 +++++++ 9 files changed, 450 insertions(+) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/action_transport.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/fault_service_transport.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/log_source.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/parameter_transport.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/service_transport.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_subscription_transport.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_transport.hpp 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 index dd352993..2df7e7f9 100644 --- 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 @@ -32,4 +32,16 @@ struct FaultResult { std::string error_message; }; +/// Neutral outcome of `get_fault_with_env`. `data` carries +/// `{ "fault": {...}, "environment_data": {...} }` already converted to JSON +/// by the transport. The legacy `FaultWithEnvResult` (which exposes raw +/// message types) still lives next to the FaultManager facade until a later +/// phase reconciles the two; the transport port returns this neutral form so +/// the interface compiles in the ROS-free build layer. +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/transports/action_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/action_transport.hpp new file mode 100644 index 00000000..7ac541af --- /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 00000000..63963d08 --- /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 00000000..f65e0f7b --- /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 00000000..f10242d7 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/parameter_transport.hpp @@ -0,0 +1,74 @@ +// 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; + + /// 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 00000000..c208f7c9 --- /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 00000000..75ace0f4 --- /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 00000000..bd10a241 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_transport.hpp @@ -0,0 +1,73 @@ +// 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_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; +}; + +} // 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 106ba80b..a0510eea 100644 --- a/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp +++ b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp @@ -30,6 +30,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,17 +48,25 @@ 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::DataProvider; 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::LogProvider; +using ros2_medkit_gateway::LogSource; using ros2_medkit_gateway::OperationProvider; +using ros2_medkit_gateway::ParameterTransport; using ros2_medkit_gateway::ScriptProvider; +using ros2_medkit_gateway::ServiceTransport; +using ros2_medkit_gateway::TopicSubscriptionHandle; +using ros2_medkit_gateway::TopicSubscriptionTransport; +using ros2_medkit_gateway::TopicTransport; using ros2_medkit_gateway::UpdateProvider; static_assert(sizeof(Area) > 0); @@ -66,6 +81,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 From 36a8bdecd5ef79003bc14d24d29699bc01e69f10 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 09:23:48 +0200 Subject: [PATCH 05/18] refactor(gateway): route DataAccessManager through TopicTransport adapter The data-access manager body becomes pure C++. All rclcpp use - generic publisher cache, JSON serializer instance, native sample backend, type introspection - moves into the new Ros2TopicTransport adapter under src/ros2/transports/. The manager now takes a shared_ptr plus the sample timeout and routes publish / sample / native-sample through it. The adapter exposes the type-introspection helper through the transport interface so handlers retain their existing accessor. The handler-facing public API (publish_to_topic, get_topic_sample_with_ fallback, get_topic_sample_native, get_type_introspection, set_topic_data_provider, get_topic_data_provider, get_topic_sample_ timeout) is preserved verbatim. The legacy include path ros2_medkit_gateway/data_access_manager.hpp is preserved as a forwarding shim. New mock-transport tests link only against gateway_core + GTest, proving the manager logic compiles without rclcpp on the link line. --- src/ros2_medkit_gateway/CMakeLists.txt | 18 +- .../core/managers/data_access_manager.hpp | 129 +++++++++++ .../core/transports/topic_transport.hpp | 10 + .../data_access_manager.hpp | 133 +---------- .../ros2_medkit_gateway/gateway_node.hpp | 6 + .../ros2/transports/ros2_topic_transport.hpp | 97 ++++++++ .../src/core/managers/data_access_manager.cpp | 82 +++++++ .../src/data_access_manager.cpp | 210 ------------------ src/ros2_medkit_gateway/src/gateway_node.cpp | 10 +- .../ros2/transports/ros2_topic_transport.cpp | 177 +++++++++++++++ .../test/test_data_access_manager.cpp | 52 +++-- .../test/test_data_access_manager_routing.cpp | 127 +++++++++++ .../test/test_gateway_core_smoke.cpp | 3 + 13 files changed, 692 insertions(+), 362 deletions(-) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/data_access_manager.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp create mode 100644 src/ros2_medkit_gateway/src/core/managers/data_access_manager.cpp delete mode 100644 src/ros2_medkit_gateway/src/data_access_manager.cpp create mode 100644 src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_transport.cpp create mode 100644 src/ros2_medkit_gateway/test/test_data_access_manager_routing.cpp diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index 40fc7ef6..be668561 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -150,8 +150,8 @@ target_precompile_headers(gateway_core PRIVATE 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/transports/ros2_topic_transport.cpp src/default_script_provider.cpp src/discovery/discovery_manager.cpp src/discovery/hybrid_discovery.cpp @@ -366,6 +366,22 @@ 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") + # Add GTest find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp_action REQUIRED) 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 00000000..c7e2cd6a --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/data_access_manager.hpp @@ -0,0 +1,129 @@ +// 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_gateway { + +using json = nlohmann::json; + +class TopicDataProvider; // forward decl, see core/data/topic_data_provider.hpp +class TypeIntrospection; // forward decl, defined in core/type_introspection.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. + */ + 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/transports/topic_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/topic_transport.hpp index bd10a241..465b9b2f 100644 --- 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 @@ -22,6 +22,8 @@ namespace ros2_medkit_gateway { +class TypeIntrospection; // forward decl - defined in core/type_introspection.hpp. + using json = nlohmann::json; /// Neutral result for a topic-sample call routed through the transport port. @@ -68,6 +70,14 @@ class TopicTransport { /// 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 is + /// rclcpp-coupled in the current implementation; the transport adapter owns + /// it so the manager body remains middleware-neutral. May return nullptr in + /// transports that do not support introspection (test mocks, alternative + /// middlewares). + virtual 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 1c5d12e8..524720c5 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/gateway_node.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/gateway_node.hpp index e07b50fd..385cab06 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,6 +49,7 @@ #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_topic_transport.hpp" #include "ros2_medkit_gateway/trigger_fault_subscriber.hpp" #include "ros2_medkit_gateway/trigger_topic_subscriber.hpp" @@ -252,6 +253,11 @@ 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_; + // Managers std::unique_ptr discovery_mgr_; std::unique_ptr data_access_mgr_; 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 00000000..d6fbe8c6 --- /dev/null +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp @@ -0,0 +1,97 @@ +// 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_gateway/core/type_introspection.hpp" +#include "ros2_medkit_serialization/json_serializer.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 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; + + 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/src/core/managers/data_access_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/data_access_manager.cpp new file mode 100644 index 00000000..da6b132b --- /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/data_access_manager.cpp b/src/ros2_medkit_gateway/src/data_access_manager.cpp deleted file mode 100644 index 90519f93..00000000 --- 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/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 75b50911..3372bf6c 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -160,6 +160,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", ""); @@ -559,7 +562,9 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki throw std::runtime_error("Discovery initialization failed"); } - data_access_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); operation_mgr_ = std::make_unique(this, discovery_mgr_.get()); config_mgr_ = std::make_unique(this); fault_mgr_ = std::make_unique(this); @@ -1447,6 +1452,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()); } 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 00000000..456a44a5 --- /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 { + 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/test/test_data_access_manager.cpp b/src/ros2_medkit_gateway/test/test_data_access_manager.cpp index c8fa508b..9fbfcdda 100644 --- a/src/ros2_medkit_gateway/test/test_data_access_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_data_access_manager.cpp @@ -25,6 +25,7 @@ #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" @@ -115,19 +116,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 +148,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 +210,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 +222,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 +249,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 +264,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 +296,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 +343,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 00000000..4bb805f0 --- /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 &) const override { + return {sample_publishers_, 0}; + } + + 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_gateway_core_smoke.cpp b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp index a0510eea..eb831536 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,7 @@ #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/managers/data_access_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" @@ -52,6 +53,7 @@ using ros2_medkit_gateway::ActionTransport; using ros2_medkit_gateway::App; using ros2_medkit_gateway::Area; using ros2_medkit_gateway::Component; +using ros2_medkit_gateway::DataAccessManager; using ros2_medkit_gateway::DataProvider; using ros2_medkit_gateway::FaultProvider; using ros2_medkit_gateway::FaultServiceTransport; @@ -73,6 +75,7 @@ static_assert(sizeof(Area) > 0); static_assert(sizeof(Component) > 0); static_assert(sizeof(App) > 0); static_assert(sizeof(Function) > 0); +static_assert(sizeof(DataAccessManager) > 0); static_assert(std::is_abstract_v); static_assert(std::is_abstract_v); static_assert(std::is_abstract_v); From 11cd08d49c2befcbdee995ecde698bdc8579e1ac Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 09:57:13 +0200 Subject: [PATCH 06/18] refactor(gateway): route OperationManager through Service/Action transports OperationManager retains all goal-tracking, UUID generation, validation, and component-namespace resolution logic in pure C++. The rclcpp side - GenericServiceClient cache, action-internal-service clients, the GoalStatusArray subscription cache - moves into two new adapters, Ros2ServiceTransport and Ros2ActionTransport, under src/ros2/transports/. The manager registers a per-goal status callback at subscribe time so the action transport pushes goal-status updates back into the manager's tracking map on its own thread. Goal-status array decoding moves into the transport; the manager only consumes the typed (path, goal_id, status) tuples. The handler-facing public API (call_service, call_component_service, send_action_goal, send_component_action_goal, cancel_action_goal, get_action_result, list_tracked_goals, update_goal_status, update_goal_feedback, cleanup_old_goals, subscribe_to_action_status, unsubscribe_from_action_status, the four static is_*-type helpers, the UUID helpers) is preserved. The component-name resolver dependency moves behind a new ServiceActionResolver port that DiscoveryManager now implements; this keeps the manager translation unit out of discovery_manager.hpp's rclcpp transitive include chain. Mock-transport tests link only against gateway_core + GTest, exercising 13 routing / lifecycle scenarios. --- scripts/check_no_naked_subscriptions.sh | 2 +- src/ros2_medkit_gateway/CMakeLists.txt | 20 +- .../discovery/service_action_resolver.hpp | 57 ++ .../core/managers/operation_manager.hpp | 189 ++++ .../discovery/discovery_manager.hpp | 9 +- .../ros2_medkit_gateway/gateway_node.hpp | 8 + .../ros2_medkit_gateway/operation_manager.hpp | 233 +---- .../ros2/transports/ros2_action_transport.hpp | 101 ++ .../transports/ros2_service_transport.hpp | 67 ++ .../src/core/managers/operation_manager.cpp | 491 ++++++++++ src/ros2_medkit_gateway/src/gateway_node.cpp | 7 +- .../src/operation_manager.cpp | 917 ------------------ .../ros2/transports/ros2_action_transport.cpp | 469 +++++++++ .../transports/ros2_service_transport.cpp | 160 +++ .../test/test_gateway_core_smoke.cpp | 6 + .../test/test_operation_manager.cpp | 17 +- .../test/test_operation_manager_routing.cpp | 387 ++++++++ 17 files changed, 1982 insertions(+), 1158 deletions(-) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/discovery/service_action_resolver.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/operation_manager.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_action_transport.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_service_transport.hpp create mode 100644 src/ros2_medkit_gateway/src/core/managers/operation_manager.cpp delete mode 100644 src/ros2_medkit_gateway/src/operation_manager.cpp create mode 100644 src/ros2_medkit_gateway/src/ros2/transports/ros2_action_transport.cpp create mode 100644 src/ros2_medkit_gateway/src/ros2/transports/ros2_service_transport.cpp create mode 100644 src/ros2_medkit_gateway/test/test_operation_manager_routing.cpp diff --git a/scripts/check_no_naked_subscriptions.sh b/scripts/check_no_naked_subscriptions.sh index e9514aab..96160def 100755 --- a/scripts/check_no_naked_subscriptions.sh +++ b/scripts/check_no_naked_subscriptions.sh @@ -47,7 +47,7 @@ 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/ros2/transports/ros2_action_transport.cpp" # operations provider follow-up "${GATEWAY_ROOT}/src/log_manager.cpp" # logs provider follow-up "${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 diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index be668561..ea2d43e7 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -151,6 +151,8 @@ add_library(gateway_ros2 STATIC src/aggregation/aggregation_manager.cpp src/configuration_manager.cpp src/data/ros2_topic_data_provider.cpp + src/ros2/transports/ros2_action_transport.cpp + src/ros2/transports/ros2_service_transport.cpp src/ros2/transports/ros2_topic_transport.cpp src/default_script_provider.cpp src/discovery/discovery_manager.cpp @@ -187,7 +189,6 @@ add_library(gateway_ros2 STATIC 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 @@ -382,6 +383,23 @@ if(BUILD_TESTING) ) 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") + # Add GTest find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp_action REQUIRED) 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 00000000..d895db87 --- /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/managers/operation_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/operation_manager.hpp new file mode 100644 index 00000000..51e740d7 --- /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/discovery/discovery_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_manager.hpp index 03cdc578..a2b085ff 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 @@ -22,6 +22,7 @@ #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" @@ -130,7 +131,7 @@ struct DiscoveryConfig { * @see discovery::HybridDiscoveryStrategy * @see discovery::ManifestManager */ -class DiscoveryManager { +class DiscoveryManager : public ServiceActionResolver { public: /** * @brief Construct the discovery manager @@ -282,7 +283,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 +292,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 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 385cab06..906fcd79 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,6 +49,8 @@ #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_service_transport.hpp" #include "ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp" #include "ros2_medkit_gateway/trigger_fault_subscriber.hpp" #include "ros2_medkit_gateway/trigger_topic_subscriber.hpp" @@ -258,6 +260,12 @@ class GatewayNode : public rclcpp::Node { // 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_; + // Managers std::unique_ptr discovery_mgr_; std::unique_ptr data_access_mgr_; 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 29179112..d1c4c43c 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,232 +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/core/operations/operation_types.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; - -/// 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/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 00000000..69b6d141 --- /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_service_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_service_transport.hpp new file mode 100644 index 00000000..c61936b5 --- /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/src/core/managers/operation_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/operation_manager.cpp new file mode 100644 index 00000000..1abbebd3 --- /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/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 3372bf6c..bb550215 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -565,7 +565,12 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki 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); - operation_mgr_ = std::make_unique(this, discovery_mgr_.get()); + 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); config_mgr_ = std::make_unique(this); fault_mgr_ = std::make_unique(this); 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 f7391da6..00000000 --- a/src/ros2_medkit_gateway/src/operation_manager.cpp +++ /dev/null @@ -1,917 +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::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/transports/ros2_action_transport.cpp b/src/ros2_medkit_gateway/src/ros2/transports/ros2_action_transport.cpp new file mode 100644 index 00000000..0dc35655 --- /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/ros2/transports/ros2_service_transport.cpp b/src/ros2_medkit_gateway/src/ros2/transports/ros2_service_transport.cpp new file mode 100644 index 00000000..2adc5109 --- /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/test/test_gateway_core_smoke.cpp b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp index eb831536..fd4e0edf 100644 --- a/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp +++ b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp @@ -22,7 +22,9 @@ #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/data_access_manager.hpp" +#include "ros2_medkit_gateway/core/managers/operation_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" @@ -62,9 +64,11 @@ using ros2_medkit_gateway::HostInfoProvider; using ros2_medkit_gateway::IntrospectionProvider; 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; @@ -76,6 +80,8 @@ static_assert(sizeof(Component) > 0); static_assert(sizeof(App) > 0); static_assert(sizeof(Function) > 0); static_assert(sizeof(DataAccessManager) > 0); +static_assert(sizeof(OperationManager) > 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); diff --git a/src/ros2_medkit_gateway/test/test_operation_manager.cpp b/src/ros2_medkit_gateway/test/test_operation_manager.cpp index 26f1b4be..3d30a4bb 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 00000000..d2341a6a --- /dev/null +++ b/src/ros2_medkit_gateway/test/test_operation_manager_routing.cpp @@ -0,0 +1,387 @@ +// 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 &, const std::string &) const override { + return service_; + } + std::optional find_action(const std::string &, const std::string &) 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 From 9ad4485dcd7fa383abd0c0c780b77d581c4c1baa Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 10:20:12 +0200 Subject: [PATCH 07/18] refactor(gateway): route ConfigurationManager through ParameterTransport ConfigurationManager loses its rclcpp::SyncParametersClient cache, the rclcpp::Parameter defaults cache, the negative-cache for unreachable nodes, and the spin_mutex serialising parameter-client spins. All of these move into the new Ros2ParameterTransport adapter under src/ros2/transports/. The adapter also owns the JSON <-> rclcpp:: ParameterValue conversion helpers and the gateway-own-FQN check that previously lived as private members of the manager. Manager retains: per-entity reset orchestration (reset_parameter and reset_all_parameters compose set_parameter with transport-supplied defaults via the new get_default and list_defaults methods on ParameterTransport), the self-node short circuit (delegated to the transport's is_self_node), and the shutdown ordering contract (idempotent manager.shutdown() fans out to transport.shutdown() before rclcpp::shutdown()). The handler-facing public API (list_parameters, get_parameter, set_parameter, reset_parameter, reset_all_parameters, shutdown) is preserved verbatim; the legacy include path ros2_medkit_gateway/configuration_manager.hpp is preserved as a forwarding shim. Parameter-service tuning parameters (parameter_service_timeout_sec, parameter_service_negative_cache_sec) are declared at gateway_node wiring time and passed to the transport constructor. ParameterTransport gains two new pure-virtual methods (get_default, list_defaults) that surface the cached defaults as neutral JSON for manager-level reset orchestration. The gateway_core link-time smoke test now also pins ConfigurationManager. Mock-transport tests link exclusively against gateway_core + GTest, with no rclcpp on the link line, covering CRUD delegation, self-node short-circuit, reset via cached defaults, partial-failure aggregation in reset_all_parameters, and shutdown idempotency. --- src/ros2_medkit_gateway/CMakeLists.txt | 20 +- .../configuration_manager.hpp | 166 +---- .../core/managers/configuration_manager.hpp | 107 +++ .../core/transports/parameter_transport.hpp | 16 + .../ros2_medkit_gateway/gateway_node.hpp | 6 + .../transports/ros2_parameter_transport.hpp | 163 +++++ .../core/managers/configuration_manager.cpp | 148 +++++ src/ros2_medkit_gateway/src/gateway_node.cpp | 26 +- .../transports/ros2_parameter_transport.cpp} | 621 ++++++++---------- .../test/test_configuration_manager.cpp | 8 +- .../test_configuration_manager_routing.cpp | 323 +++++++++ .../test/test_gateway_core_smoke.cpp | 3 + 12 files changed, 1078 insertions(+), 529 deletions(-) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/configuration_manager.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_parameter_transport.hpp create mode 100644 src/ros2_medkit_gateway/src/core/managers/configuration_manager.cpp rename src/ros2_medkit_gateway/src/{configuration_manager.cpp => ros2/transports/ros2_parameter_transport.cpp} (62%) create mode 100644 src/ros2_medkit_gateway/test/test_configuration_manager_routing.cpp diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index ea2d43e7..e8ebfe73 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -149,9 +149,9 @@ 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/ros2_topic_data_provider.cpp src/ros2/transports/ros2_action_transport.cpp + src/ros2/transports/ros2_parameter_transport.cpp src/ros2/transports/ros2_service_transport.cpp src/ros2/transports/ros2_topic_transport.cpp src/default_script_provider.cpp @@ -400,6 +400,24 @@ if(BUILD_TESTING) ) 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") + # Add GTest find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp_action REQUIRED) 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 82a85e6c..7c072593 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,165 +14,5 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ros2_medkit_gateway/core/configuration/parameter_types.hpp" - -namespace ros2_medkit_gateway { - -using json = nlohmann::json; - -/// 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/managers/configuration_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/configuration_manager.hpp new file mode 100644 index 00000000..6d0a9323 --- /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/transports/parameter_transport.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/transports/parameter_transport.hpp index f10242d7..7aed276b 100644 --- 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 @@ -60,6 +60,22 @@ class ParameterTransport { 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; 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 906fcd79..2e19a218 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 @@ -50,6 +50,7 @@ #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_parameter_transport.hpp" #include "ros2_medkit_gateway/ros2/transports/ros2_service_transport.hpp" #include "ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp" #include "ros2_medkit_gateway/trigger_fault_subscriber.hpp" @@ -266,6 +267,11 @@ class GatewayNode : public rclcpp::Node { 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_; + // Managers std::unique_ptr discovery_mgr_; std::unique_ptr data_access_mgr_; 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 00000000..992ac867 --- /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/src/core/managers/configuration_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/configuration_manager.cpp new file mode 100644 index 00000000..bf9f4dbc --- /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/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index bb550215..64f35346 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -571,7 +571,31 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki 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); - config_mgr_ = std::make_unique(this); + + // 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_mgr_ = std::make_unique(this); // Initialize bulk data store 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 913e9744..0fe8acfb 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/test/test_configuration_manager.cpp b/src/ros2_medkit_gateway/test/test_configuration_manager.cpp index a6f2ce21..11599a95 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_; 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 00000000..80911f3f --- /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 &) 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_gateway_core_smoke.cpp b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp index fd4e0edf..ff0d1f37 100644 --- a/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp +++ b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp @@ -23,6 +23,7 @@ #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/operation_manager.hpp" #include "ros2_medkit_gateway/core/providers/data_provider.hpp" @@ -55,6 +56,7 @@ 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::FaultProvider; @@ -79,6 +81,7 @@ 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(OperationManager) > 0); static_assert(std::is_abstract_v); From 01bf7aa0750860ffc5983f9b4e8e0e7e24f656bb Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 11:18:20 +0200 Subject: [PATCH 08/18] refactor(gateway): route FaultManager through FaultServiceTransport FaultManager loses its seven rclcpp::Client members and their seven per-client mutexes; all move into the new Ros2FaultServiceTransport adapter under src/ros2/transports/. The adapter performs the ros2_medkit_msgs to JSON conversion internally and returns the neutral FaultResult / FaultWithEnvJsonResult structures, so the manager body now lives in the ROS-free build layer (gateway_core). The handler-facing public API (report_fault, list_faults, get_fault, get_fault_with_env, clear_fault, get_snapshots, get_rosbag, list_rosbags, is_available, wait_for_services) is preserved verbatim. The single behaviour change is that get_fault_with_env now returns the response body as JSON: data carries { "fault": {...}, "environment_data": {...} } already converted by the transport. The single handler call site (build_sovd_fault_response) is updated to consume the JSON, post-processing rosbag snapshots to add the per- request bulk_data_uri and freeze_frame snapshots to extract the primary value. The previously static FaultManager::fault_to_json helper is removed. Two external call sites (SSEFaultHandler, TriggerFaultSubscriber) subscribe directly to the FaultEvent topic and convert the message themselves; they now use the small ros2/conversions/fault_msg_conversions module that lives at the ROS-coupled boundary alongside the transport. The legacy include path ros2_medkit_gateway/fault_manager.hpp is preserved as a forwarding shim. New mock-transport tests link only against gateway_core + GTest, proving the manager logic compiles without rclcpp on the link line. --- src/ros2_medkit_gateway/CMakeLists.txt | 21 +- .../core/faults/fault_types.hpp | 11 +- .../core/managers/fault_manager.hpp | 104 +++++ .../ros2_medkit_gateway/fault_manager.hpp | 137 +------ .../ros2_medkit_gateway/gateway_node.hpp | 7 + .../http/handlers/fault_handlers.hpp | 30 +- .../conversions/fault_msg_conversions.hpp | 55 +++ .../ros2_fault_service_transport.hpp | 111 ++++++ .../trigger_fault_subscriber.hpp | 2 +- .../src/core/managers/fault_manager.cpp | 68 ++++ src/ros2_medkit_gateway/src/gateway_node.cpp | 3 +- .../src/http/handlers/fault_handlers.cpp | 123 +++--- .../src/http/handlers/sse_fault_handler.cpp | 4 +- .../src/openapi/schema_builder.hpp | 2 +- .../conversions/fault_msg_conversions.cpp | 100 +++++ .../ros2_fault_service_transport.cpp} | 169 ++++---- .../src/trigger_fault_subscriber.cpp | 6 +- .../test/test_fault_handlers.cpp | 53 ++- .../test/test_fault_manager.cpp | 25 +- .../test/test_fault_manager_routing.cpp | 368 ++++++++++++++++++ .../test/test_gateway_core_smoke.cpp | 3 + 21 files changed, 1058 insertions(+), 344 deletions(-) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/fault_manager.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/conversions/fault_msg_conversions.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_fault_service_transport.hpp create mode 100644 src/ros2_medkit_gateway/src/core/managers/fault_manager.cpp create mode 100644 src/ros2_medkit_gateway/src/ros2/conversions/fault_msg_conversions.cpp rename src/ros2_medkit_gateway/src/{fault_manager.cpp => ros2/transports/ros2_fault_service_transport.cpp} (72%) create mode 100644 src/ros2_medkit_gateway/test/test_fault_manager_routing.cpp diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index e8ebfe73..e44eea34 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -150,7 +150,9 @@ target_precompile_headers(gateway_core PRIVATE add_library(gateway_ros2 STATIC src/aggregation/aggregation_manager.cpp src/data/ros2_topic_data_provider.cpp + src/ros2/conversions/fault_msg_conversions.cpp src/ros2/transports/ros2_action_transport.cpp + src/ros2/transports/ros2_fault_service_transport.cpp src/ros2/transports/ros2_parameter_transport.cpp src/ros2/transports/ros2_service_transport.cpp src/ros2/transports/ros2_topic_transport.cpp @@ -165,7 +167,6 @@ add_library(gateway_ros2 STATIC 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 @@ -418,6 +419,24 @@ if(BUILD_TESTING) ) 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") + # Add GTest find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp_action REQUIRED) 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 index 2df7e7f9..82080076 100644 --- 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 @@ -23,9 +23,7 @@ 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. The richer `FaultWithEnvResult`, which still exposes raw message -/// types, lives alongside the FaultManager facade until the transport -/// extraction lands. +/// errors. struct FaultResult { bool success; json data; @@ -34,10 +32,9 @@ struct FaultResult { /// Neutral outcome of `get_fault_with_env`. `data` carries /// `{ "fault": {...}, "environment_data": {...} }` already converted to JSON -/// by the transport. The legacy `FaultWithEnvResult` (which exposes raw -/// message types) still lives next to the FaultManager facade until a later -/// phase reconciles the two; the transport port returns this neutral form so -/// the interface compiles in the ROS-free build layer. +/// 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; 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 00000000..85798df2 --- /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/fault_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/fault_manager.hpp index f76ec57f..ac0d7670 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,136 +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" - -#include "ros2_medkit_gateway/core/faults/fault_types.hpp" - -namespace ros2_medkit_gateway { - -using json = nlohmann::json; - -/// 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 2e19a218..a5242cab 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 @@ -50,6 +50,7 @@ #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_parameter_transport.hpp" #include "ros2_medkit_gateway/ros2/transports/ros2_service_transport.hpp" #include "ros2_medkit_gateway/ros2/transports/ros2_topic_transport.hpp" @@ -272,6 +273,12 @@ class GatewayNode : public rclcpp::Node { // 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_; + // Managers std::unique_ptr discovery_mgr_; std::unique_ptr data_access_mgr_; 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 f84a066f..014fdf0f 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/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 00000000..2869f394 --- /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/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 00000000..7d8ddf96 --- /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/trigger_fault_subscriber.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_fault_subscriber.hpp index 89ec9779..1fc5ba4d 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/src/core/managers/fault_manager.cpp b/src/ros2_medkit_gateway/src/core/managers/fault_manager.cpp new file mode 100644 index 00000000..68f22b20 --- /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/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 64f35346..5ad450f3 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -596,7 +596,8 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki parameter_transport_ = std::make_shared(this, parameter_service_timeout_sec, parameter_service_negative_cache_sec); config_mgr_ = std::make_unique(parameter_transport_); - fault_mgr_ = std::make_unique(this); + 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(); 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 b8886335..716dd6c8 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,91 @@ 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); + snap["bulk_data_uri"] = entity_path + "/bulk-data/rosbags/" + snap_fault_code; + 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 +658,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/sse_fault_handler.cpp b/src/ros2_medkit_gateway/src/http/handlers/sse_fault_handler.cpp index 8010dbb4..3046599b 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 6bf365dd..86ffa5ed 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/ros2/conversions/fault_msg_conversions.cpp b/src/ros2_medkit_gateway/src/ros2/conversions/fault_msg_conversions.cpp new file mode 100644 index 00000000..06302c77 --- /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/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 68d31633..1cff05a5 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/trigger_fault_subscriber.cpp b/src/ros2_medkit_gateway/src/trigger_fault_subscriber.cpp index ff05b455..31a334f9 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/test/test_fault_handlers.cpp b/src/ros2_medkit_gateway/test/test_fault_handlers.cpp index f01ea6b9..677c3fd0 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(); @@ -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 1126110b..a7e65764 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 00000000..59fd6c5d --- /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 ff0d1f37..eb8cb869 100644 --- a/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp +++ b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp @@ -25,6 +25,7 @@ #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/operation_manager.hpp" #include "ros2_medkit_gateway/core/providers/data_provider.hpp" #include "ros2_medkit_gateway/core/providers/fault_provider.hpp" @@ -59,6 +60,7 @@ 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; @@ -83,6 +85,7 @@ 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(OperationManager) > 0); static_assert(std::is_abstract_v); static_assert(std::is_abstract_v); From 67b11e69c13449857b1ed9225b8f2ba18dda46e5 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 11:52:42 +0200 Subject: [PATCH 09/18] refactor(gateway): route LogManager through LogSource adapter LogManager keeps its ring-buffer storage, per-entity config map, monotonic id counter, plugin observer pattern, and the manages_ingestion short-circuit - all pure C++. The /rosout subscription and the rcl_interfaces::msg::Log to LogEntry conversion move into the new Ros2LogSource adapter under src/ros2/transports/. The adapter emits LogEntry through a callback the manager registers at start() time; when the primary LogProvider declares full ingestion, the manager never calls start() (matching today's behaviour of not creating the subscription at all in that mode). To let the manager body live in gateway_core alongside the other managers, the PluginManager pointer is replaced with a thin LogProviderRegistry port (primary_log_provider + log_observers). PluginManager implements the port inline, so production wiring is unchanged. The handler-facing public API (get_logs, get_config, update_config, add_log_entry, set_notifier, set_node_to_entity_resolver, the static helpers, inject_entry_for_testing) is preserved verbatim. Mock-source tests link only against gateway_core + GTest, proving the manager logic compiles without rclcpp on the link line. The legacy include path ros2_medkit_gateway/log_manager.hpp is preserved as a forwarding shim. --- scripts/check_no_naked_subscriptions.sh | 2 +- src/ros2_medkit_gateway/CMakeLists.txt | 20 +- .../core/managers/log_manager.hpp | 208 ++++++++++++ .../core/plugins/plugin_manager.hpp | 16 +- .../core/providers/log_provider_registry.hpp | 57 ++++ .../ros2_medkit_gateway/gateway_node.hpp | 6 + .../ros2_medkit_gateway/log_manager.hpp | 184 +---------- .../ros2/transports/ros2_log_source.hpp | 78 +++++ .../src/{ => core/managers}/log_manager.cpp | 128 +++---- src/ros2_medkit_gateway/src/gateway_node.cpp | 3 +- .../src/ros2/transports/ros2_log_source.cpp | 84 +++++ .../test/test_gateway_core_smoke.cpp | 3 + .../test/test_log_manager.cpp | 37 ++- .../test/test_log_manager_routing.cpp | 312 ++++++++++++++++++ 14 files changed, 881 insertions(+), 257 deletions(-) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/managers/log_manager.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/providers/log_provider_registry.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_log_source.hpp rename src/ros2_medkit_gateway/src/{ => core/managers}/log_manager.cpp (80%) create mode 100644 src/ros2_medkit_gateway/src/ros2/transports/ros2_log_source.cpp create mode 100644 src/ros2_medkit_gateway/test/test_log_manager_routing.cpp diff --git a/scripts/check_no_naked_subscriptions.sh b/scripts/check_no_naked_subscriptions.sh index 96160def..f0746317 100755 --- a/scripts/check_no_naked_subscriptions.sh +++ b/scripts/check_no_naked_subscriptions.sh @@ -48,7 +48,7 @@ ALLOWED_LEGACY_FILES=( "${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/ros2/transports/ros2_action_transport.cpp" # operations provider follow-up - "${GATEWAY_ROOT}/src/log_manager.cpp" # logs 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 e44eea34..871b5326 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -153,6 +153,7 @@ add_library(gateway_ros2 STATIC src/ros2/conversions/fault_msg_conversions.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_transport.cpp @@ -189,7 +190,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/openapi/capability_generator.cpp src/openapi/openapi_spec_builder.cpp src/openapi/path_builder.cpp @@ -437,6 +437,24 @@ if(BUILD_TESTING) ) 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") + # Add GTest find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp_action REQUIRED) 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 00000000..22669140 --- /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/plugins/plugin_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/core/plugins/plugin_manager.hpp index 079dea3c..1f1aabc8 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 00000000..7a68f3b3 --- /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/gateway_node.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/gateway_node.hpp index a5242cab..1a93f8d1 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 @@ -51,6 +51,7 @@ #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_transport.hpp" @@ -279,6 +280,11 @@ class GatewayNode : public rclcpp::Node { // 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_; 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 a2361009..00508b87 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/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 00000000..adaf04bb --- /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/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 1df9dfcd..571a9f56 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/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 5ad450f3..4d8abb10 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -708,7 +708,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(); 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 00000000..dcb22652 --- /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/test/test_gateway_core_smoke.cpp b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp index eb8cb869..fc368715 100644 --- a/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp +++ b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp @@ -26,6 +26,7 @@ #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/providers/data_provider.hpp" #include "ros2_medkit_gateway/core/providers/fault_provider.hpp" @@ -66,6 +67,7 @@ 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; @@ -86,6 +88,7 @@ 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(std::is_abstract_v); static_assert(std::is_abstract_v); diff --git a/src/ros2_medkit_gateway/test/test_log_manager.cpp b/src/ros2_medkit_gateway/test/test_log_manager.cpp index 839a5709..763b552a 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 00000000..cda99585 --- /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 From 05b9af7b3d92a1430fbe81331bc9f083018accf1 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 12:21:21 +0200 Subject: [PATCH 10/18] refactor(gateway): route TriggerManager through TopicSubscriptionTransport TriggerManager keeps all its condition-evaluation, retry-unresolved, sweep-orphaned, persistence, dispatch-index, and entity-cache logic in pure C++. The pointer to TriggerTopicSubscriber is replaced with a shared TopicSubscriptionTransport interface; the new Ros2TopicSubscriptionTransport adapter wraps the existing TriggerTopicSubscriber, preserving its subscription-destructor pattern. Per-trigger subscription handles live in the manager's tracking map - destruction unsubscribes - so trigger lifetime fully drives subscription lifetime. The trigger manager source moves to src/core/managers/ so it is picked up by gateway_core's GLOB. TriggerTopicSubscriber moves to include/ros2_medkit_gateway/ros2/ and src/ros2/, and a forwarding shim at the old header path keeps existing includes working. TriggerTopicSubscriber itself becomes a generic per-handle subscription executor: subscribe(topic, type, handle_key, callback) creates one GenericSubscription per key, unsubscribe(handle_key) tears it down, and pending/retry semantics are preserved with the same 60s timeout. The handler-facing public API (create / get / list / update / remove, wait_for_event, consume_pending_event, set_on_removed, set_entity_children_fn, set_entity_exists_fn, set_resolve_topic_fn, retry_unresolved_triggers, sweep_orphaned_triggers, load_persistent_triggers) is preserved verbatim. Mock-transport tests link only against gateway_core + GTest, proving the manager body compiles without rclcpp on the link line. --- scripts/check_no_naked_subscriptions.sh | 2 +- src/ros2_medkit_gateway/CMakeLists.txt | 23 +- .../core/managers/trigger_manager.hpp | 32 +- .../ros2_medkit_gateway/gateway_node.hpp | 5 +- .../ros2_topic_subscription_transport.hpp | 87 +++++ .../ros2/trigger_topic_subscriber.hpp | 152 +++++++++ .../trigger_topic_subscriber.hpp | 143 +------- .../{ => core/managers}/trigger_manager.cpp | 102 ++++-- src/ros2_medkit_gateway/src/gateway_node.cpp | 12 +- .../ros2_topic_subscription_transport.cpp | 56 +++ .../src/ros2/trigger_topic_subscriber.cpp | 244 +++++++++++++ .../src/trigger_topic_subscriber.cpp | 260 -------------- .../test/test_gateway_core_smoke.cpp | 3 + .../test/test_trigger_manager_routing.cpp | 323 ++++++++++++++++++ 14 files changed, 995 insertions(+), 449 deletions(-) create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/transports/ros2_topic_subscription_transport.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/trigger_topic_subscriber.hpp rename src/ros2_medkit_gateway/src/{ => core/managers}/trigger_manager.cpp (87%) create mode 100644 src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_subscription_transport.cpp create mode 100644 src/ros2_medkit_gateway/src/ros2/trigger_topic_subscriber.cpp delete mode 100644 src/ros2_medkit_gateway/src/trigger_topic_subscriber.cpp create mode 100644 src/ros2_medkit_gateway/test/test_trigger_manager_routing.cpp diff --git a/scripts/check_no_naked_subscriptions.sh b/scripts/check_no_naked_subscriptions.sh index f0746317..38f0309a 100755 --- a/scripts/check_no_naked_subscriptions.sh +++ b/scripts/check_no_naked_subscriptions.sh @@ -46,7 +46,7 @@ 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/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) diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index 871b5326..40e48816 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -156,7 +156,9 @@ add_library(gateway_ros2 STATIC 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 @@ -202,8 +204,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 ) @@ -455,6 +455,25 @@ if(BUILD_TESTING) ) 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) 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 ac45da3c..0c0e0394 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/gateway_node.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/gateway_node.hpp index 1a93f8d1..42aa3267 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 @@ -54,9 +54,10 @@ #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 { @@ -314,6 +315,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. 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 00000000..2a3ee215 --- /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/trigger_topic_subscriber.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/trigger_topic_subscriber.hpp new file mode 100644 index 00000000..da8aa896 --- /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_topic_subscriber.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/trigger_topic_subscriber.hpp index 7c1f67b4..8a188960 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/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 a5ba6a78..65c24860 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/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 4d8abb10..f551e77e 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -870,8 +870,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( @@ -918,10 +924,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. 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 00000000..43aa67fa --- /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/trigger_topic_subscriber.cpp b/src/ros2_medkit_gateway/src/ros2/trigger_topic_subscriber.cpp new file mode 100644 index 00000000..cf5e318c --- /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_topic_subscriber.cpp b/src/ros2_medkit_gateway/src/trigger_topic_subscriber.cpp deleted file mode 100644 index 7f518ea3..00000000 --- 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_gateway_core_smoke.cpp b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp index fc368715..93cc79f3 100644 --- a/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp +++ b/src/ros2_medkit_gateway/test/test_gateway_core_smoke.cpp @@ -28,6 +28,7 @@ #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" @@ -79,6 +80,7 @@ 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); @@ -90,6 +92,7 @@ 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); 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 00000000..4d3e6fd5 --- /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_FALSE(manager_->list("gone_app").size()); + 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 From 1e57616f7f7b603ec10dbc3d19921b1485df798f Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 12:48:02 +0200 Subject: [PATCH 11/18] refactor(gateway): convert runtime discovery to IntrospectionProvider RuntimeDiscoveryStrategy is replaced by Ros2RuntimeIntrospection, which implements the existing IntrospectionProvider interface used by the merge pipeline for plugin-driven entity discovery. Built-in ROS graph queries are now treated identically to plugin-provided introspection - one chain, one merge policy. HybridDiscoveryStrategy is removed; the equivalent runtime + manifest + plugin merge is the default MergePipeline configuration. The discovery_mode parameter (runtime_only / manifest_only / hybrid) now controls which layers the merge pipeline activates rather than which strategy class to instantiate. --- src/ros2_medkit_gateway/CMakeLists.txt | 5 +- .../config/gateway_params.yaml | 12 +- src/ros2_medkit_gateway/design/index.rst | 36 ++-- .../core/discovery/discovery_strategy.hpp | 11 +- .../core/discovery/layers/runtime_layer.hpp | 8 +- .../discovery/discovery_manager.hpp | 75 ++++--- .../discovery/hybrid_discovery.hpp | 93 --------- .../discovery/merge_pipeline.hpp | 6 +- .../discovery/runtime_discovery.hpp | 191 ------------------ .../ros2_medkit_gateway/gateway_node.hpp | 15 +- .../providers/ros2_runtime_introspection.hpp | 158 +++++++++++++++ .../src/discovery/discovery_manager.cpp | 166 +++++++++------ .../src/discovery/hybrid_discovery.cpp | 84 -------- .../src/discovery/layers/runtime_layer.cpp | 17 +- src/ros2_medkit_gateway/src/gateway_node.cpp | 6 +- .../providers/ros2_runtime_introspection.cpp} | 144 +++++-------- .../test/test_runtime_discovery.cpp | 52 ++--- 17 files changed, 444 insertions(+), 635 deletions(-) delete mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/hybrid_discovery.hpp delete mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/runtime_discovery.hpp create mode 100644 src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2/providers/ros2_runtime_introspection.hpp delete mode 100644 src/ros2_medkit_gateway/src/discovery/hybrid_discovery.cpp rename src/ros2_medkit_gateway/src/{discovery/runtime_discovery.cpp => ros2/providers/ros2_runtime_introspection.cpp} (71%) diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index 40e48816..602448f8 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -151,6 +151,7 @@ add_library(gateway_ros2 STATIC src/aggregation/aggregation_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 @@ -161,7 +162,6 @@ add_library(gateway_ros2 STATIC 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 @@ -169,7 +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_paths.cpp src/gateway_node.cpp src/http/handlers/auth_handlers.cpp @@ -517,7 +516,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/config/gateway_params.yaml b/src/ros2_medkit_gateway/config/gateway_params.yaml index 93b04ebe..d7283dc1 100644 --- a/src/ros2_medkit_gateway/config/gateway_params.yaml +++ b/src/ros2_medkit_gateway/config/gateway_params.yaml @@ -156,10 +156,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/index.rst b/src/ros2_medkit_gateway/design/index.rst index b9c6e95f..f2b0f3e9 100644 --- a/src/ros2_medkit_gateway/design/index.rst +++ b/src/ros2_medkit_gateway/design/index.rst @@ -109,7 +109,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 +121,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 +140,7 @@ The following diagram shows the relationships between the main components of the } class RuntimeLayer { - - strategy_: RuntimeDiscoveryStrategy* + - runtime_introspection_: Ros2RuntimeIntrospection* - gap_fill_config_: GapFillConfig } @@ -398,11 +392,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 +402,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 @@ -461,17 +453,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 +483,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). 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 2a80ac1d..e1ab7ecf 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 6aa89603..79b9228d 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/discovery/discovery_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_manager.hpp index a2b085ff..9a829f3f 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,7 +15,6 @@ #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" @@ -24,12 +23,13 @@ #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 @@ -113,22 +113,23 @@ 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 : public ServiceActionResolver { @@ -333,6 +334,15 @@ class DiscoveryManager : public ServiceActionResolver { */ 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) * @@ -371,8 +381,8 @@ class DiscoveryManager : public ServiceActionResolver { } /** - * @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; @@ -390,9 +400,21 @@ class DiscoveryManager : public ServiceActionResolver { 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 @@ -406,13 +428,18 @@ class DiscoveryManager : public ServiceActionResolver { // 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 5009c1dc..00000000 --- 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 2a880c76..4d2db9ac 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 c25ce9c6..00000000 --- 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/gateway_node.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/gateway_node.hpp index 42aa3267..7eb382e0 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 @@ -218,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); @@ -335,10 +336,10 @@ class GatewayNode : public rclcpp::Node { // 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/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 00000000..e05f0552 --- /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_gateway/core/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(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}; + 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/src/discovery/discovery_manager.cpp b/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp index c6cf21e8..754c694d 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); + 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 464651d1..00000000 --- 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 84020722..1c990d83 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 f551e77e..786ee33a 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -1689,9 +1689,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/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 bfb55872..adab2a2f 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(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/test/test_runtime_discovery.cpp b/src/ros2_medkit_gateway/test/test_runtime_discovery.cpp index 62b0795c..6f758f89 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"; } From fa3e40af24a4d4aa9346d0096d2b498b113982e3 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 13:22:45 +0200 Subject: [PATCH 12/18] refactor: relocate TypeIntrospection to ros2_medkit_serialization The class is the rosidl_typesupport_cpp + rosidl_typesupport_introspection_cpp bridge built on top of JsonSerializer; it semantically belongs alongside the rest of the rosidl glue rather than in the gateway. Gateway packages now depend on the serialization library for type schemas instead of duplicating the introspection backend. - Move type_introspection.{hpp,cpp} to ros2_medkit_serialization (preserves Apache 2.0 header verbatim). - Migrate the namespace from ros2_medkit_gateway to ros2_medkit_serialization; qualify all consumer call sites (gateway managers, transports, providers, handlers, discovery, tests). - Update forward declarations in topic_transport.hpp, data_access_manager.hpp, discovery_manager.hpp. - Move the TypeIntrospection unit suite from src/ros2_medkit_gateway/test/test_data_access_manager.cpp to src/ros2_medkit_serialization/test/test_type_introspection.cpp; register the new test target. - Drop src/type_introspection.cpp from the gateway gateway_ros2 source list. The rosidl_typesupport_cpp / rosidl_typesupport_introspection_cpp deps remain in the gateway because the GenericClient compat shim still uses them. --- src/ros2_medkit_gateway/CMakeLists.txt | 1 - .../core/managers/data_access_manager.hpp | 7 +- .../core/transports/topic_transport.hpp | 18 ++-- .../discovery/discovery_manager.hpp | 7 +- .../providers/ros2_runtime_introspection.hpp | 6 +- .../ros2/transports/ros2_topic_transport.hpp | 11 +-- .../src/discovery/discovery_manager.cpp | 2 +- .../src/http/handlers/data_handlers.cpp | 1 + .../src/http/handlers/operation_handlers.cpp | 1 + .../providers/ros2_runtime_introspection.cpp | 2 +- .../ros2/transports/ros2_topic_transport.cpp | 4 +- .../test/test_data_access_manager.cpp | 70 --------------- .../test/test_data_access_manager_routing.cpp | 2 +- src/ros2_medkit_serialization/CMakeLists.txt | 10 +++ .../type_introspection.hpp | 6 +- .../src/type_introspection.cpp | 12 +-- .../test/test_type_introspection.cpp | 88 +++++++++++++++++++ 17 files changed, 143 insertions(+), 105 deletions(-) rename src/{ros2_medkit_gateway/include/ros2_medkit_gateway/core => ros2_medkit_serialization/include/ros2_medkit_serialization}/type_introspection.hpp (96%) rename src/{ros2_medkit_gateway => ros2_medkit_serialization}/src/type_introspection.cpp (88%) create mode 100644 src/ros2_medkit_serialization/test/test_type_introspection.cpp diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index 602448f8..01797487 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -203,7 +203,6 @@ add_library(gateway_ros2 STATIC src/ros2_common/ros2_subscription_slot.cpp src/script_manager.cpp src/trigger_fault_subscriber.cpp - src/type_introspection.cpp ) # Private implementation headers for openapi module (shared by both layers) 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 index c7e2cd6a..a2bf7722 100644 --- 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 @@ -20,12 +20,15 @@ #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 -class TypeIntrospection; // forward decl, defined in core/type_introspection.hpp /** * @brief Application service for topic publish + sample. @@ -95,7 +98,7 @@ class DataAccessManager { * implementation. May return nullptr for transports that do not support * introspection. */ - TypeIntrospection * get_type_introspection() const { + ros2_medkit_serialization::TypeIntrospection * get_type_introspection() const { return transport_ ? transport_->get_type_introspection() : nullptr; } 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 index 465b9b2f..16cfeb03 100644 --- 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 @@ -20,9 +20,11 @@ #include #include -namespace ros2_medkit_gateway { +namespace ros2_medkit_serialization { +class TypeIntrospection; // forward decl - defined in ros2_medkit_serialization/type_introspection.hpp. +} // namespace ros2_medkit_serialization -class TypeIntrospection; // forward decl - defined in core/type_introspection.hpp. +namespace ros2_medkit_gateway { using json = nlohmann::json; @@ -72,12 +74,12 @@ class TopicTransport { 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 is - /// rclcpp-coupled in the current implementation; the transport adapter owns - /// it so the manager body remains middleware-neutral. May return nullptr in - /// transports that do not support introspection (test mocks, alternative - /// middlewares). - virtual TypeIntrospection * get_type_introspection() const = 0; + /// 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/discovery/discovery_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/discovery/discovery_manager.hpp index 9a829f3f..e1c08194 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 @@ -35,11 +35,14 @@ #include #include +namespace ros2_medkit_serialization { +class TypeIntrospection; +} // namespace ros2_medkit_serialization + namespace ros2_medkit_gateway { // Forward declarations class TopicDataProvider; -class TypeIntrospection; class IntrospectionProvider; /** @@ -306,7 +309,7 @@ class DiscoveryManager : public ServiceActionResolver { * @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 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 index e05f0552..9a2a4eaf 100644 --- 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 @@ -21,7 +21,7 @@ #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_gateway/core/type_introspection.hpp" +#include "ros2_medkit_serialization/type_introspection.hpp" #include #include @@ -120,7 +120,7 @@ class Ros2RuntimeIntrospection : public IntrospectionProvider { /// Provide the type introspection used to enrich service/action schemas. /// Pointer must outlive this provider. - void set_type_introspection(TypeIntrospection * introspection); + 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. @@ -146,7 +146,7 @@ class Ros2RuntimeIntrospection : public IntrospectionProvider { rclcpp::Node * node_; RuntimeConfig config_; TopicDataProvider * topic_data_provider_{nullptr}; - TypeIntrospection * type_introspection_{nullptr}; + ros2_medkit_serialization::TypeIntrospection * type_introspection_{nullptr}; std::vector cached_services_; std::vector cached_actions_; 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 index d6fbe8c6..04b7dfe7 100644 --- 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 @@ -24,8 +24,8 @@ #include #include "ros2_medkit_gateway/core/transports/topic_transport.hpp" -#include "ros2_medkit_gateway/core/type_introspection.hpp" #include "ros2_medkit_serialization/json_serializer.hpp" +#include "ros2_medkit_serialization/type_introspection.hpp" namespace ros2_medkit_gateway { @@ -37,8 +37,9 @@ namespace ros2 { * @brief rclcpp adapter implementing the TopicTransport port. * * Owns the publisher cache, the JsonSerializer instance and the - * TypeIntrospection helper that the manager previously held directly. The - * sample path delegates to an attached TopicDataProvider; the adapter + * 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 { @@ -74,7 +75,7 @@ class Ros2TopicTransport : public TopicTransport { std::pair count_publishers_subscribers(const std::string & topic_name) const override; - TypeIntrospection * get_type_introspection() const override { + ros2_medkit_serialization::TypeIntrospection * get_type_introspection() const override { return type_introspection_.get(); } @@ -84,7 +85,7 @@ class Ros2TopicTransport : public TopicTransport { rclcpp::Node * node_; std::shared_ptr serializer_; - std::unique_ptr type_introspection_; + std::unique_ptr type_introspection_; mutable std::shared_mutex publishers_mutex_; std::unordered_map publishers_; diff --git a/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp b/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp index 754c694d..63de692c 100644 --- a/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp +++ b/src/ros2_medkit_gateway/src/discovery/discovery_manager.cpp @@ -394,7 +394,7 @@ void DiscoveryManager::set_topic_data_provider(TopicDataProvider * provider) { runtime_introspection_->set_topic_data_provider(provider); } -void DiscoveryManager::set_type_introspection(TypeIntrospection * introspection) { +void DiscoveryManager::set_type_introspection(ros2_medkit_serialization::TypeIntrospection * introspection) { runtime_introspection_->set_type_introspection(introspection); } 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 e7c739a0..fe7cc264 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/operation_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp index 34437889..7978ffcb 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/ros2/providers/ros2_runtime_introspection.cpp b/src/ros2_medkit_gateway/src/ros2/providers/ros2_runtime_introspection.cpp index adab2a2f..c36412f8 100644 --- a/src/ros2_medkit_gateway/src/ros2/providers/ros2_runtime_introspection.cpp +++ b/src/ros2_medkit_gateway/src/ros2/providers/ros2_runtime_introspection.cpp @@ -397,7 +397,7 @@ void Ros2RuntimeIntrospection::set_topic_data_provider(TopicDataProvider * provi topic_data_provider_ = provider; } -void Ros2RuntimeIntrospection::set_type_introspection(TypeIntrospection * introspection) { +void Ros2RuntimeIntrospection::set_type_introspection(ros2_medkit_serialization::TypeIntrospection * introspection) { type_introspection_ = introspection; } 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 index 456a44a5..3897aabe 100644 --- a/src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_transport.cpp +++ b/src/ros2_medkit_gateway/src/ros2/transports/ros2_topic_transport.cpp @@ -30,7 +30,7 @@ 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("")) + , 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(), @@ -155,7 +155,7 @@ TopicSample Ros2TopicTransport::sample(const std::string & topic_name, std::chro if (!res.message_type.empty()) { try { - TopicTypeInfo info = type_introspection_->get_type_info(res.message_type); + 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()); 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 9fbfcdda..7b3874ad 100644 --- a/src/ros2_medkit_gateway/test/test_data_access_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_data_access_manager.cpp @@ -22,7 +22,6 @@ #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" @@ -32,75 +31,6 @@ 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 // ============================================================================= 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 index 4bb805f0..aab3b7a1 100644 --- a/src/ros2_medkit_gateway/test/test_data_access_manager_routing.cpp +++ b/src/ros2_medkit_gateway/test/test_data_access_manager_routing.cpp @@ -63,7 +63,7 @@ class MockTopicTransport : public TopicTransport { return {sample_publishers_, 0}; } - TypeIntrospection * get_type_introspection() const override { + ros2_medkit_serialization::TypeIntrospection * get_type_introspection() const override { return nullptr; } diff --git a/src/ros2_medkit_serialization/CMakeLists.txt b/src/ros2_medkit_serialization/CMakeLists.txt index 3c2bd8b5..e8b0b8bc 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 0ae51c54..1c84feca 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 ec279599..5b85ded8 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 00000000..1913915c --- /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 From 22fc8f51b6f51364f7f0dbb44089aa5e181c7316 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 13:54:39 +0200 Subject: [PATCH 13/18] refactor(gateway): drive discovery refresh from rclcpp graph events Replace the cyclic wall-timer loop that called refresh_cache() every refresh_interval_ms with an event-driven design: - A 100 ms wall timer polls rclcpp::Node::get_graph_event()->check_and_clear() and runs refresh_cache() only when the ROS 2 graph actually changed (node up/down, topic/service/action add or remove). On a stable graph the callback is a single atomic check. - A second wall timer at refresh_interval_ms acts as a safety backstop, forcing an unconditional refresh so liveness is preserved if a graph event is ever missed. Semantics of the existing refresh_interval_ms parameter shift from "primary refresh interval" to "safety-backstop interval", and the default moves from 10 s (gateway_params.yaml) / 2 s (gateway.launch.py) to 30 s. The validation range stays 100-60000 ms; existing test overrides at 1000 ms continue to work, just as a tighter backstop. Test impact: - New integration test verifies that a node spawned mid-run appears in /apps within 5 s while the gateway runs with a 30 s backstop, proving the graph-event poll path is what drives the refresh. - test_operation_handlers seeds the entity cache directly and used to rely on refresh_cache() never firing during the 1 s settle period. Graph events from the test executor now do trigger refreshes; seed after the settle so the test's manually-injected component wins. Design intent on idle CPU: the previous timer woke and ran the full refresh_cache() pipeline every refresh_interval_ms regardless of whether the graph had changed. With this change an idle gateway runs only the 100 ms graph-event check (a single atomic load) plus the 30 s backstop. Documentation updated: README.md parameter tables, gateway_params.yaml comments, design/aggregation.rst. --- src/ros2_medkit_gateway/README.md | 4 +- .../config/gateway_params.yaml | 15 +- .../design/aggregation.rst | 9 +- .../ros2_medkit_gateway/gateway_node.hpp | 23 +- .../launch/gateway.launch.py | 5 +- .../launch/gateway_https.launch.py | 6 +- src/ros2_medkit_gateway/src/gateway_node.cpp | 51 +++- .../test/test_operation_handlers.cpp | 11 +- .../test_graph_event_discovery.test.py | 257 ++++++++++++++++++ 9 files changed, 358 insertions(+), 23 deletions(-) create mode 100644 src/ros2_medkit_integration_tests/test/features/test_graph_event_discovery.test.py diff --git a/src/ros2_medkit_gateway/README.md b/src/ros2_medkit_gateway/README.md index a90c2234..ff478cab 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 d7283dc1..515c6ddb 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 diff --git a/src/ros2_medkit_gateway/design/aggregation.rst b/src/ros2_medkit_gateway/design/aggregation.rst index 68151275..388a1229 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/include/ros2_medkit_gateway/gateway_node.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/gateway_node.hpp index 7eb382e0..a82984a6 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 @@ -248,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_; @@ -330,8 +334,23 @@ 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 diff --git a/src/ros2_medkit_gateway/launch/gateway.launch.py b/src/ros2_medkit_gateway/launch/gateway.launch.py index 41fcbdf8..50d39d3e 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 f90e6a6d..5d9051c9 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/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 786ee33a..72f578da 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{}); @@ -354,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) { @@ -1384,8 +1391,38 @@ 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(); + + // Sweep triggers whose entities disappeared from discovery + if (trigger_mgr_) { + trigger_mgr_->sweep_orphaned_triggers(); + } + }); + + // 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. + backstop_timer_ = create_wall_timer(std::chrono::milliseconds(refresh_interval_ms_), [this]() { refresh_cache(); // Sweep triggers whose entities disappeared from discovery diff --git a/src/ros2_medkit_gateway/test/test_operation_handlers.cpp b/src/ros2_medkit_gateway/test/test_operation_handlers.cpp index 95eb0cf8..178761a3 100644 --- a/src/ros2_medkit_gateway/test/test_operation_handlers.cpp +++ b/src/ros2_medkit_gateway/test/test_operation_handlers.cpp @@ -277,8 +277,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 +285,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_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 00000000..26a60850 --- /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 + ) From 2a06f9ef1f48abe67d9edde21b054b202e9c7b54 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 14:54:57 +0200 Subject: [PATCH 14/18] fix(gateway): only sweep orphan triggers on the backstop refresh Persistent triggers loaded at startup were being removed before DDS discovery had populated the entity cache. The graph-event refresh path fires on every node up/down event, including the cold-start window where the cache reflects only a partial view of the ROS 2 graph. Running the orphan sweep on every graph event treated restored triggers as orphans because their target entities had not yet been seen by this gateway, removing them from both memory and the persistent SQLite store. Move the sweep call to the backstop timer only. The backstop runs at the configured refresh cadence (default 30 s, 1 s in tests), giving DDS time to converge before any orphan check. The graph-event path keeps doing the cheap cache refresh, so spawn-detection latency is unchanged. --- src/ros2_medkit_gateway/src/gateway_node.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/ros2_medkit_gateway/src/gateway_node.cpp b/src/ros2_medkit_gateway/src/gateway_node.cpp index 72f578da..f189ef43 100644 --- a/src/ros2_medkit_gateway/src/gateway_node.cpp +++ b/src/ros2_medkit_gateway/src/gateway_node.cpp @@ -1411,17 +1411,22 @@ GatewayNode::GatewayNode(const rclcpp::NodeOptions & options) : Node("ros2_medki return; } refresh_cache(); - - // Sweep triggers whose entities disappeared from discovery - if (trigger_mgr_) { - trigger_mgr_->sweep_orphaned_triggers(); - } + // 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. + // 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(); From f49c978b405ec2a65938362c5a949caa5db1282d Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 14:55:06 +0200 Subject: [PATCH 15/18] fix(opcua): bump test_opcua_plugin TIMEOUT to 240s Each test in the suite connects to a non-existent OPC UA host and waits about 3.8 s for the DNS / TCP failure path. With 13 tests the wallclock run requires roughly 90 s. The ament_add_gtest default timeout of 60 s truncated the run mid-suite, which CTest then surfaced as a "missing_result" error under heavy parallelism (visible in concurrent colcon test invocations on multi-core hosts). Set TIMEOUT 240 to give a comfortable margin without hiding genuine hangs. --- src/ros2_medkit_plugins/ros2_medkit_opcua/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/ros2_medkit_plugins/ros2_medkit_opcua/CMakeLists.txt b/src/ros2_medkit_plugins/ros2_medkit_opcua/CMakeLists.txt index 92b4e2bf..e1dfa5a6 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 From 14338d4f06ba019b77b25a980489dc49ca6e4ca7 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 15:08:58 +0200 Subject: [PATCH 16/18] docs(gateway): refresh design index for managers-in-core layout The neutral-managers refactor moved DataAccessManager, OperationManager, ConfigurationManager, FaultManager, LogManager, and TriggerManager into ``gateway_core`` and gave each one a Transport interface for ROS interaction. Update the layered-library note to reflect that placement and the new neutral interfaces. TriggerTopicSubscriber became a generic per-handle subscription executor wrapped by Ros2TopicSubscriptionTransport. Update its class summary, component list, and the TriggerManager arrow on the class diagram so they describe the new per-trigger handle ownership rather than the old reference-counted shared-subscription model. --- src/ros2_medkit_gateway/design/index.rst | 35 +++++++++++++++--------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/src/ros2_medkit_gateway/design/index.rst b/src/ros2_medkit_gateway/design/index.rst index f2b0f3e9..2c0d5ae4 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 @@ -317,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 { @@ -420,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 @@ -606,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 ---------------------------- From 902bc7a8905107de48d2ba4c0e028d003c08ce50 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 15:27:42 +0200 Subject: [PATCH 17/18] fix(gateway): resolve clang-tidy findings on touched files Apply clang-tidy fixes across the files this branch already modifies so the incremental clang-tidy job (which scans every file changed in a PR) stays clean. - ``fault_handlers.cpp``: replace temp-allocating ``operator+`` chain in ``bulk_data_uri`` construction with explicit ``operator+=`` / ``std::move`` (performance-inefficient-string-concatenation). - ``test_configuration_manager.cpp``: pre-allocate ``threads.reserve(...)`` before ``emplace_back`` loops (performance-inefficient-vector-operation). - ``test_configuration_manager_routing.cpp``, ``test_data_access_manager_routing.cpp``, ``test_operation_manager_routing.cpp``, ``test_operation_handlers.cpp``: name unused parameters in mock and override signatures (readability-named-parameter, misc-unused-parameters). - ``test_fault_handlers.cpp``: switch single-character ``std::string::find("X")`` calls to ``find('X')`` (performance-faster-string-find). - ``test_operation_handlers.cpp``: replace ``std::bind(...)`` with forwarding lambdas (modernize-avoid-bind), and pass shared_ptr arguments by const-reference instead of by value (performance-unnecessary-value-param). - ``test_trigger_manager_routing.cpp``: use ``empty()`` instead of ``size()`` in a boolean context (readability-container-size-empty). After these changes ``clang-tidy -p build`` is clean on every cpp this branch touches. --- .../src/http/handlers/fault_handlers.cpp | 5 +++- .../test/test_configuration_manager.cpp | 3 ++ .../test_configuration_manager_routing.cpp | 2 +- .../test/test_data_access_manager_routing.cpp | 2 +- .../test/test_fault_handlers.cpp | 6 ++-- .../test/test_operation_handlers.cpp | 29 +++++++++++-------- .../test/test_operation_manager_routing.cpp | 6 ++-- .../test/test_trigger_manager_routing.cpp | 2 +- 8 files changed, 34 insertions(+), 21 deletions(-) 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 716dd6c8..a87cecc1 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/fault_handlers.cpp @@ -213,7 +213,10 @@ json FaultHandlers::build_sovd_fault_response(const json & fault_json, const jso // 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); - snap["bulk_data_uri"] = entity_path + "/bulk-data/rosbags/" + snap_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"]; } diff --git a/src/ros2_medkit_gateway/test/test_configuration_manager.cpp b/src/ros2_medkit_gateway/test/test_configuration_manager.cpp index 11599a95..4e387a36 100644 --- a/src/ros2_medkit_gateway/test/test_configuration_manager.cpp +++ b/src/ros2_medkit_gateway/test/test_configuration_manager.cpp @@ -351,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)); @@ -426,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) { @@ -465,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 index 80911f3f..43ba57ae 100644 --- a/src/ros2_medkit_gateway/test/test_configuration_manager_routing.cpp +++ b/src/ros2_medkit_gateway/test/test_configuration_manager_routing.cpp @@ -155,7 +155,7 @@ class MockParameterTransport : public ParameterTransport { return r; } - bool is_node_available(const std::string &) const override { + bool is_node_available(const std::string & /*node_name*/) const override { return true; } 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 index aab3b7a1..df5ef0e2 100644 --- a/src/ros2_medkit_gateway/test/test_data_access_manager_routing.cpp +++ b/src/ros2_medkit_gateway/test/test_data_access_manager_routing.cpp @@ -59,7 +59,7 @@ class MockTopicTransport : public TopicTransport { return r; } - std::pair count_publishers_subscribers(const std::string &) const override { + std::pair count_publishers_subscribers(const std::string & /*topic*/) const override { return {sample_publishers_, 0}; } diff --git a/src/ros2_medkit_gateway/test/test_fault_handlers.cpp b/src/ros2_medkit_gateway/test/test_fault_handlers.cpp index 677c3fd0..1b4b5639 100644 --- a/src/ros2_medkit_gateway/test/test_fault_handlers.cpp +++ b/src/ros2_medkit_gateway/test/test_fault_handlers.cpp @@ -267,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 diff --git a/src/ros2_medkit_gateway/test/test_operation_handlers.cpp b/src/ros2_medkit_gateway/test/test_operation_handlers.cpp index 178761a3..9afa57c4 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, 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"; }); diff --git a/src/ros2_medkit_gateway/test/test_operation_manager_routing.cpp b/src/ros2_medkit_gateway/test/test_operation_manager_routing.cpp index d2341a6a..b199c074 100644 --- a/src/ros2_medkit_gateway/test/test_operation_manager_routing.cpp +++ b/src/ros2_medkit_gateway/test/test_operation_manager_routing.cpp @@ -173,10 +173,12 @@ class MockActionTransport : public ActionTransport { class MockResolver : public ServiceActionResolver { public: MockResolver() = default; - std::optional find_service(const std::string &, const std::string &) const override { + 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 &, const std::string &) const override { + std::optional find_action(const std::string & /*entity_id*/, + const std::string & /*operation_id*/) const override { return action_; } diff --git a/src/ros2_medkit_gateway/test/test_trigger_manager_routing.cpp b/src/ros2_medkit_gateway/test/test_trigger_manager_routing.cpp index 4d3e6fd5..13af23cf 100644 --- a/src/ros2_medkit_gateway/test/test_trigger_manager_routing.cpp +++ b/src/ros2_medkit_gateway/test/test_trigger_manager_routing.cpp @@ -270,7 +270,7 @@ TEST_F(TriggerManagerRoutingTest, OrphanedTriggerSweepUnsubscribes) { manager_->sweep_orphaned_triggers(); EXPECT_EQ(transport_->alive_count(), 1u) << "Orphan sweep must unsubscribe gone_app's transport handle"; - EXPECT_FALSE(manager_->list("gone_app").size()); + EXPECT_TRUE(manager_->list("gone_app").empty()); EXPECT_EQ(manager_->list("alive_app").size(), 1u); } From 9bae856485255e2dcc69c40b96da451440586bbf Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Wed, 29 Apr 2026 15:41:46 +0200 Subject: [PATCH 18/18] fix(gateway): pass goal by const-reference in goal-callback lambda Follow-up to the earlier clang-tidy fixup. After clang-format wrapped the goal-callback lambda onto its own line, clang-tidy re-evaluated the parameter list and re-flagged ``goal`` as a copied ``std::shared_ptr`` only used as a const reference. Switch the parameter to ``const ...&`` to match the rest of the action lambdas in the file. --- src/ros2_medkit_gateway/test/test_operation_handlers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros2_medkit_gateway/test/test_operation_handlers.cpp b/src/ros2_medkit_gateway/test/test_operation_handlers.cpp index 9afa57c4..6b2072d1 100644 --- a/src/ros2_medkit_gateway/test/test_operation_handlers.cpp +++ b/src/ros2_medkit_gateway/test/test_operation_handlers.cpp @@ -110,7 +110,7 @@ class TestLongCalibrationActionServer : public rclcpp::Node { TestLongCalibrationActionServer() : rclcpp::Node("test_long_calibration_action", "/powertrain/engine") { action_server_ = rclcpp_action::create_server( this, "long_calibration", - [this](const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { + [this](const rclcpp_action::GoalUUID & uuid, const std::shared_ptr & goal) { return handle_goal(uuid, goal); }, [this](const std::shared_ptr & goal_handle) {