From 83c229b67a3d3d5932da5421ec7e9358eb4b4ef6 Mon Sep 17 00:00:00 2001 From: mfaferek93 Date: Tue, 31 Mar 2026 18:06:42 +0200 Subject: [PATCH 1/8] feat(vda5050): add VDA 5050 message definitions and SOVD Service Interface plugin Add vendor-neutral VDA 5050 v2.0.0 message definitions (44 msg files) and a gateway plugin exposing medkit entity/fault data via ROS 2 services. New packages: - ros2_medkit_vda5050_msgs: all VDA 5050 v2.0.0 types (Order, State, InstantActions, Connection, Visualization, Factsheet + sub-messages) - ros2_medkit_sovd_service_interface: gateway plugin creating 4 ROS 2 service servers (ListEntities, ListFaultsForEntity, GetEntityData, GetCapabilities) for consumption by VDA 5050 agent, BT.CPP, etc. New services in ros2_medkit_msgs: - ListEntities.srv, GetEntityData.srv, GetCapabilities.srv, EntityInfo.msg Closes #330 --- .../cmake/ROS2MedkitTestDomain.cmake | 2 +- src/ros2_medkit_msgs/CMakeLists.txt | 4 + src/ros2_medkit_msgs/msg/EntityInfo.msg | 37 ++ src/ros2_medkit_msgs/srv/GetCapabilities.srv | 34 ++ src/ros2_medkit_msgs/srv/GetEntityData.srv | 34 ++ src/ros2_medkit_msgs/srv/ListEntities.srv | 33 ++ .../CMakeLists.txt | 116 ++++++ .../package.xml | 29 ++ .../src/service_exports.cpp | 26 ++ .../src/sovd_service_interface.cpp | 250 ++++++++++++ .../src/sovd_service_interface.hpp | 71 ++++ .../test/test_sovd_service_interface.cpp | 384 ++++++++++++++++++ src/ros2_medkit_vda5050_msgs/CMakeLists.txt | 82 ++++ src/ros2_medkit_vda5050_msgs/msg/Action.msg | 34 ++ .../msg/ActionParameter.msg | 23 ++ .../msg/ActionState.msg | 33 ++ .../msg/AgvAction.msg | 26 ++ .../msg/AgvActionParameter.msg | 25 ++ .../msg/AgvGeometry.msg | 20 + .../msg/AgvPosition.msg | 28 ++ .../msg/BatteryState.msg | 25 ++ .../msg/BoundingBoxReference.msg | 24 ++ .../msg/Connection.msg | 24 ++ .../msg/ControlPoint.msg | 23 ++ src/ros2_medkit_vda5050_msgs/msg/Edge.msg | 37 ++ .../msg/EdgeState.msg | 25 ++ .../msg/Envelope2d.msg | 20 + .../msg/Envelope3d.msg | 20 + .../msg/ErrorEntry.msg | 28 ++ .../msg/ErrorReference.msg | 19 + .../msg/Factsheet.msg | 26 ++ src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg | 22 + .../msg/InfoEntry.msg | 28 ++ .../msg/InstantActions.msg | 19 + src/ros2_medkit_vda5050_msgs/msg/Load.msg | 23 ++ .../msg/LoadDimensions.msg | 23 ++ src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg | 29 ++ .../msg/LoadSpecification.msg | 19 + .../msg/MaxArrayLens.msg | 33 ++ .../msg/MaxStringLens.msg | 24 ++ src/ros2_medkit_vda5050_msgs/msg/Node.msg | 26 ++ .../msg/NodePosition.msg | 27 ++ .../msg/NodeState.msg | 25 ++ .../msg/OptionalParameter.msg | 27 ++ src/ros2_medkit_vda5050_msgs/msg/Order.msg | 27 ++ .../msg/PhysicalParameters.msg | 28 ++ .../msg/ProtocolFeatures.msg | 19 + .../msg/ProtocolLimits.msg | 20 + .../msg/SafetyState.msg | 26 ++ src/ros2_medkit_vda5050_msgs/msg/State.msg | 49 +++ src/ros2_medkit_vda5050_msgs/msg/Timing.msg | 24 ++ .../msg/Trajectory.msg | 25 ++ .../msg/TypeSpecification.msg | 27 ++ src/ros2_medkit_vda5050_msgs/msg/Velocity.msg | 20 + .../msg/Visualization.msg | 21 + .../msg/WheelDefinition.msg | 28 ++ .../msg/WheelPosition.msg | 20 + src/ros2_medkit_vda5050_msgs/package.xml | 30 ++ 58 files changed, 2250 insertions(+), 1 deletion(-) create mode 100644 src/ros2_medkit_msgs/msg/EntityInfo.msg create mode 100644 src/ros2_medkit_msgs/srv/GetCapabilities.srv create mode 100644 src/ros2_medkit_msgs/srv/GetEntityData.srv create mode 100644 src/ros2_medkit_msgs/srv/ListEntities.srv create mode 100644 src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/CMakeLists.txt create mode 100644 src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/package.xml create mode 100644 src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/service_exports.cpp create mode 100644 src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.cpp create mode 100644 src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.hpp create mode 100644 src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/test/test_sovd_service_interface.cpp create mode 100644 src/ros2_medkit_vda5050_msgs/CMakeLists.txt create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Action.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/ActionParameter.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/ActionState.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/AgvAction.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/AgvActionParameter.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/AgvGeometry.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/AgvPosition.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/BatteryState.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/BoundingBoxReference.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Connection.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/ControlPoint.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Edge.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/EdgeState.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Envelope2d.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Envelope3d.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/ErrorEntry.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/ErrorReference.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Factsheet.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/InfoEntry.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/InstantActions.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Load.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/LoadDimensions.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/LoadSpecification.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/MaxArrayLens.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/MaxStringLens.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Node.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/NodePosition.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/NodeState.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/OptionalParameter.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Order.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/PhysicalParameters.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/ProtocolFeatures.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/ProtocolLimits.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/SafetyState.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/State.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Timing.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Trajectory.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/TypeSpecification.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Velocity.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/Visualization.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/WheelDefinition.msg create mode 100644 src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg create mode 100644 src/ros2_medkit_vda5050_msgs/package.xml diff --git a/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake b/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake index e252d3c7b..bb2009dcd 100644 --- a/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake +++ b/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake @@ -27,7 +27,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) +# ros2_medkit_sovd_service_iface: 230 - 232 (3 slots, max domain ID is 232) # # 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..e6ec492d1 --- /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 top-level +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..2d705861f --- /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 230 END 232) + + 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/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..538df94d1 --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.cpp @@ -0,0 +1,250 @@ +// 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/area.hpp" +#include "ros2_medkit_gateway/discovery/models/app.hpp" +#include "ros2_medkit_gateway/discovery/models/component.hpp" +#include "ros2_medkit_gateway/discovery/models/function.hpp" + +namespace ros2_medkit_gateway { + +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 == "app" ? SovdEntityType::APP + : entity_type == "component" ? SovdEntityType::COMPONENT + : entity_type == "area" ? SovdEntityType::AREA + : entity_type == "function" ? SovdEntityType::FUNCTION + : SovdEntityType::UNKNOWN); + } + 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) { + add_entity(func.id, func.name, "function", "", func.id); + } + + 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 + if (faults_json.is_array()) { + for (const auto & fault_json : faults_json) { + ros2_medkit_msgs::msg::Fault fault; + if (fault_json.contains("fault_code")) { + fault.fault_code = fault_json["fault_code"].get(); + } + if (fault_json.contains("severity")) { + fault.severity = fault_json["severity"].get(); + } + if (fault_json.contains("description")) { + fault.description = fault_json["description"].get(); + } + if (fault_json.contains("status")) { + fault.status = fault_json["status"].get(); + } + if (fault_json.contains("occurrence_count")) { + fault.occurrence_count = fault_json["occurrence_count"].get(); + } + if (fault_json.contains("reporting_sources") && fault_json["reporting_sources"].is_array()) { + for (const auto & src : fault_json["reporting_sources"]) { + 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; + } + + // Use the registered data sampler if available + nlohmann::json data = nlohmann::json::object(); + + // Try to get data via the sampler registered for "data" collection + // The data sampler is registered by the gateway's data handler + // For now, return an empty JSON object - the agent can fall back to + // other data sources if needed + response->data_json = data.dump(); + response->success = true; + } 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..db28e9600 --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.hpp @@ -0,0 +1,71 @@ +// 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..3147f46b7 --- /dev/null +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/test/test_sovd_service_interface.cpp @@ -0,0 +1,384 @@ +// 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 SovdServiceInterfaceTest : public ::testing::Test { + protected: + void SetUp() override { + rclcpp::init(0, nullptr); + 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(); + rclcpp::shutdown(); + } + + 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); + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + auto response = future.get(); + + ASSERT_TRUE(response->success); + EXPECT_EQ(response->entities.size(), 2u); // 2 apps under engine component +} + +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); + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + auto response = future.get(); + + ASSERT_TRUE(response->success); + EXPECT_FALSE(response->capabilities.empty()); + EXPECT_FALSE(response->resource_types.empty()); +} + +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); + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + auto response = future.get(); + + ASSERT_FALSE(response->success); + EXPECT_FALSE(response->error_message.empty()); +} + +TEST_F(SovdServiceInterfaceTest, GetEntityData) { + 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); + rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + auto response = future.get(); + + ASSERT_TRUE(response->success); + // data_json should be valid JSON (even if empty object) + auto parsed = nlohmann::json::parse(response->data_json, nullptr, false); + EXPECT_FALSE(parsed.is_discarded()); +} + +TEST_F(SovdServiceInterfaceTest, PluginName) { + EXPECT_EQ(plugin_->name(), "sovd_service_interface"); +} + +} // namespace diff --git a/src/ros2_medkit_vda5050_msgs/CMakeLists.txt b/src/ros2_medkit_vda5050_msgs/CMakeLists.txt new file mode 100644 index 000000000..854eb2103 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/CMakeLists.txt @@ -0,0 +1,82 @@ +# 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_vda5050_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + # Shared sub-messages (leaf types first, then composites) + "msg/ActionParameter.msg" + "msg/ErrorReference.msg" + "msg/ControlPoint.msg" + "msg/BoundingBoxReference.msg" + "msg/LoadDimensions.msg" + "msg/Trajectory.msg" + "msg/NodePosition.msg" + "msg/Action.msg" + "msg/Node.msg" + "msg/Edge.msg" + "msg/ActionState.msg" + "msg/NodeState.msg" + "msg/EdgeState.msg" + "msg/AgvPosition.msg" + "msg/Velocity.msg" + "msg/BatteryState.msg" + "msg/SafetyState.msg" + "msg/ErrorEntry.msg" + "msg/InfoEntry.msg" + "msg/Load.msg" + "msg/HeaderId.msg" + # Factsheet sub-types + "msg/MaxStringLens.msg" + "msg/MaxArrayLens.msg" + "msg/Timing.msg" + "msg/ProtocolLimits.msg" + "msg/OptionalParameter.msg" + "msg/AgvActionParameter.msg" + "msg/AgvAction.msg" + "msg/ProtocolFeatures.msg" + "msg/TypeSpecification.msg" + "msg/PhysicalParameters.msg" + "msg/WheelPosition.msg" + "msg/WheelDefinition.msg" + "msg/Envelope2d.msg" + "msg/Envelope3d.msg" + "msg/AgvGeometry.msg" + "msg/LoadSet.msg" + "msg/LoadSpecification.msg" + # Top-level messages (1:1 with MQTT topics) + "msg/Order.msg" + "msg/InstantActions.msg" + "msg/State.msg" + "msg/Visualization.msg" + "msg/Connection.msg" + "msg/Factsheet.msg" + DEPENDENCIES builtin_interfaces +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/ros2_medkit_vda5050_msgs/msg/Action.msg b/src/ros2_medkit_vda5050_msgs/msg/Action.msg new file mode 100644 index 000000000..853476748 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Action.msg @@ -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. +# +# VDA 5050 v2.0.0 - Action +# Action to be executed at a node or on an edge, or as an instant action. +# +# Both order and instantActions schemas use "actionType" in v2.0.0. +# The JSON field is camelCase "actionType"; the ROS 2 field is snake_case "action_type". +# The agent's JSON serializer handles the case conversion. + +# Required +string action_id +string action_type +string blocking_type + +# Optional +string action_description +ActionParameter[] action_parameters + +# Blocking type constants +string BLOCKING_NONE=NONE +string BLOCKING_SOFT=SOFT +string BLOCKING_HARD=HARD diff --git a/src/ros2_medkit_vda5050_msgs/msg/ActionParameter.msg b/src/ros2_medkit_vda5050_msgs/msg/ActionParameter.msg new file mode 100644 index 000000000..53a19af95 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/ActionParameter.msg @@ -0,0 +1,23 @@ +# 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. +# +# VDA 5050 v2.0.0 - ActionParameter +# Key-value pair for action parameters. +# +# Note: VDA 5050 defines value as a union type (array|boolean|number|string). +# ROS 2 messages do not support union types, so value is serialized as a string. +# The consumer must parse the value based on context. + +string key +string value diff --git a/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg b/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg new file mode 100644 index 000000000..5374c057d --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg @@ -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. +# +# VDA 5050 v2.0.0 - ActionState +# Current state of an action reported in the AGV state message. + +# Required +string action_id +string action_status + +# Optional +string action_type +string action_description +string result_description + +# Action status constants +string STATUS_WAITING=WAITING +string STATUS_INITIALIZING=INITIALIZING +string STATUS_RUNNING=RUNNING +string STATUS_PAUSED=PAUSED +string STATUS_FINISHED=FINISHED +string STATUS_FAILED=FAILED diff --git a/src/ros2_medkit_vda5050_msgs/msg/AgvAction.msg b/src/ros2_medkit_vda5050_msgs/msg/AgvAction.msg new file mode 100644 index 000000000..132b31563 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/AgvAction.msg @@ -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. +# +# VDA 5050 v2.0.0 - AgvAction (Factsheet sub-type) +# Description of an action type supported by the AGV. + +# Required +string action_type +string[] action_scopes # INSTANT, NODE, EDGE +string[] blocking_types # NONE, SOFT, HARD + +# Optional +string action_description +string result_description +AgvActionParameter[] action_parameters diff --git a/src/ros2_medkit_vda5050_msgs/msg/AgvActionParameter.msg b/src/ros2_medkit_vda5050_msgs/msg/AgvActionParameter.msg new file mode 100644 index 000000000..2c6d14dd2 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/AgvActionParameter.msg @@ -0,0 +1,25 @@ +# 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. +# +# VDA 5050 v2.0.0 - AgvActionParameter (Factsheet sub-type) +# Parameter description for a factsheet agvAction. +# Different from ActionParameter (used in orders) which has key+value. + +# Required +string key +string value_data_type # BOOL, NUMBER, INTEGER, FLOAT, STRING, OBJECT, ARRAY + +# Optional +string description +bool is_optional diff --git a/src/ros2_medkit_vda5050_msgs/msg/AgvGeometry.msg b/src/ros2_medkit_vda5050_msgs/msg/AgvGeometry.msg new file mode 100644 index 000000000..741ac232c --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/AgvGeometry.msg @@ -0,0 +1,20 @@ +# 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. +# +# VDA 5050 v2.0.0 - AgvGeometry (Factsheet sub-type) +# Physical geometry of the AGV. All fields optional. + +WheelDefinition[] wheel_definitions +Envelope2d[] envelopes_2d +Envelope3d[] envelopes_3d diff --git a/src/ros2_medkit_vda5050_msgs/msg/AgvPosition.msg b/src/ros2_medkit_vda5050_msgs/msg/AgvPosition.msg new file mode 100644 index 000000000..b266c1390 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/AgvPosition.msg @@ -0,0 +1,28 @@ +# 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. +# +# VDA 5050 v2.0.0 - AgvPosition +# Current position of the AGV on the map. + +# Required +float64 x +float64 y +float64 theta +string map_id +bool position_initialized + +# Optional +string map_description +float64 localization_score +float64 deviation_range diff --git a/src/ros2_medkit_vda5050_msgs/msg/BatteryState.msg b/src/ros2_medkit_vda5050_msgs/msg/BatteryState.msg new file mode 100644 index 000000000..d743fd1f6 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/BatteryState.msg @@ -0,0 +1,25 @@ +# 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. +# +# VDA 5050 v2.0.0 - BatteryState +# Current battery state of the AGV. + +# Required +float64 battery_charge # State of charge in percent (0.0-100.0) +bool charging # Whether the AGV is currently charging + +# Optional +float64 battery_voltage +int32 battery_health # Health in percent (0-100) +float64 reach # Estimated reach in meters diff --git a/src/ros2_medkit_vda5050_msgs/msg/BoundingBoxReference.msg b/src/ros2_medkit_vda5050_msgs/msg/BoundingBoxReference.msg new file mode 100644 index 000000000..e07170fe3 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/BoundingBoxReference.msg @@ -0,0 +1,24 @@ +# 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. +# +# VDA 5050 v2.0.0 - BoundingBoxReference +# Point of reference for a bounding box, described in coordinates of the battery. + +# Required +float64 x +float64 y +float64 z + +# Optional +float64 theta diff --git a/src/ros2_medkit_vda5050_msgs/msg/Connection.msg b/src/ros2_medkit_vda5050_msgs/msg/Connection.msg new file mode 100644 index 000000000..3b42a02e2 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Connection.msg @@ -0,0 +1,24 @@ +# 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. +# +# VDA 5050 v2.0.0 - Connection (MQTT topic: connection) +# Connection state message. Published on connect and as MQTT LWT on disconnect. + +HeaderId header +string connection_state + +# Connection state constants +string STATE_ONLINE=ONLINE +string STATE_OFFLINE=OFFLINE +string STATE_CONNECTIONBROKEN=CONNECTIONBROKEN diff --git a/src/ros2_medkit_vda5050_msgs/msg/ControlPoint.msg b/src/ros2_medkit_vda5050_msgs/msg/ControlPoint.msg new file mode 100644 index 000000000..643154351 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/ControlPoint.msg @@ -0,0 +1,23 @@ +# 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. +# +# VDA 5050 v2.0.0 - ControlPoint +# NURBS control point for trajectory definition. + +# Required +float64 x +float64 y + +# Optional (default 1.0) +float64 weight 1.0 diff --git a/src/ros2_medkit_vda5050_msgs/msg/Edge.msg b/src/ros2_medkit_vda5050_msgs/msg/Edge.msg new file mode 100644 index 000000000..24e8a39a6 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Edge.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. +# +# VDA 5050 v2.0.0 - Edge +# An edge connecting two nodes in the order graph. + +# Required +string edge_id +uint32 sequence_id +bool released +string start_node_id +string end_node_id +Action[] actions + +# Optional +string edge_description +float64 max_speed +string orientation_type +float64 max_height +float64 min_height +float64 orientation +string direction +bool rotation_allowed +float64 max_rotation_speed +float64 length +Trajectory trajectory diff --git a/src/ros2_medkit_vda5050_msgs/msg/EdgeState.msg b/src/ros2_medkit_vda5050_msgs/msg/EdgeState.msg new file mode 100644 index 000000000..3b145e2db --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/EdgeState.msg @@ -0,0 +1,25 @@ +# 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. +# +# VDA 5050 v2.0.0 - EdgeState +# State of an edge as reported in the AGV state message. + +# Required +string edge_id +uint32 sequence_id +bool released + +# Optional +string edge_description +Trajectory trajectory diff --git a/src/ros2_medkit_vda5050_msgs/msg/Envelope2d.msg b/src/ros2_medkit_vda5050_msgs/msg/Envelope2d.msg new file mode 100644 index 000000000..455d13d06 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Envelope2d.msg @@ -0,0 +1,20 @@ +# 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. +# +# VDA 5050 v2.0.0 - Envelope2d (Factsheet sub-type) +# 2D polygon envelope of the AGV footprint. + +string set_id +string description +ControlPoint[] polygon_points diff --git a/src/ros2_medkit_vda5050_msgs/msg/Envelope3d.msg b/src/ros2_medkit_vda5050_msgs/msg/Envelope3d.msg new file mode 100644 index 000000000..eaedfd310 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Envelope3d.msg @@ -0,0 +1,20 @@ +# 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. +# +# VDA 5050 v2.0.0 - Envelope3d (Factsheet sub-type) +# 3D bounding envelope of the AGV. + +string set_id +string description +string data_format # JSON-encoded 3D data diff --git a/src/ros2_medkit_vda5050_msgs/msg/ErrorEntry.msg b/src/ros2_medkit_vda5050_msgs/msg/ErrorEntry.msg new file mode 100644 index 000000000..daff829a0 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/ErrorEntry.msg @@ -0,0 +1,28 @@ +# 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. +# +# VDA 5050 v2.0.0 - ErrorEntry +# An error reported in the AGV state message. + +# Required +string error_type +string error_level + +# Optional +string error_description +ErrorReference[] error_references + +# Error level constants (v2.0.0) +string LEVEL_WARNING=WARNING +string LEVEL_FATAL=FATAL diff --git a/src/ros2_medkit_vda5050_msgs/msg/ErrorReference.msg b/src/ros2_medkit_vda5050_msgs/msg/ErrorReference.msg new file mode 100644 index 000000000..20ca4e540 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/ErrorReference.msg @@ -0,0 +1,19 @@ +# 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. +# +# VDA 5050 v2.0.0 - ErrorReference / InfoReference +# Key-value pair referencing an entity related to an error or info entry. + +string reference_key +string reference_value diff --git a/src/ros2_medkit_vda5050_msgs/msg/Factsheet.msg b/src/ros2_medkit_vda5050_msgs/msg/Factsheet.msg new file mode 100644 index 000000000..b26f0f9b8 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Factsheet.msg @@ -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. +# +# VDA 5050 v2.0.0 - Factsheet (MQTT topic: factsheet) +# Self-description of the AGV, published on factsheetRequest instant action. + +HeaderId header + +# Required +TypeSpecification type_specification +PhysicalParameters physical_parameters +ProtocolLimits protocol_limits +ProtocolFeatures protocol_features +AgvGeometry agv_geometry +LoadSpecification load_specification diff --git a/src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg b/src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg new file mode 100644 index 000000000..4d035d3ef --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg @@ -0,0 +1,22 @@ +# 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. +# +# VDA 5050 v2.0.0 - HeaderId +# Common header fields shared across all VDA 5050 MQTT message types. + +uint32 header_id +string timestamp # ISO 8601 format +string version # VDA 5050 protocol version (e.g. "2.0.0") +string manufacturer +string serial_number diff --git a/src/ros2_medkit_vda5050_msgs/msg/InfoEntry.msg b/src/ros2_medkit_vda5050_msgs/msg/InfoEntry.msg new file mode 100644 index 000000000..b89c9089e --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/InfoEntry.msg @@ -0,0 +1,28 @@ +# 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. +# +# VDA 5050 v2.0.0 - InfoEntry +# An informational entry in the AGV state message. + +# Required +string info_type +string info_level + +# Optional +string info_description +ErrorReference[] info_references + +# Info level constants +string LEVEL_INFO=INFO +string LEVEL_DEBUG=DEBUG diff --git a/src/ros2_medkit_vda5050_msgs/msg/InstantActions.msg b/src/ros2_medkit_vda5050_msgs/msg/InstantActions.msg new file mode 100644 index 000000000..c129c949a --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/InstantActions.msg @@ -0,0 +1,19 @@ +# 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. +# +# VDA 5050 v2.0.0 - InstantActions (MQTT topic: instantActions) +# Instant actions to be executed immediately, independent of the current order. + +HeaderId header +Action[] instant_actions diff --git a/src/ros2_medkit_vda5050_msgs/msg/Load.msg b/src/ros2_medkit_vda5050_msgs/msg/Load.msg new file mode 100644 index 000000000..ea256f676 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Load.msg @@ -0,0 +1,23 @@ +# 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. +# +# VDA 5050 v2.0.0 - Load +# Description of a load currently carried by the AGV. All fields optional. + +string load_id +string load_type +string load_position +BoundingBoxReference bounding_box_reference +LoadDimensions load_dimensions +float64 weight diff --git a/src/ros2_medkit_vda5050_msgs/msg/LoadDimensions.msg b/src/ros2_medkit_vda5050_msgs/msg/LoadDimensions.msg new file mode 100644 index 000000000..f29d37b31 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/LoadDimensions.msg @@ -0,0 +1,23 @@ +# 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. +# +# VDA 5050 v2.0.0 - LoadDimensions +# Dimensions of a load in meters. + +# Required +float64 length +float64 width + +# Optional +float64 height diff --git a/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg b/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg new file mode 100644 index 000000000..d0128c948 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg @@ -0,0 +1,29 @@ +# 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. +# +# VDA 5050 v2.0.0 - LoadSet (Factsheet sub-type) +# Description of a type of load the AGV can handle. + +# Required +string set_name +string load_type + +# Optional (dimensions, weight, handling limits) +float64 max_weight +float64 min_length +float64 max_length +float64 min_width +float64 max_width +float64 min_height +float64 max_height diff --git a/src/ros2_medkit_vda5050_msgs/msg/LoadSpecification.msg b/src/ros2_medkit_vda5050_msgs/msg/LoadSpecification.msg new file mode 100644 index 000000000..07cf056dd --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/LoadSpecification.msg @@ -0,0 +1,19 @@ +# 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. +# +# VDA 5050 v2.0.0 - LoadSpecification (Factsheet sub-type) +# Load handling capabilities of the AGV. All fields optional. + +string[] load_positions +LoadSet[] load_sets diff --git a/src/ros2_medkit_vda5050_msgs/msg/MaxArrayLens.msg b/src/ros2_medkit_vda5050_msgs/msg/MaxArrayLens.msg new file mode 100644 index 000000000..14d34d8ee --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/MaxArrayLens.msg @@ -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. +# +# VDA 5050 v2.0.0 - MaxArrayLens (Factsheet sub-type) +# Maximum array lengths supported by this AGV. All optional. + +uint32 order_nodes +uint32 order_edges +uint32 node_actions +uint32 edge_actions +uint32 actions_action_parameters +uint32 instant_actions +uint32 trajectory_knot_vector +uint32 trajectory_control_points +uint32 state_node_states +uint32 state_edge_states +uint32 state_loads +uint32 state_action_states +uint32 state_errors +uint32 state_information +uint32 error_error_references +uint32 information_info_references diff --git a/src/ros2_medkit_vda5050_msgs/msg/MaxStringLens.msg b/src/ros2_medkit_vda5050_msgs/msg/MaxStringLens.msg new file mode 100644 index 000000000..f76572084 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/MaxStringLens.msg @@ -0,0 +1,24 @@ +# 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. +# +# VDA 5050 v2.0.0 - MaxStringLens (Factsheet sub-type) +# Maximum string lengths supported by this AGV. All optional. + +uint32 msg_len +uint32 topic_serial_len +uint32 topic_elem_len +uint32 id_len +bool id_numerical_only +uint32 enum_len +uint32 load_id_len diff --git a/src/ros2_medkit_vda5050_msgs/msg/Node.msg b/src/ros2_medkit_vda5050_msgs/msg/Node.msg new file mode 100644 index 000000000..9d8970014 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Node.msg @@ -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. +# +# VDA 5050 v2.0.0 - Node +# A node in the order graph. + +# Required +string node_id +uint32 sequence_id +bool released +Action[] actions + +# Optional +string node_description +NodePosition node_position diff --git a/src/ros2_medkit_vda5050_msgs/msg/NodePosition.msg b/src/ros2_medkit_vda5050_msgs/msg/NodePosition.msg new file mode 100644 index 000000000..280c5aefe --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/NodePosition.msg @@ -0,0 +1,27 @@ +# 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. +# +# VDA 5050 v2.0.0 - NodePosition +# Position of a node on the map. + +# Required +float64 x +float64 y +string map_id + +# Optional +float64 theta +float64 allowed_deviation_xy +float64 allowed_deviation_theta +string map_description diff --git a/src/ros2_medkit_vda5050_msgs/msg/NodeState.msg b/src/ros2_medkit_vda5050_msgs/msg/NodeState.msg new file mode 100644 index 000000000..2ff65c9ea --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/NodeState.msg @@ -0,0 +1,25 @@ +# 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. +# +# VDA 5050 v2.0.0 - NodeState +# State of a node as reported in the AGV state message. + +# Required +string node_id +uint32 sequence_id +bool released + +# Optional +string node_description +NodePosition node_position diff --git a/src/ros2_medkit_vda5050_msgs/msg/OptionalParameter.msg b/src/ros2_medkit_vda5050_msgs/msg/OptionalParameter.msg new file mode 100644 index 000000000..bc433dddf --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/OptionalParameter.msg @@ -0,0 +1,27 @@ +# 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. +# +# VDA 5050 v2.0.0 - OptionalParameter (Factsheet sub-type) +# Description of an optional parameter supported by the AGV. + +# Required +string parameter +string support # SUPPORTED or REQUIRED + +# Optional +string description + +# Support level constants +string SUPPORT_SUPPORTED=SUPPORTED +string SUPPORT_REQUIRED=REQUIRED diff --git a/src/ros2_medkit_vda5050_msgs/msg/Order.msg b/src/ros2_medkit_vda5050_msgs/msg/Order.msg new file mode 100644 index 000000000..907a2c5e8 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Order.msg @@ -0,0 +1,27 @@ +# 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. +# +# VDA 5050 v2.0.0 - Order (MQTT topic: order) +# Order message sent from master control to AGV. + +HeaderId header + +# Required +string order_id +uint32 order_update_id +Node[] nodes +Edge[] edges + +# Optional +string zone_set_id diff --git a/src/ros2_medkit_vda5050_msgs/msg/PhysicalParameters.msg b/src/ros2_medkit_vda5050_msgs/msg/PhysicalParameters.msg new file mode 100644 index 000000000..a24752ee7 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/PhysicalParameters.msg @@ -0,0 +1,28 @@ +# 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. +# +# VDA 5050 v2.0.0 - PhysicalParameters (Factsheet sub-type) +# Physical dimensions and speed limits of the AGV. + +# Required (all in SI units: m, m/s, m/s^2) +float64 speed_min +float64 speed_max +float64 acceleration_max +float64 deceleration_max +float64 height_max +float64 width +float64 length + +# Required per v2.0.0 schema (despite being logically optional) +float64 height_min diff --git a/src/ros2_medkit_vda5050_msgs/msg/ProtocolFeatures.msg b/src/ros2_medkit_vda5050_msgs/msg/ProtocolFeatures.msg new file mode 100644 index 000000000..c746c8ad3 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/ProtocolFeatures.msg @@ -0,0 +1,19 @@ +# 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. +# +# VDA 5050 v2.0.0 - ProtocolFeatures (Factsheet sub-type) +# Protocol features supported by this AGV. + +OptionalParameter[] optional_parameters +AgvAction[] agv_actions diff --git a/src/ros2_medkit_vda5050_msgs/msg/ProtocolLimits.msg b/src/ros2_medkit_vda5050_msgs/msg/ProtocolLimits.msg new file mode 100644 index 000000000..905693789 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/ProtocolLimits.msg @@ -0,0 +1,20 @@ +# 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. +# +# VDA 5050 v2.0.0 - ProtocolLimits (Factsheet sub-type) +# Protocol limits describing the capabilities of this AGV. + +MaxStringLens max_string_lens +MaxArrayLens max_array_lens +Timing timing diff --git a/src/ros2_medkit_vda5050_msgs/msg/SafetyState.msg b/src/ros2_medkit_vda5050_msgs/msg/SafetyState.msg new file mode 100644 index 000000000..f2dc06afe --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/SafetyState.msg @@ -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. +# +# VDA 5050 v2.0.0 - SafetyState +# Safety-related state of the AGV. + +# Required +string e_stop +bool field_violation + +# E-stop type constants +string ESTOP_AUTOACK=AUTOACK +string ESTOP_MANUAL=MANUAL +string ESTOP_REMOTE=REMOTE +string ESTOP_NONE=NONE diff --git a/src/ros2_medkit_vda5050_msgs/msg/State.msg b/src/ros2_medkit_vda5050_msgs/msg/State.msg new file mode 100644 index 000000000..36c4e5b35 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/State.msg @@ -0,0 +1,49 @@ +# 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. +# +# VDA 5050 v2.0.0 - State (MQTT topic: state) +# Full AGV state published periodically and on state changes. + +HeaderId header + +# Required +string order_id +uint32 order_update_id +string last_node_id +uint32 last_node_sequence_id +bool driving +NodeState[] node_states +EdgeState[] edge_states +ActionState[] action_states +BatteryState battery_state +string operating_mode +ErrorEntry[] errors +SafetyState safety_state + +# Optional +string zone_set_id +bool paused +bool new_base_request +float64 distance_since_last_node +AgvPosition agv_position +Velocity velocity +Load[] loads +InfoEntry[] information + +# Operating mode constants +string MODE_AUTOMATIC=AUTOMATIC +string MODE_SEMIAUTOMATIC=SEMIAUTOMATIC +string MODE_MANUAL=MANUAL +string MODE_SERVICE=SERVICE +string MODE_TEACHIN=TEACHIN diff --git a/src/ros2_medkit_vda5050_msgs/msg/Timing.msg b/src/ros2_medkit_vda5050_msgs/msg/Timing.msg new file mode 100644 index 000000000..04d1e898a --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Timing.msg @@ -0,0 +1,24 @@ +# 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. +# +# VDA 5050 v2.0.0 - Timing (Factsheet sub-type) +# Timing constraints for communication intervals. + +# Required +float64 min_order_interval # Minimum interval for order messages (seconds) +float64 min_state_interval # Minimum interval for state messages (seconds) + +# Optional +float64 default_state_interval +float64 visualization_interval diff --git a/src/ros2_medkit_vda5050_msgs/msg/Trajectory.msg b/src/ros2_medkit_vda5050_msgs/msg/Trajectory.msg new file mode 100644 index 000000000..43b942253 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Trajectory.msg @@ -0,0 +1,25 @@ +# 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. +# +# VDA 5050 v2.0.0 - Trajectory +# NURBS trajectory definition for edge traversal. + +# Required - degree of the NURBS curve +uint32 degree + +# Required - knot vector (values 0.0 to 1.0) +float64[] knot_vector + +# Required - control points +ControlPoint[] control_points diff --git a/src/ros2_medkit_vda5050_msgs/msg/TypeSpecification.msg b/src/ros2_medkit_vda5050_msgs/msg/TypeSpecification.msg new file mode 100644 index 000000000..abc221c31 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/TypeSpecification.msg @@ -0,0 +1,27 @@ +# 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. +# +# VDA 5050 v2.0.0 - TypeSpecification (Factsheet sub-type) +# Basic type information about the AGV. + +# Required +string series_name +string agv_kinematic # DIFF, OMNI, THREEWHEEL +string agv_class # FORKLIFT, CONVEYOR, TUGGER, CARRIER +float64 max_load_mass # kg +string[] localization_types # NATURAL, REFLECTOR, RFID, DMC, SPOT, GRID +string[] navigation_types # PHYSICAL_LINE_GUIDED, VIRTUAL_LINE_GUIDED, AUTONOMOUS + +# Optional +string series_description diff --git a/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg b/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg new file mode 100644 index 000000000..53b4b7b4e --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg @@ -0,0 +1,20 @@ +# 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. +# +# VDA 5050 v2.0.0 - Velocity +# AGV velocity in robot coordinates. All fields optional. + +float64 vx +float64 vy +float64 omega diff --git a/src/ros2_medkit_vda5050_msgs/msg/Visualization.msg b/src/ros2_medkit_vda5050_msgs/msg/Visualization.msg new file mode 100644 index 000000000..a3ec6ee8d --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/Visualization.msg @@ -0,0 +1,21 @@ +# 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. +# +# VDA 5050 v2.0.0 - Visualization (MQTT topic: visualization) +# Higher-frequency position data for fleet manager visualization. +# All fields optional per spec (for bandwidth), but header recommended. + +HeaderId header +AgvPosition agv_position +Velocity velocity diff --git a/src/ros2_medkit_vda5050_msgs/msg/WheelDefinition.msg b/src/ros2_medkit_vda5050_msgs/msg/WheelDefinition.msg new file mode 100644 index 000000000..5b9c8449d --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/WheelDefinition.msg @@ -0,0 +1,28 @@ +# 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. +# +# VDA 5050 v2.0.0 - WheelDefinition (Factsheet sub-type) +# Definition of a single wheel on the AGV. + +# Required +string type # DRIVE, CASTER, FIXED, MECANUM +bool is_active_driven +bool is_active_steered +WheelPosition position +float64 diameter # meters +float64 width # meters + +# Optional +float64 center_displacement # meters +string constraints diff --git a/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg b/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg new file mode 100644 index 000000000..14fd13aa4 --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg @@ -0,0 +1,20 @@ +# 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. +# +# VDA 5050 v2.0.0 - WheelPosition (Factsheet sub-type) +# Position of a wheel relative to the AGV origin. + +float64 x +float64 y +float64 theta diff --git a/src/ros2_medkit_vda5050_msgs/package.xml b/src/ros2_medkit_vda5050_msgs/package.xml new file mode 100644 index 000000000..f8222819a --- /dev/null +++ b/src/ros2_medkit_vda5050_msgs/package.xml @@ -0,0 +1,30 @@ + + + + ros2_medkit_vda5050_msgs + 0.4.0 + VDA 5050 v2.0.0 message definitions for ROS 2. Vendor-neutral types matching the official VDA/VDMA specification. + + bburda + mfaferek93 + Apache-2.0 + + https://github.com/selfpatch/ros2_medkit + https://github.com/VDA5050/VDA5050 + + ament_cmake + rosidl_default_generators + + builtin_interfaces + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + From 0392de92f75c2422610c97c098aea9b5fa314a80 Mon Sep 17 00:00:00 2001 From: mfaferek93 Date: Wed, 1 Apr 2026 19:11:18 +0200 Subject: [PATCH 2/8] style(sovd_service_interface): fix clang-format lint failures --- .../src/sovd_service_interface.cpp | 13 ++++++------ .../src/sovd_service_interface.hpp | 20 ++++++++----------- .../test/test_sovd_service_interface.cpp | 8 +++----- 3 files changed, 17 insertions(+), 24 deletions(-) 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 index 538df94d1..6a2aaf1b2 100644 --- 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 @@ -16,8 +16,8 @@ #include -#include "ros2_medkit_gateway/discovery/models/area.hpp" #include "ros2_medkit_gateway/discovery/models/app.hpp" +#include "ros2_medkit_gateway/discovery/models/area.hpp" #include "ros2_medkit_gateway/discovery/models/component.hpp" #include "ros2_medkit_gateway/discovery/models/function.hpp" @@ -107,12 +107,11 @@ void SovdServiceInterface::handle_list_entities( info.fqn = fqn; info.capabilities = context_->get_entity_capabilities(id); if (info.capabilities.empty()) { - info.capabilities = context_->get_type_capabilities( - entity_type == "app" ? SovdEntityType::APP - : entity_type == "component" ? SovdEntityType::COMPONENT - : entity_type == "area" ? SovdEntityType::AREA - : entity_type == "function" ? SovdEntityType::FUNCTION - : SovdEntityType::UNKNOWN); + info.capabilities = context_->get_type_capabilities(entity_type == "app" ? SovdEntityType::APP + : entity_type == "component" ? SovdEntityType::COMPONENT + : entity_type == "area" ? SovdEntityType::AREA + : entity_type == "function" ? SovdEntityType::FUNCTION + : SovdEntityType::UNKNOWN); } response->entities.push_back(std::move(info)); }; 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 index db28e9600..7a8f792ad 100644 --- 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 @@ -43,21 +43,17 @@ class SovdServiceInterface : public GatewayPlugin { void shutdown() override; private: - void handle_list_entities( - const std::shared_ptr request, - std::shared_ptr response); + 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_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_entity_data(const std::shared_ptr request, + std::shared_ptr response); - void handle_get_capabilities( - 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"}; 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 index 3147f46b7..8e67be1fe 100644 --- 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 @@ -138,7 +138,7 @@ class FakePluginContext : public PluginContext { void register_sampler( const std::string & /*collection*/, const std::function(const std::string &, const std::string &)> & - /*fn*/) override { + /*fn*/) override { } ResourceChangeNotifier * get_resource_change_notifier() override { @@ -328,8 +328,7 @@ TEST_F(SovdServiceInterfaceTest, ListEntityFaults) { {"reporting_sources", {"temp_sensor"}}}); context_->set_entity_faults("temp_sensor", faults); - auto client = - node_->create_client("/test_medkit/list_entity_faults"); + auto client = node_->create_client("/test_medkit/list_entity_faults"); auto request = std::make_shared(); request->entity_id = "temp_sensor"; @@ -347,8 +346,7 @@ TEST_F(SovdServiceInterfaceTest, ListEntityFaults) { } TEST_F(SovdServiceInterfaceTest, ListEntityFaultsNotFound) { - auto client = - node_->create_client("/test_medkit/list_entity_faults"); + auto client = node_->create_client("/test_medkit/list_entity_faults"); auto request = std::make_shared(); request->entity_id = "nonexistent"; From 7223118c5f09231bd46019ca91737f5ac78219e5 Mon Sep 17 00:00:00 2001 From: mfaferek93 Date: Sat, 4 Apr 2026 19:28:58 +0200 Subject: [PATCH 3/8] fix: address review findings for VDA 5050 service interface - GetEntityData: return explicit not-implemented error instead of silent empty JSON (B1) - ListEntities.srv: fix parent_id comment to match code behavior - empty string means no filter, not top-level (B2) - LoadSet.msg: add missing VDA 5050 v2.0.0 spec fields (bounding_box_reference, load_dimensions, load_positions, etc.) (M1) - ActionState.msg: document PAUSED as forward-compat extension from v2.1+, not in v2.0.0 (M2) - Add tests: entity-level GetCapabilities, type fallback, combined type+parent filter, GetEntityData not-found (M3) - Extract entity_type_from_string() helper replacing inline ternary chain (m4) - Use nlohmann::json value() with defaults for type-safe fault deserialization (m3) - Functions: use empty FQN instead of duplicating id (m1) - Use global rclcpp test environment instead of per-test init/shutdown (m5) - Check spin_until_future_complete return code in all tests (m8) - Add Required/Optional markers to Velocity.msg, WheelPosition.msg (m7) --- src/ros2_medkit_msgs/srv/ListEntities.srv | 2 +- .../src/sovd_service_interface.cpp | 65 ++++---- .../test/test_sovd_service_interface.cpp | 142 ++++++++++++++++-- .../msg/ActionState.msg | 1 + src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg | 24 ++- src/ros2_medkit_vda5050_msgs/msg/Velocity.msg | 3 +- .../msg/WheelPosition.msg | 3 + 7 files changed, 185 insertions(+), 55 deletions(-) diff --git a/src/ros2_medkit_msgs/srv/ListEntities.srv b/src/ros2_medkit_msgs/srv/ListEntities.srv index e6ec492d1..0c6af19cc 100644 --- a/src/ros2_medkit_msgs/srv/ListEntities.srv +++ b/src/ros2_medkit_msgs/srv/ListEntities.srv @@ -23,7 +23,7 @@ # Filter by entity type: "app", "component", "area", "function", "" (all) string entity_type -# Filter by parent entity ID, "" for top-level +# Filter by parent entity ID, "" for no filter (returns all entities) string parent_id --- # Response fields 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 index 6a2aaf1b2..5df340773 100644 --- 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 @@ -23,6 +23,18 @@ 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"; } @@ -107,11 +119,7 @@ void SovdServiceInterface::handle_list_entities( info.fqn = fqn; info.capabilities = context_->get_entity_capabilities(id); if (info.capabilities.empty()) { - info.capabilities = context_->get_type_capabilities(entity_type == "app" ? SovdEntityType::APP - : entity_type == "component" ? SovdEntityType::COMPONENT - : entity_type == "area" ? SovdEntityType::AREA - : entity_type == "function" ? SovdEntityType::FUNCTION - : SovdEntityType::UNKNOWN); + info.capabilities = context_->get_type_capabilities(entity_type_from_string(entity_type)); } response->entities.push_back(std::move(info)); }; @@ -126,7 +134,8 @@ void SovdServiceInterface::handle_list_entities( add_entity(app.id, app.name, "app", app.component_id, app.effective_fqn()); } for (const auto & func : snapshot.functions) { - add_entity(func.id, func.name, "function", "", func.id); + // Functions are namespace groupings - they don't have a ROS 2 FQN + add_entity(func.id, func.name, "function", "", ""); } response->success = true; @@ -150,28 +159,21 @@ void SovdServiceInterface::handle_list_entity_faults( auto faults_json = context_->list_entity_faults(request->entity_id); - // Convert JSON faults to Fault messages + // 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; - if (fault_json.contains("fault_code")) { - fault.fault_code = fault_json["fault_code"].get(); - } - if (fault_json.contains("severity")) { - fault.severity = fault_json["severity"].get(); - } - if (fault_json.contains("description")) { - fault.description = fault_json["description"].get(); - } - if (fault_json.contains("status")) { - fault.status = fault_json["status"].get(); - } - if (fault_json.contains("occurrence_count")) { - fault.occurrence_count = fault_json["occurrence_count"].get(); - } + 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("reporting_sources") && fault_json["reporting_sources"].is_array()) { for (const auto & src : fault_json["reporting_sources"]) { - fault.reporting_sources.push_back(src.get()); + if (src.is_string()) { + fault.reporting_sources.push_back(src.get()); + } } } response->faults.push_back(std::move(fault)); @@ -197,15 +199,14 @@ void SovdServiceInterface::handle_get_entity_data( return; } - // Use the registered data sampler if available - nlohmann::json data = nlohmann::json::object(); - - // Try to get data via the sampler registered for "data" collection - // The data sampler is registered by the gateway's data handler - // For now, return an empty JSON object - the agent can fall back to - // other data sources if needed - response->data_json = data.dump(); - response->success = true; + // TODO(#332): Implement data retrieval via PluginContext::get_entity_data() + // when the method is added. Currently PluginContext exposes entity metadata + // and faults but not live topic data. The VDA 5050 agent should use the + // HTTP REST API (/api/v1/apps/{id}/data) as the primary data source until + // this service is fully implemented. + 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(); 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 index 8e67be1fe..a4433dad3 100644 --- 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 @@ -168,10 +168,21 @@ class FakePluginContext : public PluginContext { 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 { - rclcpp::init(0, nullptr); node_ = std::make_shared("test_sovd_service_interface"); context_ = std::make_unique(node_.get()); @@ -217,7 +228,6 @@ class SovdServiceInterfaceTest : public ::testing::Test { plugin_.reset(); context_.reset(); node_.reset(); - rclcpp::shutdown(); } std::shared_ptr node_; @@ -234,7 +244,8 @@ TEST_F(SovdServiceInterfaceTest, ListAllEntities) { request->parent_id = ""; auto future = client->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); @@ -249,7 +260,8 @@ TEST_F(SovdServiceInterfaceTest, ListEntitiesFilterByType) { request->parent_id = ""; auto future = client->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); @@ -266,13 +278,50 @@ TEST_F(SovdServiceInterfaceTest, ListEntitiesFilterByParent) { request->parent_id = "engine"; auto future = client->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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{}; @@ -281,7 +330,8 @@ TEST_F(SovdServiceInterfaceTest, ListEntitiesEmpty) { auto request = std::make_shared(); auto future = client->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); @@ -295,7 +345,8 @@ TEST_F(SovdServiceInterfaceTest, GetCapabilitiesServerLevel) { request->entity_id = ""; // Server-level auto future = client->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); @@ -303,6 +354,48 @@ TEST_F(SovdServiceInterfaceTest, GetCapabilitiesServerLevel) { 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"); @@ -310,7 +403,8 @@ TEST_F(SovdServiceInterfaceTest, GetCapabilitiesEntityNotFound) { request->entity_id = "nonexistent"; auto future = client->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); @@ -334,7 +428,8 @@ TEST_F(SovdServiceInterfaceTest, ListEntityFaults) { request->entity_id = "temp_sensor"; auto future = client->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); @@ -352,29 +447,48 @@ TEST_F(SovdServiceInterfaceTest, ListEntityFaultsNotFound) { request->entity_id = "nonexistent"; auto future = client->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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, GetEntityData) { +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); - rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(5)); + 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); - // data_json should be valid JSON (even if empty object) + // 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"); } diff --git a/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg b/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg index 5374c057d..6316506f8 100644 --- a/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg +++ b/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg @@ -28,6 +28,7 @@ string result_description string STATUS_WAITING=WAITING string STATUS_INITIALIZING=INITIALIZING string STATUS_RUNNING=RUNNING +# Note: PAUSED is not in v2.0.0 - forward-compatible extension from v2.1+ string STATUS_PAUSED=PAUSED string STATUS_FINISHED=FINISHED string STATUS_FAILED=FAILED diff --git a/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg b/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg index d0128c948..539b32946 100644 --- a/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg +++ b/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg @@ -18,12 +18,22 @@ # Required string set_name string load_type +BoundingBoxReference bounding_box_reference +LoadDimensions load_dimensions -# Optional (dimensions, weight, handling limits) +# Optional +string[] load_positions float64 max_weight -float64 min_length -float64 max_length -float64 min_width -float64 max_width -float64 min_height -float64 max_height +bool rotation_allowed +string description +float64 min_loadhandling_height +float64 max_loadhandling_height +float64 min_loadhandling_depth +float64 max_loadhandling_depth +float64 min_loadhandling_tilt +float64 max_loadhandling_tilt +float64 agv_speed_limit +float64 agv_acceleration_limit +float64 agv_deceleration_limit +float64 pick_time +float64 drop_time diff --git a/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg b/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg index 53b4b7b4e..1ee239381 100644 --- a/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg +++ b/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg @@ -13,8 +13,9 @@ # limitations under the License. # # VDA 5050 v2.0.0 - Velocity -# AGV velocity in robot coordinates. All fields optional. +# AGV velocity in robot coordinates. +# Optional (all fields) float64 vx float64 vy float64 omega diff --git a/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg b/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg index 14fd13aa4..b7ba09525 100644 --- a/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg +++ b/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg @@ -15,6 +15,9 @@ # VDA 5050 v2.0.0 - WheelPosition (Factsheet sub-type) # Position of a wheel relative to the AGV origin. +# Required float64 x float64 y + +# Optional float64 theta From aceadf493bd7e840a101db2cacdf368cf8576d9b Mon Sep 17 00:00:00 2001 From: mfaferek93 Date: Sat, 4 Apr 2026 20:03:01 +0200 Subject: [PATCH 4/8] style: fix clang-format InsertBraces in entity_type_from_string --- .../src/sovd_service_interface.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) 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 index 5df340773..3a9a0c421 100644 --- 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 @@ -26,10 +26,18 @@ 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; + 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; } From a6a53dfe1956f579ffd353b97a98f3129d16c372 Mon Sep 17 00:00:00 2001 From: mfaferek93 Date: Sat, 4 Apr 2026 20:26:25 +0200 Subject: [PATCH 5/8] style: fix remaining InsertBraces format issue --- .../src/sovd_service_interface.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 index 3a9a0c421..3f8674fdb 100644 --- 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 @@ -170,7 +170,9 @@ void SovdServiceInterface::handle_list_entity_faults( // 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; + 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)); From 03c358bbe48af2c88f1e2ece0c5571156e3a1afa Mon Sep 17 00:00:00 2001 From: mfaferek93 Date: Sun, 5 Apr 2026 20:28:09 +0200 Subject: [PATCH 6/8] refactor: remove unused ros2_medkit_vda5050_msgs package VDA 5050 agent uses standalone C++ types with nlohmann/json serialization (vda5050_types.hpp) instead of ROS 2 message types. The msgs package was never imported by any package in the repo and had 6 interop-breaking schema errors identified in review. The SOVD Service Interface plugin (which stays) uses ros2_medkit_msgs for its service types, not VDA 5050 msgs. --- src/ros2_medkit_vda5050_msgs/CMakeLists.txt | 82 ------------------- src/ros2_medkit_vda5050_msgs/msg/Action.msg | 34 -------- .../msg/ActionParameter.msg | 23 ------ .../msg/ActionState.msg | 34 -------- .../msg/AgvAction.msg | 26 ------ .../msg/AgvActionParameter.msg | 25 ------ .../msg/AgvGeometry.msg | 20 ----- .../msg/AgvPosition.msg | 28 ------- .../msg/BatteryState.msg | 25 ------ .../msg/BoundingBoxReference.msg | 24 ------ .../msg/Connection.msg | 24 ------ .../msg/ControlPoint.msg | 23 ------ src/ros2_medkit_vda5050_msgs/msg/Edge.msg | 37 --------- .../msg/EdgeState.msg | 25 ------ .../msg/Envelope2d.msg | 20 ----- .../msg/Envelope3d.msg | 20 ----- .../msg/ErrorEntry.msg | 28 ------- .../msg/ErrorReference.msg | 19 ----- .../msg/Factsheet.msg | 26 ------ src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg | 22 ----- .../msg/InfoEntry.msg | 28 ------- .../msg/InstantActions.msg | 19 ----- src/ros2_medkit_vda5050_msgs/msg/Load.msg | 23 ------ .../msg/LoadDimensions.msg | 23 ------ src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg | 39 --------- .../msg/LoadSpecification.msg | 19 ----- .../msg/MaxArrayLens.msg | 33 -------- .../msg/MaxStringLens.msg | 24 ------ src/ros2_medkit_vda5050_msgs/msg/Node.msg | 26 ------ .../msg/NodePosition.msg | 27 ------ .../msg/NodeState.msg | 25 ------ .../msg/OptionalParameter.msg | 27 ------ src/ros2_medkit_vda5050_msgs/msg/Order.msg | 27 ------ .../msg/PhysicalParameters.msg | 28 ------- .../msg/ProtocolFeatures.msg | 19 ----- .../msg/ProtocolLimits.msg | 20 ----- .../msg/SafetyState.msg | 26 ------ src/ros2_medkit_vda5050_msgs/msg/State.msg | 49 ----------- src/ros2_medkit_vda5050_msgs/msg/Timing.msg | 24 ------ .../msg/Trajectory.msg | 25 ------ .../msg/TypeSpecification.msg | 27 ------ src/ros2_medkit_vda5050_msgs/msg/Velocity.msg | 21 ----- .../msg/Visualization.msg | 21 ----- .../msg/WheelDefinition.msg | 28 ------- .../msg/WheelPosition.msg | 23 ------ src/ros2_medkit_vda5050_msgs/package.xml | 30 ------- 46 files changed, 1246 deletions(-) delete mode 100644 src/ros2_medkit_vda5050_msgs/CMakeLists.txt delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Action.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/ActionParameter.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/ActionState.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/AgvAction.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/AgvActionParameter.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/AgvGeometry.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/AgvPosition.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/BatteryState.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/BoundingBoxReference.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Connection.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/ControlPoint.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Edge.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/EdgeState.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Envelope2d.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Envelope3d.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/ErrorEntry.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/ErrorReference.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Factsheet.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/InfoEntry.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/InstantActions.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Load.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/LoadDimensions.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/LoadSpecification.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/MaxArrayLens.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/MaxStringLens.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Node.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/NodePosition.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/NodeState.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/OptionalParameter.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Order.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/PhysicalParameters.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/ProtocolFeatures.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/ProtocolLimits.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/SafetyState.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/State.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Timing.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Trajectory.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/TypeSpecification.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Velocity.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/Visualization.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/WheelDefinition.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg delete mode 100644 src/ros2_medkit_vda5050_msgs/package.xml diff --git a/src/ros2_medkit_vda5050_msgs/CMakeLists.txt b/src/ros2_medkit_vda5050_msgs/CMakeLists.txt deleted file mode 100644 index 854eb2103..000000000 --- a/src/ros2_medkit_vda5050_msgs/CMakeLists.txt +++ /dev/null @@ -1,82 +0,0 @@ -# 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_vda5050_msgs) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(builtin_interfaces REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - # Shared sub-messages (leaf types first, then composites) - "msg/ActionParameter.msg" - "msg/ErrorReference.msg" - "msg/ControlPoint.msg" - "msg/BoundingBoxReference.msg" - "msg/LoadDimensions.msg" - "msg/Trajectory.msg" - "msg/NodePosition.msg" - "msg/Action.msg" - "msg/Node.msg" - "msg/Edge.msg" - "msg/ActionState.msg" - "msg/NodeState.msg" - "msg/EdgeState.msg" - "msg/AgvPosition.msg" - "msg/Velocity.msg" - "msg/BatteryState.msg" - "msg/SafetyState.msg" - "msg/ErrorEntry.msg" - "msg/InfoEntry.msg" - "msg/Load.msg" - "msg/HeaderId.msg" - # Factsheet sub-types - "msg/MaxStringLens.msg" - "msg/MaxArrayLens.msg" - "msg/Timing.msg" - "msg/ProtocolLimits.msg" - "msg/OptionalParameter.msg" - "msg/AgvActionParameter.msg" - "msg/AgvAction.msg" - "msg/ProtocolFeatures.msg" - "msg/TypeSpecification.msg" - "msg/PhysicalParameters.msg" - "msg/WheelPosition.msg" - "msg/WheelDefinition.msg" - "msg/Envelope2d.msg" - "msg/Envelope3d.msg" - "msg/AgvGeometry.msg" - "msg/LoadSet.msg" - "msg/LoadSpecification.msg" - # Top-level messages (1:1 with MQTT topics) - "msg/Order.msg" - "msg/InstantActions.msg" - "msg/State.msg" - "msg/Visualization.msg" - "msg/Connection.msg" - "msg/Factsheet.msg" - DEPENDENCIES builtin_interfaces -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/ros2_medkit_vda5050_msgs/msg/Action.msg b/src/ros2_medkit_vda5050_msgs/msg/Action.msg deleted file mode 100644 index 853476748..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Action.msg +++ /dev/null @@ -1,34 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Action -# Action to be executed at a node or on an edge, or as an instant action. -# -# Both order and instantActions schemas use "actionType" in v2.0.0. -# The JSON field is camelCase "actionType"; the ROS 2 field is snake_case "action_type". -# The agent's JSON serializer handles the case conversion. - -# Required -string action_id -string action_type -string blocking_type - -# Optional -string action_description -ActionParameter[] action_parameters - -# Blocking type constants -string BLOCKING_NONE=NONE -string BLOCKING_SOFT=SOFT -string BLOCKING_HARD=HARD diff --git a/src/ros2_medkit_vda5050_msgs/msg/ActionParameter.msg b/src/ros2_medkit_vda5050_msgs/msg/ActionParameter.msg deleted file mode 100644 index 53a19af95..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/ActionParameter.msg +++ /dev/null @@ -1,23 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - ActionParameter -# Key-value pair for action parameters. -# -# Note: VDA 5050 defines value as a union type (array|boolean|number|string). -# ROS 2 messages do not support union types, so value is serialized as a string. -# The consumer must parse the value based on context. - -string key -string value diff --git a/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg b/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg deleted file mode 100644 index 6316506f8..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/ActionState.msg +++ /dev/null @@ -1,34 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - ActionState -# Current state of an action reported in the AGV state message. - -# Required -string action_id -string action_status - -# Optional -string action_type -string action_description -string result_description - -# Action status constants -string STATUS_WAITING=WAITING -string STATUS_INITIALIZING=INITIALIZING -string STATUS_RUNNING=RUNNING -# Note: PAUSED is not in v2.0.0 - forward-compatible extension from v2.1+ -string STATUS_PAUSED=PAUSED -string STATUS_FINISHED=FINISHED -string STATUS_FAILED=FAILED diff --git a/src/ros2_medkit_vda5050_msgs/msg/AgvAction.msg b/src/ros2_medkit_vda5050_msgs/msg/AgvAction.msg deleted file mode 100644 index 132b31563..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/AgvAction.msg +++ /dev/null @@ -1,26 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - AgvAction (Factsheet sub-type) -# Description of an action type supported by the AGV. - -# Required -string action_type -string[] action_scopes # INSTANT, NODE, EDGE -string[] blocking_types # NONE, SOFT, HARD - -# Optional -string action_description -string result_description -AgvActionParameter[] action_parameters diff --git a/src/ros2_medkit_vda5050_msgs/msg/AgvActionParameter.msg b/src/ros2_medkit_vda5050_msgs/msg/AgvActionParameter.msg deleted file mode 100644 index 2c6d14dd2..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/AgvActionParameter.msg +++ /dev/null @@ -1,25 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - AgvActionParameter (Factsheet sub-type) -# Parameter description for a factsheet agvAction. -# Different from ActionParameter (used in orders) which has key+value. - -# Required -string key -string value_data_type # BOOL, NUMBER, INTEGER, FLOAT, STRING, OBJECT, ARRAY - -# Optional -string description -bool is_optional diff --git a/src/ros2_medkit_vda5050_msgs/msg/AgvGeometry.msg b/src/ros2_medkit_vda5050_msgs/msg/AgvGeometry.msg deleted file mode 100644 index 741ac232c..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/AgvGeometry.msg +++ /dev/null @@ -1,20 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - AgvGeometry (Factsheet sub-type) -# Physical geometry of the AGV. All fields optional. - -WheelDefinition[] wheel_definitions -Envelope2d[] envelopes_2d -Envelope3d[] envelopes_3d diff --git a/src/ros2_medkit_vda5050_msgs/msg/AgvPosition.msg b/src/ros2_medkit_vda5050_msgs/msg/AgvPosition.msg deleted file mode 100644 index b266c1390..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/AgvPosition.msg +++ /dev/null @@ -1,28 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - AgvPosition -# Current position of the AGV on the map. - -# Required -float64 x -float64 y -float64 theta -string map_id -bool position_initialized - -# Optional -string map_description -float64 localization_score -float64 deviation_range diff --git a/src/ros2_medkit_vda5050_msgs/msg/BatteryState.msg b/src/ros2_medkit_vda5050_msgs/msg/BatteryState.msg deleted file mode 100644 index d743fd1f6..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/BatteryState.msg +++ /dev/null @@ -1,25 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - BatteryState -# Current battery state of the AGV. - -# Required -float64 battery_charge # State of charge in percent (0.0-100.0) -bool charging # Whether the AGV is currently charging - -# Optional -float64 battery_voltage -int32 battery_health # Health in percent (0-100) -float64 reach # Estimated reach in meters diff --git a/src/ros2_medkit_vda5050_msgs/msg/BoundingBoxReference.msg b/src/ros2_medkit_vda5050_msgs/msg/BoundingBoxReference.msg deleted file mode 100644 index e07170fe3..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/BoundingBoxReference.msg +++ /dev/null @@ -1,24 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - BoundingBoxReference -# Point of reference for a bounding box, described in coordinates of the battery. - -# Required -float64 x -float64 y -float64 z - -# Optional -float64 theta diff --git a/src/ros2_medkit_vda5050_msgs/msg/Connection.msg b/src/ros2_medkit_vda5050_msgs/msg/Connection.msg deleted file mode 100644 index 3b42a02e2..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Connection.msg +++ /dev/null @@ -1,24 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Connection (MQTT topic: connection) -# Connection state message. Published on connect and as MQTT LWT on disconnect. - -HeaderId header -string connection_state - -# Connection state constants -string STATE_ONLINE=ONLINE -string STATE_OFFLINE=OFFLINE -string STATE_CONNECTIONBROKEN=CONNECTIONBROKEN diff --git a/src/ros2_medkit_vda5050_msgs/msg/ControlPoint.msg b/src/ros2_medkit_vda5050_msgs/msg/ControlPoint.msg deleted file mode 100644 index 643154351..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/ControlPoint.msg +++ /dev/null @@ -1,23 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - ControlPoint -# NURBS control point for trajectory definition. - -# Required -float64 x -float64 y - -# Optional (default 1.0) -float64 weight 1.0 diff --git a/src/ros2_medkit_vda5050_msgs/msg/Edge.msg b/src/ros2_medkit_vda5050_msgs/msg/Edge.msg deleted file mode 100644 index 24e8a39a6..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Edge.msg +++ /dev/null @@ -1,37 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Edge -# An edge connecting two nodes in the order graph. - -# Required -string edge_id -uint32 sequence_id -bool released -string start_node_id -string end_node_id -Action[] actions - -# Optional -string edge_description -float64 max_speed -string orientation_type -float64 max_height -float64 min_height -float64 orientation -string direction -bool rotation_allowed -float64 max_rotation_speed -float64 length -Trajectory trajectory diff --git a/src/ros2_medkit_vda5050_msgs/msg/EdgeState.msg b/src/ros2_medkit_vda5050_msgs/msg/EdgeState.msg deleted file mode 100644 index 3b145e2db..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/EdgeState.msg +++ /dev/null @@ -1,25 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - EdgeState -# State of an edge as reported in the AGV state message. - -# Required -string edge_id -uint32 sequence_id -bool released - -# Optional -string edge_description -Trajectory trajectory diff --git a/src/ros2_medkit_vda5050_msgs/msg/Envelope2d.msg b/src/ros2_medkit_vda5050_msgs/msg/Envelope2d.msg deleted file mode 100644 index 455d13d06..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Envelope2d.msg +++ /dev/null @@ -1,20 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Envelope2d (Factsheet sub-type) -# 2D polygon envelope of the AGV footprint. - -string set_id -string description -ControlPoint[] polygon_points diff --git a/src/ros2_medkit_vda5050_msgs/msg/Envelope3d.msg b/src/ros2_medkit_vda5050_msgs/msg/Envelope3d.msg deleted file mode 100644 index eaedfd310..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Envelope3d.msg +++ /dev/null @@ -1,20 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Envelope3d (Factsheet sub-type) -# 3D bounding envelope of the AGV. - -string set_id -string description -string data_format # JSON-encoded 3D data diff --git a/src/ros2_medkit_vda5050_msgs/msg/ErrorEntry.msg b/src/ros2_medkit_vda5050_msgs/msg/ErrorEntry.msg deleted file mode 100644 index daff829a0..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/ErrorEntry.msg +++ /dev/null @@ -1,28 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - ErrorEntry -# An error reported in the AGV state message. - -# Required -string error_type -string error_level - -# Optional -string error_description -ErrorReference[] error_references - -# Error level constants (v2.0.0) -string LEVEL_WARNING=WARNING -string LEVEL_FATAL=FATAL diff --git a/src/ros2_medkit_vda5050_msgs/msg/ErrorReference.msg b/src/ros2_medkit_vda5050_msgs/msg/ErrorReference.msg deleted file mode 100644 index 20ca4e540..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/ErrorReference.msg +++ /dev/null @@ -1,19 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - ErrorReference / InfoReference -# Key-value pair referencing an entity related to an error or info entry. - -string reference_key -string reference_value diff --git a/src/ros2_medkit_vda5050_msgs/msg/Factsheet.msg b/src/ros2_medkit_vda5050_msgs/msg/Factsheet.msg deleted file mode 100644 index b26f0f9b8..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Factsheet.msg +++ /dev/null @@ -1,26 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Factsheet (MQTT topic: factsheet) -# Self-description of the AGV, published on factsheetRequest instant action. - -HeaderId header - -# Required -TypeSpecification type_specification -PhysicalParameters physical_parameters -ProtocolLimits protocol_limits -ProtocolFeatures protocol_features -AgvGeometry agv_geometry -LoadSpecification load_specification diff --git a/src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg b/src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg deleted file mode 100644 index 4d035d3ef..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/HeaderId.msg +++ /dev/null @@ -1,22 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - HeaderId -# Common header fields shared across all VDA 5050 MQTT message types. - -uint32 header_id -string timestamp # ISO 8601 format -string version # VDA 5050 protocol version (e.g. "2.0.0") -string manufacturer -string serial_number diff --git a/src/ros2_medkit_vda5050_msgs/msg/InfoEntry.msg b/src/ros2_medkit_vda5050_msgs/msg/InfoEntry.msg deleted file mode 100644 index b89c9089e..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/InfoEntry.msg +++ /dev/null @@ -1,28 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - InfoEntry -# An informational entry in the AGV state message. - -# Required -string info_type -string info_level - -# Optional -string info_description -ErrorReference[] info_references - -# Info level constants -string LEVEL_INFO=INFO -string LEVEL_DEBUG=DEBUG diff --git a/src/ros2_medkit_vda5050_msgs/msg/InstantActions.msg b/src/ros2_medkit_vda5050_msgs/msg/InstantActions.msg deleted file mode 100644 index c129c949a..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/InstantActions.msg +++ /dev/null @@ -1,19 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - InstantActions (MQTT topic: instantActions) -# Instant actions to be executed immediately, independent of the current order. - -HeaderId header -Action[] instant_actions diff --git a/src/ros2_medkit_vda5050_msgs/msg/Load.msg b/src/ros2_medkit_vda5050_msgs/msg/Load.msg deleted file mode 100644 index ea256f676..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Load.msg +++ /dev/null @@ -1,23 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Load -# Description of a load currently carried by the AGV. All fields optional. - -string load_id -string load_type -string load_position -BoundingBoxReference bounding_box_reference -LoadDimensions load_dimensions -float64 weight diff --git a/src/ros2_medkit_vda5050_msgs/msg/LoadDimensions.msg b/src/ros2_medkit_vda5050_msgs/msg/LoadDimensions.msg deleted file mode 100644 index f29d37b31..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/LoadDimensions.msg +++ /dev/null @@ -1,23 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - LoadDimensions -# Dimensions of a load in meters. - -# Required -float64 length -float64 width - -# Optional -float64 height diff --git a/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg b/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg deleted file mode 100644 index 539b32946..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/LoadSet.msg +++ /dev/null @@ -1,39 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - LoadSet (Factsheet sub-type) -# Description of a type of load the AGV can handle. - -# Required -string set_name -string load_type -BoundingBoxReference bounding_box_reference -LoadDimensions load_dimensions - -# Optional -string[] load_positions -float64 max_weight -bool rotation_allowed -string description -float64 min_loadhandling_height -float64 max_loadhandling_height -float64 min_loadhandling_depth -float64 max_loadhandling_depth -float64 min_loadhandling_tilt -float64 max_loadhandling_tilt -float64 agv_speed_limit -float64 agv_acceleration_limit -float64 agv_deceleration_limit -float64 pick_time -float64 drop_time diff --git a/src/ros2_medkit_vda5050_msgs/msg/LoadSpecification.msg b/src/ros2_medkit_vda5050_msgs/msg/LoadSpecification.msg deleted file mode 100644 index 07cf056dd..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/LoadSpecification.msg +++ /dev/null @@ -1,19 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - LoadSpecification (Factsheet sub-type) -# Load handling capabilities of the AGV. All fields optional. - -string[] load_positions -LoadSet[] load_sets diff --git a/src/ros2_medkit_vda5050_msgs/msg/MaxArrayLens.msg b/src/ros2_medkit_vda5050_msgs/msg/MaxArrayLens.msg deleted file mode 100644 index 14d34d8ee..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/MaxArrayLens.msg +++ /dev/null @@ -1,33 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - MaxArrayLens (Factsheet sub-type) -# Maximum array lengths supported by this AGV. All optional. - -uint32 order_nodes -uint32 order_edges -uint32 node_actions -uint32 edge_actions -uint32 actions_action_parameters -uint32 instant_actions -uint32 trajectory_knot_vector -uint32 trajectory_control_points -uint32 state_node_states -uint32 state_edge_states -uint32 state_loads -uint32 state_action_states -uint32 state_errors -uint32 state_information -uint32 error_error_references -uint32 information_info_references diff --git a/src/ros2_medkit_vda5050_msgs/msg/MaxStringLens.msg b/src/ros2_medkit_vda5050_msgs/msg/MaxStringLens.msg deleted file mode 100644 index f76572084..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/MaxStringLens.msg +++ /dev/null @@ -1,24 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - MaxStringLens (Factsheet sub-type) -# Maximum string lengths supported by this AGV. All optional. - -uint32 msg_len -uint32 topic_serial_len -uint32 topic_elem_len -uint32 id_len -bool id_numerical_only -uint32 enum_len -uint32 load_id_len diff --git a/src/ros2_medkit_vda5050_msgs/msg/Node.msg b/src/ros2_medkit_vda5050_msgs/msg/Node.msg deleted file mode 100644 index 9d8970014..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Node.msg +++ /dev/null @@ -1,26 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Node -# A node in the order graph. - -# Required -string node_id -uint32 sequence_id -bool released -Action[] actions - -# Optional -string node_description -NodePosition node_position diff --git a/src/ros2_medkit_vda5050_msgs/msg/NodePosition.msg b/src/ros2_medkit_vda5050_msgs/msg/NodePosition.msg deleted file mode 100644 index 280c5aefe..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/NodePosition.msg +++ /dev/null @@ -1,27 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - NodePosition -# Position of a node on the map. - -# Required -float64 x -float64 y -string map_id - -# Optional -float64 theta -float64 allowed_deviation_xy -float64 allowed_deviation_theta -string map_description diff --git a/src/ros2_medkit_vda5050_msgs/msg/NodeState.msg b/src/ros2_medkit_vda5050_msgs/msg/NodeState.msg deleted file mode 100644 index 2ff65c9ea..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/NodeState.msg +++ /dev/null @@ -1,25 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - NodeState -# State of a node as reported in the AGV state message. - -# Required -string node_id -uint32 sequence_id -bool released - -# Optional -string node_description -NodePosition node_position diff --git a/src/ros2_medkit_vda5050_msgs/msg/OptionalParameter.msg b/src/ros2_medkit_vda5050_msgs/msg/OptionalParameter.msg deleted file mode 100644 index bc433dddf..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/OptionalParameter.msg +++ /dev/null @@ -1,27 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - OptionalParameter (Factsheet sub-type) -# Description of an optional parameter supported by the AGV. - -# Required -string parameter -string support # SUPPORTED or REQUIRED - -# Optional -string description - -# Support level constants -string SUPPORT_SUPPORTED=SUPPORTED -string SUPPORT_REQUIRED=REQUIRED diff --git a/src/ros2_medkit_vda5050_msgs/msg/Order.msg b/src/ros2_medkit_vda5050_msgs/msg/Order.msg deleted file mode 100644 index 907a2c5e8..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Order.msg +++ /dev/null @@ -1,27 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Order (MQTT topic: order) -# Order message sent from master control to AGV. - -HeaderId header - -# Required -string order_id -uint32 order_update_id -Node[] nodes -Edge[] edges - -# Optional -string zone_set_id diff --git a/src/ros2_medkit_vda5050_msgs/msg/PhysicalParameters.msg b/src/ros2_medkit_vda5050_msgs/msg/PhysicalParameters.msg deleted file mode 100644 index a24752ee7..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/PhysicalParameters.msg +++ /dev/null @@ -1,28 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - PhysicalParameters (Factsheet sub-type) -# Physical dimensions and speed limits of the AGV. - -# Required (all in SI units: m, m/s, m/s^2) -float64 speed_min -float64 speed_max -float64 acceleration_max -float64 deceleration_max -float64 height_max -float64 width -float64 length - -# Required per v2.0.0 schema (despite being logically optional) -float64 height_min diff --git a/src/ros2_medkit_vda5050_msgs/msg/ProtocolFeatures.msg b/src/ros2_medkit_vda5050_msgs/msg/ProtocolFeatures.msg deleted file mode 100644 index c746c8ad3..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/ProtocolFeatures.msg +++ /dev/null @@ -1,19 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - ProtocolFeatures (Factsheet sub-type) -# Protocol features supported by this AGV. - -OptionalParameter[] optional_parameters -AgvAction[] agv_actions diff --git a/src/ros2_medkit_vda5050_msgs/msg/ProtocolLimits.msg b/src/ros2_medkit_vda5050_msgs/msg/ProtocolLimits.msg deleted file mode 100644 index 905693789..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/ProtocolLimits.msg +++ /dev/null @@ -1,20 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - ProtocolLimits (Factsheet sub-type) -# Protocol limits describing the capabilities of this AGV. - -MaxStringLens max_string_lens -MaxArrayLens max_array_lens -Timing timing diff --git a/src/ros2_medkit_vda5050_msgs/msg/SafetyState.msg b/src/ros2_medkit_vda5050_msgs/msg/SafetyState.msg deleted file mode 100644 index f2dc06afe..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/SafetyState.msg +++ /dev/null @@ -1,26 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - SafetyState -# Safety-related state of the AGV. - -# Required -string e_stop -bool field_violation - -# E-stop type constants -string ESTOP_AUTOACK=AUTOACK -string ESTOP_MANUAL=MANUAL -string ESTOP_REMOTE=REMOTE -string ESTOP_NONE=NONE diff --git a/src/ros2_medkit_vda5050_msgs/msg/State.msg b/src/ros2_medkit_vda5050_msgs/msg/State.msg deleted file mode 100644 index 36c4e5b35..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/State.msg +++ /dev/null @@ -1,49 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - State (MQTT topic: state) -# Full AGV state published periodically and on state changes. - -HeaderId header - -# Required -string order_id -uint32 order_update_id -string last_node_id -uint32 last_node_sequence_id -bool driving -NodeState[] node_states -EdgeState[] edge_states -ActionState[] action_states -BatteryState battery_state -string operating_mode -ErrorEntry[] errors -SafetyState safety_state - -# Optional -string zone_set_id -bool paused -bool new_base_request -float64 distance_since_last_node -AgvPosition agv_position -Velocity velocity -Load[] loads -InfoEntry[] information - -# Operating mode constants -string MODE_AUTOMATIC=AUTOMATIC -string MODE_SEMIAUTOMATIC=SEMIAUTOMATIC -string MODE_MANUAL=MANUAL -string MODE_SERVICE=SERVICE -string MODE_TEACHIN=TEACHIN diff --git a/src/ros2_medkit_vda5050_msgs/msg/Timing.msg b/src/ros2_medkit_vda5050_msgs/msg/Timing.msg deleted file mode 100644 index 04d1e898a..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Timing.msg +++ /dev/null @@ -1,24 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Timing (Factsheet sub-type) -# Timing constraints for communication intervals. - -# Required -float64 min_order_interval # Minimum interval for order messages (seconds) -float64 min_state_interval # Minimum interval for state messages (seconds) - -# Optional -float64 default_state_interval -float64 visualization_interval diff --git a/src/ros2_medkit_vda5050_msgs/msg/Trajectory.msg b/src/ros2_medkit_vda5050_msgs/msg/Trajectory.msg deleted file mode 100644 index 43b942253..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Trajectory.msg +++ /dev/null @@ -1,25 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Trajectory -# NURBS trajectory definition for edge traversal. - -# Required - degree of the NURBS curve -uint32 degree - -# Required - knot vector (values 0.0 to 1.0) -float64[] knot_vector - -# Required - control points -ControlPoint[] control_points diff --git a/src/ros2_medkit_vda5050_msgs/msg/TypeSpecification.msg b/src/ros2_medkit_vda5050_msgs/msg/TypeSpecification.msg deleted file mode 100644 index abc221c31..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/TypeSpecification.msg +++ /dev/null @@ -1,27 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - TypeSpecification (Factsheet sub-type) -# Basic type information about the AGV. - -# Required -string series_name -string agv_kinematic # DIFF, OMNI, THREEWHEEL -string agv_class # FORKLIFT, CONVEYOR, TUGGER, CARRIER -float64 max_load_mass # kg -string[] localization_types # NATURAL, REFLECTOR, RFID, DMC, SPOT, GRID -string[] navigation_types # PHYSICAL_LINE_GUIDED, VIRTUAL_LINE_GUIDED, AUTONOMOUS - -# Optional -string series_description diff --git a/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg b/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg deleted file mode 100644 index 1ee239381..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Velocity.msg +++ /dev/null @@ -1,21 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Velocity -# AGV velocity in robot coordinates. - -# Optional (all fields) -float64 vx -float64 vy -float64 omega diff --git a/src/ros2_medkit_vda5050_msgs/msg/Visualization.msg b/src/ros2_medkit_vda5050_msgs/msg/Visualization.msg deleted file mode 100644 index a3ec6ee8d..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/Visualization.msg +++ /dev/null @@ -1,21 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - Visualization (MQTT topic: visualization) -# Higher-frequency position data for fleet manager visualization. -# All fields optional per spec (for bandwidth), but header recommended. - -HeaderId header -AgvPosition agv_position -Velocity velocity diff --git a/src/ros2_medkit_vda5050_msgs/msg/WheelDefinition.msg b/src/ros2_medkit_vda5050_msgs/msg/WheelDefinition.msg deleted file mode 100644 index 5b9c8449d..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/WheelDefinition.msg +++ /dev/null @@ -1,28 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - WheelDefinition (Factsheet sub-type) -# Definition of a single wheel on the AGV. - -# Required -string type # DRIVE, CASTER, FIXED, MECANUM -bool is_active_driven -bool is_active_steered -WheelPosition position -float64 diameter # meters -float64 width # meters - -# Optional -float64 center_displacement # meters -string constraints diff --git a/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg b/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg deleted file mode 100644 index b7ba09525..000000000 --- a/src/ros2_medkit_vda5050_msgs/msg/WheelPosition.msg +++ /dev/null @@ -1,23 +0,0 @@ -# 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. -# -# VDA 5050 v2.0.0 - WheelPosition (Factsheet sub-type) -# Position of a wheel relative to the AGV origin. - -# Required -float64 x -float64 y - -# Optional -float64 theta diff --git a/src/ros2_medkit_vda5050_msgs/package.xml b/src/ros2_medkit_vda5050_msgs/package.xml deleted file mode 100644 index f8222819a..000000000 --- a/src/ros2_medkit_vda5050_msgs/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - ros2_medkit_vda5050_msgs - 0.4.0 - VDA 5050 v2.0.0 message definitions for ROS 2. Vendor-neutral types matching the official VDA/VDMA specification. - - bburda - mfaferek93 - Apache-2.0 - - https://github.com/selfpatch/ros2_medkit - https://github.com/VDA5050/VDA5050 - - ament_cmake - rosidl_default_generators - - builtin_interfaces - - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - From 1ee29c2212f475a1cafe61be1aa480bbfa7f2682 Mon Sep 17 00:00:00 2001 From: mfaferek93 Date: Sun, 5 Apr 2026 20:36:21 +0200 Subject: [PATCH 7/8] fix: address review findings from PR #331 - Populate fault timestamps (first/last_occurred) from JSON response - Fix TODO reference (removed stale #332 link) - Add README.md for sovd_service_interface plugin - Move test domain range from 230-232 to 1-9 (avoid collision with peer_aggregation secondary domains at 230) --- .../cmake/ROS2MedkitTestDomain.cmake | 3 +- .../CMakeLists.txt | 2 +- .../README.md | 31 +++++++++++++++++++ .../src/sovd_service_interface.cpp | 18 ++++++++--- 4 files changed, 47 insertions(+), 7 deletions(-) create mode 100644 src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/README.md diff --git a/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake b/src/ros2_medkit_cmake/cmake/ROS2MedkitTestDomain.cmake index bb2009dcd..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) -# ros2_medkit_sovd_service_iface: 230 - 232 (3 slots, max domain ID is 232) +# 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_plugins/ros2_medkit_sovd_service_interface/CMakeLists.txt b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/CMakeLists.txt index 2d705861f..4c2f755aa 100644 --- a/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/CMakeLists.txt +++ b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/CMakeLists.txt @@ -91,7 +91,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) include(ROS2MedkitTestDomain) - medkit_init_test_domains(START 230 END 232) + medkit_init_test_domains(START 1 END 9) ament_add_gtest(test_sovd_service_interface test/test_sovd_service_interface.cpp 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/src/sovd_service_interface.cpp b/src/ros2_medkit_plugins/ros2_medkit_sovd_service_interface/src/sovd_service_interface.cpp index 3f8674fdb..da2cc304f 100644 --- 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 @@ -179,6 +179,16 @@ void SovdServiceInterface::handle_list_entity_faults( 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()) { @@ -209,11 +219,9 @@ void SovdServiceInterface::handle_get_entity_data( return; } - // TODO(#332): Implement data retrieval via PluginContext::get_entity_data() - // when the method is added. Currently PluginContext exposes entity metadata - // and faults but not live topic data. The VDA 5050 agent should use the - // HTTP REST API (/api/v1/apps/{id}/data) as the primary data source until - // this service is fully implemented. + // TODO: Implement data retrieval when PluginContext gains data access. + // Currently PluginContext exposes entity metadata and faults but not live + // topic data. Use HTTP REST API (/api/v1/apps/{id}/data) as alternative. response->data_json = "{}"; response->success = false; response->error_message = "GetEntityData not yet implemented - use HTTP REST API for topic data"; From d0f8dfc209b952e4ba6fb9e88a30c54575468936 Mon Sep 17 00:00:00 2001 From: mfaferek93 Date: Sun, 5 Apr 2026 20:45:21 +0200 Subject: [PATCH 8/8] fix: link GetEntityData stub to tracking issue #351 --- .../src/sovd_service_interface.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 index da2cc304f..ae9e4b283 100644 --- 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 @@ -219,9 +219,8 @@ void SovdServiceInterface::handle_get_entity_data( return; } - // TODO: Implement data retrieval when PluginContext gains data access. - // Currently PluginContext exposes entity metadata and faults but not live - // topic data. Use HTTP REST API (/api/v1/apps/{id}/data) as alternative. + // 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";