diff --git a/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake b/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake index e252d3c7b..0b548d9d2 100644 --- a/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake +++ b/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake @@ -19,6 +19,7 @@ # non-overlapping domain ID ranges to prevent DDS cross-contamination. # # Allocated ranges: +# ros2_medkit_sovd_service_iface: 1 - 9 (9 slots) # ros2_medkit_fault_manager: 10 - 29 (20 slots) # ros2_medkit_gateway: 30 - 89 (60 slots) # ros2_medkit_diagnostic_bridge: 90 - 99 (10 slots) @@ -27,7 +28,7 @@ # ros2_medkit_graph_provider: 120 - 129 (10 slots) # ros2_medkit_linux_introspection: 130 - 139 (10 slots) # ros2_medkit_integration_tests: 140 - 229 (90 slots) -# multi-domain tests (secondary): 230 - 232 (3 slots, via get_test_domain_id) +# multi-domain tests (secondary): 230 - 232 (3 slots, reserved for peer_aggregation etc.) # # To add a new package: pick the next free range and update this comment. diff --git a/src/ros2_medkit_msgs/CMakeLists.txt b/src/ros2_medkit_msgs/CMakeLists.txt index bd38f2d35..550c7b1aa 100644 --- a/src/ros2_medkit_msgs/CMakeLists.txt +++ b/src/ros2_medkit_msgs/CMakeLists.txt @@ -33,6 +33,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Snapshot.msg" "msg/EnvironmentData.msg" "msg/MedkitDiscoveryHint.msg" + "msg/EntityInfo.msg" "srv/ReportFault.srv" "srv/ListFaults.srv" "srv/GetFault.srv" @@ -41,6 +42,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/GetRosbag.srv" "srv/ListRosbags.srv" "srv/ListFaultsForEntity.srv" + "srv/ListEntities.srv" + "srv/GetEntityData.srv" + "srv/GetCapabilities.srv" DEPENDENCIES builtin_interfaces diagnostic_msgs ) diff --git a/src/ros2_medkit_msgs/msg/EntityInfo.msg b/src/ros2_medkit_msgs/msg/EntityInfo.msg new file mode 100644 index 000000000..182e13d9b --- /dev/null +++ b/src/ros2_medkit_msgs/msg/EntityInfo.msg @@ -0,0 +1,37 @@ +# Copyright 2026 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. +# +# EntityInfo.msg - SOVD entity descriptor for service interface responses. +# +# Provides basic metadata about a SOVD entity (app, component, area, function) +# for consumers that need entity tree information via ROS 2 services. + +# Unique entity identifier (e.g., "temp_sensor", "powertrain") +string id + +# Human-readable entity name +string name + +# Entity type: "app", "component", "area", "function" +string entity_type + +# Parent entity ID ("" for top-level entities) +string parent_id + +# Fully-qualified ROS 2 name (e.g., "/powertrain/engine/temp_sensor") +string fqn + +# SOVD capabilities available for this entity +# (e.g., ["data", "faults", "operations", "configurations"]) +string[] capabilities diff --git a/src/ros2_medkit_msgs/srv/GetCapabilities.srv b/src/ros2_medkit_msgs/srv/GetCapabilities.srv new file mode 100644 index 000000000..cec4c5407 --- /dev/null +++ b/src/ros2_medkit_msgs/srv/GetCapabilities.srv @@ -0,0 +1,34 @@ +# Copyright 2026 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. +# +# GetCapabilities.srv - Query SOVD capabilities for an entity or server. +# +# Returns the list of SOVD resource types available for the specified entity. +# Used by VDA 5050 agent to build factsheet protocolFeatures. + +# Request fields + +# Entity ID, or "" for server-level capabilities +string entity_id +--- +# Response fields + +bool success +string error_message + +# Available SOVD capabilities (e.g., "data", "faults", "operations", "configurations") +string[] capabilities + +# Available resource types (e.g., "apps", "components", "areas", "functions") +string[] resource_types diff --git a/src/ros2_medkit_msgs/srv/GetEntityData.srv b/src/ros2_medkit_msgs/srv/GetEntityData.srv new file mode 100644 index 000000000..f637e79a6 --- /dev/null +++ b/src/ros2_medkit_msgs/srv/GetEntityData.srv @@ -0,0 +1,34 @@ +# Copyright 2026 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. +# +# GetEntityData.srv - Retrieve data values for a SOVD entity. +# +# Returns latest topic data for the specified entity as a JSON string. +# Used by VDA 5050 agent to populate AGV state with sensor data. + +# Request fields + +# Entity ID to query +string entity_id + +# Specific data keys (topic names) to return, [] for all available +string[] data_keys +--- +# Response fields + +bool success +string error_message + +# JSON-encoded data: {"topic_name": {"field": value, ...}, ...} +string data_json diff --git a/src/ros2_medkit_msgs/srv/ListEntities.srv b/src/ros2_medkit_msgs/srv/ListEntities.srv new file mode 100644 index 000000000..0c6af19cc --- /dev/null +++ b/src/ros2_medkit_msgs/srv/ListEntities.srv @@ -0,0 +1,33 @@ +# Copyright 2026 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. +# +# ListEntities.srv - List SOVD entities from the gateway entity tree. +# +# Returns entities matching the requested type and parent filters. +# Used by VDA 5050 agent, BT.CPP, PlotJuggler, and other ROS 2 consumers +# to discover the SOVD entity tree without HTTP. + +# Request fields + +# Filter by entity type: "app", "component", "area", "function", "" (all) +string entity_type + +# Filter by parent entity ID, "" for no filter (returns all entities) +string parent_id +--- +# Response fields + +bool success +string error_message +EntityInfo[] entities diff --git a/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/CMakeLists.txt b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/CMakeLists.txt new file mode 100644 index 000000000..4c2f755aa --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/CMakeLists.txt @@ -0,0 +1,116 @@ +# Copyright 2026 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. + +cmake_minimum_required(VERSION 3.8) +project(ros2_medkit_sovd_service_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wshadow -Wconversion) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Shared cmake modules (multi-distro compat) +find_package(ros2_medkit_cmake REQUIRED) +include(ROS2MedkitCompat) + +find_package(ament_cmake REQUIRED) +find_package(ros2_medkit_gateway REQUIRED) +find_package(ros2_medkit_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nlohmann_json REQUIRED) +find_package(OpenSSL REQUIRED) + +# cpp-httplib via multi-distro compatibility macro +medkit_find_cpp_httplib() + +# Enable OpenSSL support for cpp-httplib +add_compile_definitions(CPPHTTPLIB_OPENSSL_SUPPORT) + +# MODULE target: loaded via dlopen at runtime by PluginManager. +# Symbols from gateway_lib are resolved from the host process at runtime. +add_library(sovd_service_interface MODULE + src/sovd_service_interface.cpp + src/service_exports.cpp +) + +target_include_directories(sovd_service_interface PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src +) + +medkit_target_dependencies(sovd_service_interface + ros2_medkit_msgs + ros2_medkit_gateway + rclcpp +) + +# Allow unresolved symbols - they resolve from the host process at runtime +target_link_options(sovd_service_interface PRIVATE + -Wl,--unresolved-symbols=ignore-all +) + +target_link_libraries(sovd_service_interface + nlohmann_json::nlohmann_json + cpp_httplib_target + OpenSSL::SSL OpenSSL::Crypto +) + +install(TARGETS sovd_service_interface + LIBRARY DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # uncrustify/cpplint conflict with project-wide clang-format (120 cols vs 100) + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_uncrustify + ament_cmake_cpplint + ament_cmake_clang_format + ) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_clang_format REQUIRED) + file(GLOB_RECURSE _format_files + "src/*.hpp" "src/*.cpp" "test/*.cpp" + ) + ament_clang_format(${_format_files} + CONFIG_FILE "${CMAKE_CURRENT_SOURCE_DIR}/../../../.clang-format") + + find_package(ament_cmake_gtest REQUIRED) + + include(ROS2MedkitTestDomain) + medkit_init_test_domains(START 1 END 9) + + ament_add_gtest(test_sovd_service_interface + test/test_sovd_service_interface.cpp + src/sovd_service_interface.cpp + ) + target_include_directories(test_sovd_service_interface PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ) + medkit_target_dependencies(test_sovd_service_interface + ros2_medkit_gateway + rclcpp + ros2_medkit_msgs + ) + target_link_libraries(test_sovd_service_interface + nlohmann_json::nlohmann_json + cpp_httplib_target + OpenSSL::SSL OpenSSL::Crypto + ) + medkit_set_test_domain(test_sovd_service_interface) +endif() + +ament_package() diff --git a/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/README.md b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/README.md new file mode 100644 index 000000000..c08ade622 --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/README.md @@ -0,0 +1,31 @@ +# ros2_medkit_sovd_service_interface + +Gateway plugin exposing medkit entity data via ROS 2 services. Enables ROS 2 nodes (e.g. VDA 5050 agent, BT.CPP, PlotJuggler) to access SOVD diagnostics without HTTP. + +## Services + +| Service | Type | Description | +|---------|------|-------------| +| `/medkit/list_entities` | `ros2_medkit_msgs/srv/ListEntities` | List all discovered entities (apps, components, areas) | +| `/medkit/list_entity_faults` | `ros2_medkit_msgs/srv/ListFaultsForEntity` | Get faults for a specific entity | +| `/medkit/get_entity_data` | `ros2_medkit_msgs/srv/GetEntityData` | Get entity topic data (not yet implemented) | +| `/medkit/get_capabilities` | `ros2_medkit_msgs/srv/GetCapabilities` | Get SOVD capabilities for an entity | + +Service prefix is configurable via `plugins.sovd_service_interface.service_prefix` parameter (default: `/medkit`). + +## Usage + +Load as a gateway plugin: + +```bash +ros2 run ros2_medkit_gateway gateway_node --ros-args \ + -p "plugins:=[\"sovd_service_interface\"]" \ + -p "plugins.sovd_service_interface.path:=/path/to/libsovd_service_interface.so" \ + -p "plugins.sovd_service_interface.service_prefix:=/medkit" +``` + +## Tests + +```bash +colcon test --packages-select ros2_medkit_sovd_service_interface +``` diff --git a/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/package.xml b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/package.xml new file mode 100644 index 000000000..86d12d3a8 --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/package.xml @@ -0,0 +1,29 @@ + + + + ros2_medkit_sovd_service_interface + 0.4.0 + SOVD Service Interface plugin - exposes medkit entity tree and fault data via ROS 2 services + bburda + mfaferek93 + Apache-2.0 + + ament_cmake + ros2_medkit_cmake + + rclcpp + ros2_medkit_msgs + ros2_medkit_gateway + nlohmann-json-dev + libcpp-httplib-dev + libssl-dev + + ament_lint_auto + ament_lint_common + ament_cmake_clang_format + ament_cmake_gtest + + + ament_cmake + + diff --git a/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/service_exports.cpp b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/service_exports.cpp new file mode 100644 index 000000000..d24386b06 --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/service_exports.cpp @@ -0,0 +1,26 @@ +// Copyright 2026 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/plugins/plugin_types.hpp" +#include "sovd_service_interface.hpp" + +using namespace ros2_medkit_gateway; + +extern "C" GATEWAY_PLUGIN_EXPORT int plugin_api_version() { + return PLUGIN_API_VERSION; // Must return 4 (exact match required) +} + +extern "C" GATEWAY_PLUGIN_EXPORT GatewayPlugin * create_plugin() { + return new SovdServiceInterface(); +} diff --git a/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.cpp b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.cpp new file mode 100644 index 000000000..ae9e4b283 --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.cpp @@ -0,0 +1,267 @@ +// Copyright 2026 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 "sovd_service_interface.hpp" + +#include + +#include "ros2_medkit_gateway/discovery/models/app.hpp" +#include "ros2_medkit_gateway/discovery/models/area.hpp" +#include "ros2_medkit_gateway/discovery/models/component.hpp" +#include "ros2_medkit_gateway/discovery/models/function.hpp" + +namespace ros2_medkit_gateway { + +namespace { + +SovdEntityType entity_type_from_string(const std::string & type) { + if (type == "app") { + return SovdEntityType::APP; + } + if (type == "component") { + return SovdEntityType::COMPONENT; + } + if (type == "area") { + return SovdEntityType::AREA; + } + if (type == "function") { + return SovdEntityType::FUNCTION; + } + return SovdEntityType::UNKNOWN; +} + +} // namespace + +std::string SovdServiceInterface::name() const { + return "sovd_service_interface"; +} + +void SovdServiceInterface::configure(const nlohmann::json & config) { + if (config.contains("service_prefix") && config["service_prefix"].is_string()) { + service_prefix_ = config["service_prefix"].get(); + } + log_info("Configured with service prefix: " + service_prefix_); +} + +void SovdServiceInterface::set_context(PluginContext & context) { + context_ = &context; + + auto * node = context.node(); + if (!node) { + log_error("No ROS 2 node available - cannot create service servers"); + return; + } + + list_entities_srv_ = node->create_service( + service_prefix_ + "/list_entities", + [this](const std::shared_ptr req, + std::shared_ptr res) { + handle_list_entities(req, res); + }); + + list_faults_srv_ = node->create_service( + service_prefix_ + "/list_entity_faults", + [this](const std::shared_ptr req, + std::shared_ptr res) { + handle_list_entity_faults(req, res); + }); + + get_data_srv_ = node->create_service( + service_prefix_ + "/get_entity_data", + [this](const std::shared_ptr req, + std::shared_ptr res) { + handle_get_entity_data(req, res); + }); + + get_capabilities_srv_ = node->create_service( + service_prefix_ + "/get_capabilities", + [this](const std::shared_ptr req, + std::shared_ptr res) { + handle_get_capabilities(req, res); + }); + + log_info("Service servers created: list_entities, list_entity_faults, get_entity_data, get_capabilities"); +} + +void SovdServiceInterface::shutdown() { + list_entities_srv_.reset(); + list_faults_srv_.reset(); + get_data_srv_.reset(); + get_capabilities_srv_.reset(); + log_info("Service servers shut down"); +} + +void SovdServiceInterface::handle_list_entities( + const std::shared_ptr request, + std::shared_ptr response) { + try { + auto snapshot = context_->get_entity_snapshot(); + const auto & type_filter = request->entity_type; + const auto & parent_filter = request->parent_id; + + auto add_entity = [&](const std::string & id, const std::string & entity_name, const std::string & entity_type, + const std::string & parent_id, const std::string & fqn) { + if (!type_filter.empty() && entity_type != type_filter) { + return; + } + if (!parent_filter.empty() && parent_id != parent_filter) { + return; + } + + ros2_medkit_msgs::msg::EntityInfo info; + info.id = id; + info.name = entity_name; + info.entity_type = entity_type; + info.parent_id = parent_id; + info.fqn = fqn; + info.capabilities = context_->get_entity_capabilities(id); + if (info.capabilities.empty()) { + info.capabilities = context_->get_type_capabilities(entity_type_from_string(entity_type)); + } + response->entities.push_back(std::move(info)); + }; + + for (const auto & area : snapshot.areas) { + add_entity(area.id, area.name, "area", area.parent_area_id, area.namespace_path); + } + for (const auto & comp : snapshot.components) { + add_entity(comp.id, comp.name, "component", comp.area, comp.fqn); + } + for (const auto & app : snapshot.apps) { + add_entity(app.id, app.name, "app", app.component_id, app.effective_fqn()); + } + for (const auto & func : snapshot.functions) { + // Functions are namespace groupings - they don't have a ROS 2 FQN + add_entity(func.id, func.name, "function", "", ""); + } + + response->success = true; + } catch (const std::exception & e) { + response->success = false; + response->error_message = std::string("Internal error: ") + e.what(); + log_error("handle_list_entities failed: " + std::string(e.what())); + } +} + +void SovdServiceInterface::handle_list_entity_faults( + const std::shared_ptr request, + std::shared_ptr response) { + try { + auto entity = context_->get_entity(request->entity_id); + if (!entity) { + response->success = false; + response->error_message = "Entity not found: " + request->entity_id; + return; + } + + auto faults_json = context_->list_entity_faults(request->entity_id); + + // Convert JSON faults to Fault messages using value() for type safety + if (faults_json.is_array()) { + for (const auto & fault_json : faults_json) { + if (!fault_json.is_object()) { + continue; + } + ros2_medkit_msgs::msg::Fault fault; + fault.fault_code = fault_json.value("fault_code", std::string{}); + fault.severity = fault_json.value("severity", static_cast(0)); + fault.description = fault_json.value("description", std::string{}); + fault.status = fault_json.value("status", std::string{}); + fault.occurrence_count = fault_json.value("occurrence_count", static_cast(0)); + if (fault_json.contains("first_occurred") && fault_json["first_occurred"].is_number()) { + double ts = fault_json["first_occurred"].get(); + fault.first_occurred.sec = static_cast(ts); + fault.first_occurred.nanosec = static_cast((ts - static_cast(ts)) * 1e9); + } + if (fault_json.contains("last_occurred") && fault_json["last_occurred"].is_number()) { + double ts = fault_json["last_occurred"].get(); + fault.last_occurred.sec = static_cast(ts); + fault.last_occurred.nanosec = static_cast((ts - static_cast(ts)) * 1e9); + } + if (fault_json.contains("reporting_sources") && fault_json["reporting_sources"].is_array()) { + for (const auto & src : fault_json["reporting_sources"]) { + if (src.is_string()) { + fault.reporting_sources.push_back(src.get()); + } + } + } + response->faults.push_back(std::move(fault)); + } + } + + response->success = true; + } catch (const std::exception & e) { + response->success = false; + response->error_message = std::string("Internal error: ") + e.what(); + log_error("handle_list_entity_faults failed: " + std::string(e.what())); + } +} + +void SovdServiceInterface::handle_get_entity_data( + const std::shared_ptr request, + std::shared_ptr response) { + try { + auto entity = context_->get_entity(request->entity_id); + if (!entity) { + response->success = false; + response->error_message = "Entity not found: " + request->entity_id; + return; + } + + // Stub: PluginContext does not expose live topic data yet. + // Tracked in https://github.com/selfpatch/ros2_medkit/issues/351 + response->data_json = "{}"; + response->success = false; + response->error_message = "GetEntityData not yet implemented - use HTTP REST API for topic data"; + } catch (const std::exception & e) { + response->success = false; + response->error_message = std::string("Internal error: ") + e.what(); + log_error("handle_get_entity_data failed: " + std::string(e.what())); + } +} + +void SovdServiceInterface::handle_get_capabilities( + const std::shared_ptr request, + std::shared_ptr response) { + try { + if (request->entity_id.empty()) { + // Server-level capabilities + response->capabilities = {"apps", "components", "areas", "functions", "faults", "health"}; + response->resource_types = {"apps", "components", "areas", "functions"}; + response->success = true; + return; + } + + auto entity = context_->get_entity(request->entity_id); + if (!entity) { + response->success = false; + response->error_message = "Entity not found: " + request->entity_id; + return; + } + + // Get entity-specific capabilities first, fall back to type-level + auto caps = context_->get_entity_capabilities(request->entity_id); + if (caps.empty()) { + caps = context_->get_type_capabilities(entity->type); + } + response->capabilities = std::move(caps); + response->success = true; + } catch (const std::exception & e) { + response->success = false; + response->error_message = std::string("Internal error: ") + e.what(); + log_error("handle_get_capabilities failed: " + std::string(e.what())); + } +} + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.hpp b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.hpp new file mode 100644 index 000000000..7a8f792ad --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.hpp @@ -0,0 +1,67 @@ +// Copyright 2026 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. + +#pragma once + +#include +#include + +#include + +#include "ros2_medkit_gateway/plugins/gateway_plugin.hpp" +#include "ros2_medkit_gateway/plugins/plugin_context.hpp" +#include "ros2_medkit_msgs/msg/entity_info.hpp" +#include "ros2_medkit_msgs/srv/get_capabilities.hpp" +#include "ros2_medkit_msgs/srv/get_entity_data.hpp" +#include "ros2_medkit_msgs/srv/list_entities.hpp" +#include "ros2_medkit_msgs/srv/list_faults_for_entity.hpp" + +namespace ros2_medkit_gateway { + +/// SOVD Service Interface plugin. +/// +/// Exposes medkit entity tree, fault data, and capabilities via ROS 2 +/// services. Designed for consumption by VDA 5050 agent, BT.CPP, +/// PlotJuggler, RTMaps, and any other ROS 2 node that needs SOVD data +/// without going through HTTP. +class SovdServiceInterface : public GatewayPlugin { + public: + std::string name() const override; + void configure(const nlohmann::json & config) override; + void set_context(PluginContext & context) override; + void shutdown() override; + + private: + void handle_list_entities(const std::shared_ptr request, + std::shared_ptr response); + + void handle_list_entity_faults(const std::shared_ptr request, + std::shared_ptr response); + + void handle_get_entity_data(const std::shared_ptr request, + std::shared_ptr response); + + void handle_get_capabilities(const std::shared_ptr request, + std::shared_ptr response); + + PluginContext * context_{nullptr}; + std::string service_prefix_{"/medkit"}; + + rclcpp::Service::SharedPtr list_entities_srv_; + rclcpp::Service::SharedPtr list_faults_srv_; + rclcpp::Service::SharedPtr get_data_srv_; + rclcpp::Service::SharedPtr get_capabilities_srv_; +}; + +} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/test/test_sovd_service_interface.cpp b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/test/test_sovd_service_interface.cpp new file mode 100644 index 000000000..a4433dad3 --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/test/test_sovd_service_interface.cpp @@ -0,0 +1,496 @@ +// Copyright 2026 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 +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ros2_medkit_gateway/discovery/models/app.hpp" +#include "ros2_medkit_gateway/discovery/models/area.hpp" +#include "ros2_medkit_gateway/discovery/models/component.hpp" +#include "ros2_medkit_gateway/discovery/models/function.hpp" +#include "ros2_medkit_gateway/plugins/plugin_context.hpp" +#include "sovd_service_interface.hpp" + +using namespace ros2_medkit_gateway; + +// Stubs for PluginContext static methods (defined in gateway_lib, not linked into tests) +namespace ros2_medkit_gateway { +void PluginContext::send_json(httplib::Response & res, const nlohmann::json & data) { + res.set_content(data.dump(), "application/json"); +} +void PluginContext::send_error(httplib::Response & res, int status, const std::string & /*error_code*/, + const std::string & message, const nlohmann::json & /*parameters*/) { + res.status = status; + nlohmann::json err = {{"error", message}}; + res.set_content(err.dump(), "application/json"); +} +} // namespace ros2_medkit_gateway + +namespace { + +class FakePluginContext : public PluginContext { + public: + explicit FakePluginContext(rclcpp::Node * node) : node_(node) { + } + + rclcpp::Node * node() const override { + return node_; + } + + std::optional get_entity(const std::string & id) const override { + auto it = entities_.find(id); + if (it == entities_.end()) { + return std::nullopt; + } + return it->second; + } + + nlohmann::json list_entity_faults(const std::string & entity_id) const override { + auto it = faults_.find(entity_id); + if (it == faults_.end()) { + return nlohmann::json::array(); + } + return it->second; + } + + std::optional validate_entity_for_route(const httplib::Request & /*req*/, httplib::Response & res, + const std::string & entity_id) const override { + auto entity = get_entity(entity_id); + if (!entity) { + send_error(res, 404, "entity-not-found", "Entity not found"); + return std::nullopt; + } + return entity; + } + + void register_capability(SovdEntityType entity_type, const std::string & capability_name) override { + type_capabilities_[entity_type].push_back(capability_name); + } + + void register_entity_capability(const std::string & entity_id, const std::string & capability_name) override { + entity_capabilities_[entity_id].push_back(capability_name); + } + + std::vector get_type_capabilities(SovdEntityType entity_type) const override { + auto it = type_capabilities_.find(entity_type); + if (it == type_capabilities_.end()) { + return {}; + } + return it->second; + } + + std::vector get_entity_capabilities(const std::string & entity_id) const override { + auto it = entity_capabilities_.find(entity_id); + if (it == entity_capabilities_.end()) { + return {}; + } + return it->second; + } + + std::vector get_child_apps(const std::string & /*component_id*/) const override { + return {}; + } + + LockAccessResult check_lock(const std::string & /*entity_id*/, const std::string & /*client_id*/, + const std::string & /*collection*/) const override { + return LockAccessResult{true, "", "", ""}; + } + + tl::expected acquire_lock(const std::string & /*entity_id*/, const std::string & /*client_id*/, + const std::vector & /*scopes*/, + int /*expiration_seconds*/) override { + return tl::make_unexpected(LockError{"lock-disabled", "Locking not available in test", 503, std::nullopt}); + } + + tl::expected release_lock(const std::string & /*entity_id*/, + const std::string & /*client_id*/) override { + return tl::make_unexpected(LockError{"lock-disabled", "Locking not available in test", 503, std::nullopt}); + } + + IntrospectionInput get_entity_snapshot() const override { + return snapshot_; + } + + nlohmann::json list_all_faults() const override { + return nlohmann::json::object(); + } + + void register_sampler( + const std::string & /*collection*/, + const std::function(const std::string &, const std::string &)> & + /*fn*/) override { + } + + ResourceChangeNotifier * get_resource_change_notifier() override { + return nullptr; + } + + ConditionRegistry * get_condition_registry() override { + return nullptr; + } + + // Test helpers + void add_entity(const std::string & id, SovdEntityType type, const std::string & fqn = "") { + entities_[id] = PluginEntityInfo{type, id, "", fqn}; + } + + void set_entity_faults(const std::string & entity_id, const nlohmann::json & faults) { + faults_[entity_id] = faults; + } + + IntrospectionInput snapshot_; + + private: + rclcpp::Node * node_{nullptr}; + std::unordered_map entities_; + std::unordered_map faults_; + std::map> type_capabilities_; + std::unordered_map> entity_capabilities_; +}; + +class RclcppEnvironment : public ::testing::Environment { + public: + void SetUp() override { + rclcpp::init(0, nullptr); + } + void TearDown() override { + rclcpp::shutdown(); + } +}; + +testing::Environment * const rclcpp_env = testing::AddGlobalTestEnvironment(new RclcppEnvironment); + +class SovdServiceInterfaceTest : public ::testing::Test { + protected: + void SetUp() override { + node_ = std::make_shared("test_sovd_service_interface"); + context_ = std::make_unique(node_.get()); + + // Set up test entity tree + Area area; + area.id = "powertrain"; + area.name = "Powertrain System"; + + Component comp; + comp.id = "engine"; + comp.name = "Engine ECU"; + comp.area = "powertrain"; + comp.fqn = "/powertrain/engine"; + + App app1; + app1.id = "temp_sensor"; + app1.name = "Temperature Sensor"; + app1.component_id = "engine"; + + App app2; + app2.id = "rpm_sensor"; + app2.name = "RPM Sensor"; + app2.component_id = "engine"; + + context_->snapshot_.areas = {area}; + context_->snapshot_.components = {comp}; + context_->snapshot_.apps = {app1, app2}; + + context_->add_entity("powertrain", SovdEntityType::AREA); + context_->add_entity("engine", SovdEntityType::COMPONENT, "/powertrain/engine"); + context_->add_entity("temp_sensor", SovdEntityType::APP); + context_->add_entity("rpm_sensor", SovdEntityType::APP); + + // Create and configure plugin + plugin_ = std::make_unique(); + nlohmann::json config = {{"service_prefix", "/test_medkit"}}; + plugin_->configure(config); + plugin_->set_context(*context_); + } + + void TearDown() override { + plugin_->shutdown(); + plugin_.reset(); + context_.reset(); + node_.reset(); + } + + std::shared_ptr node_; + std::unique_ptr context_; + std::unique_ptr plugin_; +}; + +TEST_F(SovdServiceInterfaceTest, ListAllEntities) { + auto client = node_->create_client("/test_medkit/list_entities"); + + auto request = std::make_shared(); + // Empty filters = return all + request->entity_type = ""; + request->parent_id = ""; + + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + EXPECT_EQ(response->entities.size(), 4u); // 1 area + 1 component + 2 apps +} + +TEST_F(SovdServiceInterfaceTest, ListEntitiesFilterByType) { + auto client = node_->create_client("/test_medkit/list_entities"); + + auto request = std::make_shared(); + request->entity_type = "app"; + request->parent_id = ""; + + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + EXPECT_EQ(response->entities.size(), 2u); // 2 apps + EXPECT_EQ(response->entities[0].entity_type, "app"); + EXPECT_EQ(response->entities[1].entity_type, "app"); +} + +TEST_F(SovdServiceInterfaceTest, ListEntitiesFilterByParent) { + auto client = node_->create_client("/test_medkit/list_entities"); + + auto request = std::make_shared(); + request->entity_type = ""; + request->parent_id = "engine"; + + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + EXPECT_EQ(response->entities.size(), 2u); // 2 apps under engine component +} + +TEST_F(SovdServiceInterfaceTest, ListEntitiesFilterByTypeAndParent) { + auto client = node_->create_client("/test_medkit/list_entities"); + + auto request = std::make_shared(); + request->entity_type = "app"; + request->parent_id = "engine"; + + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + EXPECT_EQ(response->entities.size(), 2u); // 2 apps under engine, both type "app" + for (const auto & e : response->entities) { + EXPECT_EQ(e.entity_type, "app"); + EXPECT_EQ(e.parent_id, "engine"); + } +} + +TEST_F(SovdServiceInterfaceTest, ListEntitiesFilterByTypeAndParentNoMatch) { + auto client = node_->create_client("/test_medkit/list_entities"); + + auto request = std::make_shared(); + request->entity_type = "component"; + request->parent_id = "engine"; // no components under engine + + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + EXPECT_TRUE(response->entities.empty()); +} + +TEST_F(SovdServiceInterfaceTest, ListEntitiesEmpty) { + // Clear snapshot + context_->snapshot_ = IntrospectionInput{}; + + auto client = node_->create_client("/test_medkit/list_entities"); + + auto request = std::make_shared(); + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + EXPECT_TRUE(response->entities.empty()); +} + +TEST_F(SovdServiceInterfaceTest, GetCapabilitiesServerLevel) { + auto client = node_->create_client("/test_medkit/get_capabilities"); + + auto request = std::make_shared(); + request->entity_id = ""; // Server-level + + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + EXPECT_FALSE(response->capabilities.empty()); + EXPECT_FALSE(response->resource_types.empty()); +} + +TEST_F(SovdServiceInterfaceTest, GetCapabilitiesEntityLevel) { + // Register entity-specific capability + context_->register_entity_capability("engine", "x-medkit-traces"); + context_->register_entity_capability("engine", "x-medkit-config"); + + auto client = node_->create_client("/test_medkit/get_capabilities"); + + auto request = std::make_shared(); + request->entity_id = "engine"; + + auto future = client->async_send_request(request); + auto result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + ASSERT_EQ(response->capabilities.size(), 2u); + EXPECT_EQ(response->capabilities[0], "x-medkit-traces"); + EXPECT_EQ(response->capabilities[1], "x-medkit-config"); +} + +TEST_F(SovdServiceInterfaceTest, GetCapabilitiesTypeFallback) { + // Register type-level capability (no entity-specific ones) + context_->register_capability(SovdEntityType::COMPONENT, "faults"); + context_->register_capability(SovdEntityType::COMPONENT, "data"); + + auto client = node_->create_client("/test_medkit/get_capabilities"); + + auto request = std::make_shared(); + request->entity_id = "engine"; + + auto future = client->async_send_request(request); + auto result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + ASSERT_EQ(response->capabilities.size(), 2u); + EXPECT_EQ(response->capabilities[0], "faults"); + EXPECT_EQ(response->capabilities[1], "data"); +} + +TEST_F(SovdServiceInterfaceTest, GetCapabilitiesEntityNotFound) { + auto client = node_->create_client("/test_medkit/get_capabilities"); + + auto request = std::make_shared(); + request->entity_id = "nonexistent"; + + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_FALSE(response->success); + EXPECT_FALSE(response->error_message.empty()); +} + +TEST_F(SovdServiceInterfaceTest, ListEntityFaults) { + // Set up test faults + nlohmann::json faults = nlohmann::json::array(); + faults.push_back({{"fault_code", "MOTOR_OVERHEAT"}, + {"severity", 2}, + {"description", "Motor temperature exceeds threshold"}, + {"status", "CONFIRMED"}, + {"occurrence_count", 3}, + {"reporting_sources", {"temp_sensor"}}}); + context_->set_entity_faults("temp_sensor", faults); + + auto client = node_->create_client("/test_medkit/list_entity_faults"); + + auto request = std::make_shared(); + request->entity_id = "temp_sensor"; + + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_TRUE(response->success); + ASSERT_EQ(response->faults.size(), 1u); + EXPECT_EQ(response->faults[0].fault_code, "MOTOR_OVERHEAT"); + EXPECT_EQ(response->faults[0].severity, 2u); + EXPECT_EQ(response->faults[0].status, "CONFIRMED"); + EXPECT_EQ(response->faults[0].occurrence_count, 3u); +} + +TEST_F(SovdServiceInterfaceTest, ListEntityFaultsNotFound) { + auto client = node_->create_client("/test_medkit/list_entity_faults"); + + auto request = std::make_shared(); + request->entity_id = "nonexistent"; + + auto future = client->async_send_request(request); + auto spin_result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(spin_result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_FALSE(response->success); + EXPECT_FALSE(response->error_message.empty()); +} + +TEST_F(SovdServiceInterfaceTest, GetEntityDataNotImplemented) { + auto client = node_->create_client("/test_medkit/get_entity_data"); + + auto request = std::make_shared(); + request->entity_id = "temp_sensor"; + + auto future = client->async_send_request(request); + auto result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + // GetEntityData is not yet implemented - returns explicit failure + ASSERT_FALSE(response->success); + EXPECT_FALSE(response->error_message.empty()); + // data_json should still be valid JSON (empty object) + auto parsed = nlohmann::json::parse(response->data_json, nullptr, false); + EXPECT_FALSE(parsed.is_discarded()); +} + +TEST_F(SovdServiceInterfaceTest, GetEntityDataEntityNotFound) { + auto client = node_->create_client("/test_medkit/get_entity_data"); + + auto request = std::make_shared(); + request->entity_id = "nonexistent"; + + auto future = client->async_send_request(request); + auto result = rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + ASSERT_EQ(result, rclcpp::FutureReturnCode::SUCCESS); + auto response = future.get(); + + ASSERT_FALSE(response->success); + EXPECT_FALSE(response->error_message.empty()); +} + +TEST_F(SovdServiceInterfaceTest, PluginName) { + EXPECT_EQ(plugin_->name(), "sovd_service_interface"); +} + +} // namespace