diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 2443d9a1f..41090d7bd 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -49,6 +49,7 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ RUN apt-get update && apt-get install -y \ ros-jazzy-desktop-full \ + ros-jazzy-test-msgs \ python3-colcon-common-extensions \ python3-rosdep \ && rm -rf /var/lib/apt/lists/* diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 3f227941c..006dbcc9c 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -18,7 +18,7 @@ // Comment this out if you don't want zsh and oh-my-zsh "ghcr.io/devcontainers/features/common-utils:2": { "installZsh": true, - "configureZshAsDefaultShell": true, + "configureZshAsDefaultShell": false, "installOhMyZsh": true }, "ghcr.io/devcontainers/features/git:1": {}, @@ -47,7 +47,7 @@ "christian-kohler.path-intellisense" ], "settings": { - "terminal.integrated.defaultProfile.linux": "zsh", + "terminal.integrated.defaultProfile.linux": "bash", "python.defaultInterpreterPath": "/usr/bin/python3", "cmake.configureOnOpen": false } diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e1aaab840..68a2ef254 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -14,6 +14,8 @@ jobs: steps: - name: Checkout repository uses: actions/checkout@v4 + with: + submodules: recursive - name: Set up ROS 2 Jazzy uses: ros-tooling/setup-ros@v0.7 @@ -23,7 +25,7 @@ jobs: - name: Install dependencies run: | sudo apt-get update - sudo apt-get install -y clang-format clang-tidy + sudo apt-get install -y clang-format clang-tidy ros-jazzy-test-msgs source /opt/ros/jazzy/setup.bash rosdep update rosdep install --from-paths src --ignore-src -r -y @@ -72,6 +74,8 @@ jobs: steps: - name: Checkout repository uses: actions/checkout@v4 + with: + submodules: recursive - name: Set up ROS 2 Jazzy uses: ros-tooling/setup-ros@v0.7 @@ -81,7 +85,7 @@ jobs: - name: Install dependencies run: | sudo apt-get update - sudo apt-get install -y lcov + sudo apt-get install -y lcov ros-jazzy-test-msgs source /opt/ros/jazzy/setup.bash rosdep update rosdep install --from-paths src --ignore-src -r -y @@ -107,38 +111,36 @@ jobs: - name: Generate coverage report run: | - # Capture coverage data with error tolerance + # Capture coverage data + # IMPORTANT: --ignore-errors gcov is required to skip .gcda files + # that have missing corresponding .gcno files (common in multi-package builds) lcov --capture --directory build --output-file coverage.raw.info \ - --ignore-errors mismatch,negative,empty || true - - # Check if we have valid coverage data - if [ -s coverage.raw.info ] && grep -q 'SF:' coverage.raw.info; then - # Extract only our source code (exclude gtest_vendor and other ROS dependencies) - lcov --extract coverage.raw.info \ - '*/ros2_medkit/src/*/src/*' \ - '*/ros2_medkit/src/*/include/*' \ - --output-file coverage.info \ - --ignore-errors unused,empty || true - - if [ -s coverage.info ]; then - lcov --list coverage.info - # Generate HTML report - genhtml coverage.info --output-directory coverage_html \ - --ignore-errors source || true - else - echo "Filtered coverage.info is empty, using raw coverage" - cp coverage.raw.info coverage.info - lcov --list coverage.info || true - genhtml coverage.info --output-directory coverage_html \ - --ignore-errors source || true - fi - else - echo "No valid coverage data found in coverage.raw.info" - echo "Creating empty coverage file to prevent upload failure" - touch coverage.info - mkdir -p coverage_html + --ignore-errors mismatch,negative,empty,gcov + + # Validate coverage data exists + if [ ! -s coverage.raw.info ] || ! grep -q 'SF:' coverage.raw.info; then + echo "::error::No valid coverage data found in coverage.raw.info" + exit 1 fi + # Extract only our source code (exclude gtest_vendor and other ROS dependencies) + lcov --extract coverage.raw.info \ + '*/ros2_medkit/src/*/src/*' \ + '*/ros2_medkit/src/*/include/*' \ + --output-file coverage.info \ + --ignore-errors unused,empty + + # Validate filtered coverage + if [ ! -s coverage.info ]; then + echo "::error::Filtered coverage.info is empty - no source files matched" + exit 1 + fi + + lcov --list coverage.info + + # Generate HTML report + genhtml coverage.info --output-directory coverage_html --ignore-errors source + - name: Upload coverage HTML report as artifact uses: actions/upload-artifact@v4 with: diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index bc548d818..565e0cdcc 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -33,6 +33,8 @@ jobs: steps: - name: Checkout repository uses: actions/checkout@v4 + with: + submodules: recursive - name: Set up Python uses: actions/setup-python@v5 diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..466fe0572 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "src/dynamic_message_introspection"] + path = src/dynamic_message_introspection + url = https://github.com/selfpatch/dynamic_message_introspection.git diff --git a/README.md b/README.md index 4ea0d71ed..0ec8442f2 100644 --- a/README.md +++ b/README.md @@ -52,11 +52,14 @@ so the same concepts can be used across robots, vehicles, and other embedded sys ```bash mkdir -p ~/ros2_medkit_ws/src cd ~/ros2_medkit_ws/src -git clone https://github.com/selfpatch/ros2_medkit.git +git clone --recurse-submodules https://github.com/selfpatch/ros2_medkit.git cd ~/ros2_medkit_ws rosdep install --from-paths src --ignore-src -r -y ``` +> **Note:** If you cloned without `--recurse-submodules`, run: +> `git submodule update --init --recursive` + ### 2. Build ```bash diff --git a/docs/Doxyfile b/docs/Doxyfile index d82d3efe2..5576925c4 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -14,6 +14,8 @@ INPUT = ../src/ros2_medkit_gateway/include \ ../src/ros2_medkit_fault_reporter/src \ ../src/ros2_medkit_diagnostic_bridge/include \ ../src/ros2_medkit_diagnostic_bridge/src \ + ../src/ros2_medkit_serialization/include \ + ../src/ros2_medkit_serialization/src \ ../src/ros2_medkit_msgs RECURSIVE = YES FILE_PATTERNS = *.hpp *.cpp *.h diff --git a/docs/api/index.rst b/docs/api/index.rst index 1e546513e..325d6c51d 100644 --- a/docs/api/index.rst +++ b/docs/api/index.rst @@ -81,3 +81,23 @@ Bridge node that converts ROS 2 /diagnostics messages to FaultManager faults. .. doxygenclass:: ros2_medkit_diagnostic_bridge::DiagnosticBridgeNode :members: + +ros2_medkit_serialization +------------------------- + +Runtime JSON ↔ ROS 2 message serialization library. + +.. doxygenclass:: ros2_medkit_serialization::JsonSerializer + :members: + +.. doxygenclass:: ros2_medkit_serialization::TypeCache + :members: + +.. doxygenclass:: ros2_medkit_serialization::SerializationError + :members: + +.. doxygenclass:: ros2_medkit_serialization::TypeNotFoundError + :members: + +.. doxygenclass:: ros2_medkit_serialization::JsonConversionError + :members: diff --git a/docs/design/index.rst b/docs/design/index.rst index 98c45011a..48611c280 100644 --- a/docs/design/index.rst +++ b/docs/design/index.rst @@ -10,4 +10,5 @@ This section contains design documentation for the ros2_medkit project packages. ros2_medkit_fault_manager/index ros2_medkit_fault_reporter/index ros2_medkit_gateway/index + ros2_medkit_serialization/index diff --git a/docs/design/ros2_medkit_serialization b/docs/design/ros2_medkit_serialization new file mode 120000 index 000000000..7fdcb92ba --- /dev/null +++ b/docs/design/ros2_medkit_serialization @@ -0,0 +1 @@ +../../src/ros2_medkit_serialization/design \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst index 26155a871..8ca63d1f1 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -15,7 +15,7 @@ endpoints for data access, operations, configurations, and fault management. .. note:: - Version 0.1.0 — First public release. + Version 0.1.0 - First public release still under construction... Quick Links ----------- diff --git a/docs/installation.rst b/docs/installation.rst index 32d33aed6..a6b14777b 100644 --- a/docs/installation.rst +++ b/docs/installation.rst @@ -75,9 +75,15 @@ ros2_medkit is currently distributed as source code. Binary packages will be ava .. code-block:: bash - git clone https://github.com/selfpatch/ros2_medkit.git + git clone --recurse-submodules https://github.com/selfpatch/ros2_medkit.git cd ros2_medkit + If you already cloned without submodules, initialize them: + + .. code-block:: bash + + git submodule update --init --recursive + 3. **Install dependencies** .. code-block:: bash diff --git a/docs/introduction.rst b/docs/introduction.rst index c819b9357..eb6934a9e 100644 --- a/docs/introduction.rst +++ b/docs/introduction.rst @@ -46,12 +46,19 @@ ros2_medkit consists of several ROS 2 packages: **ros2_medkit_gateway** The main HTTP gateway node. Discovers ROS 2 entities and exposes them via REST API. +**ros2_medkit_serialization** + Runtime JSON ↔ ROS 2 message serialization library using dynmsg. Enables native + message handling without compile-time type dependencies. + **ros2_medkit_fault_manager** Stores and manages fault lifecycle. Provides ROS 2 services for fault operations. **ros2_medkit_fault_reporter** Client library for reporting faults from your ROS 2 nodes. +**ros2_medkit_diagnostic_bridge** + Bridge node that converts standard ROS 2 ``/diagnostics`` messages to fault manager faults. + **ros2_medkit_msgs** Message and service definitions for fault management. diff --git a/docs/tutorials/devcontainer.rst b/docs/tutorials/devcontainer.rst index eee783d28..96c1a91b1 100644 --- a/docs/tutorials/devcontainer.rst +++ b/docs/tutorials/devcontainer.rst @@ -31,13 +31,19 @@ Prerequisites Quick Start ----------- -1. **Clone the repository:** +1. **Clone the repository with submodules:** .. code-block:: bash - git clone https://github.com/selfpatch/ros2_medkit.git + git clone --recurse-submodules https://github.com/selfpatch/ros2_medkit.git cd ros2_medkit + If you already cloned without submodules: + + .. code-block:: bash + + git submodule update --init --recursive + 2. **Open in VS Code:** .. code-block:: bash diff --git a/src/dynamic_message_introspection b/src/dynamic_message_introspection new file mode 160000 index 000000000..b38e36c2e --- /dev/null +++ b/src/dynamic_message_introspection @@ -0,0 +1 @@ +Subproject commit b38e36c2e2ff2a490d1b1e9db99c55ecb5dd5a07 diff --git a/src/ros2_medkit_gateway/CMakeLists.txt b/src/ros2_medkit_gateway/CMakeLists.txt index 184094c3c..6c6d18470 100644 --- a/src/ros2_medkit_gateway/CMakeLists.txt +++ b/src/ros2_medkit_gateway/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(action_msgs REQUIRED) find_package(rclcpp_action REQUIRED) find_package(example_interfaces REQUIRED) find_package(ros2_medkit_msgs REQUIRED) +find_package(ros2_medkit_serialization REQUIRED) # Find cpp-httplib using pkg-config find_package(PkgConfig REQUIRED) @@ -103,8 +104,6 @@ add_library(gateway_lib STATIC src/config.cpp src/gateway_node.cpp src/discovery_manager.cpp - src/ros2_cli_wrapper.cpp - src/output_parser.cpp src/data_access_manager.cpp src/type_introspection.cpp src/native_topic_sampler.cpp @@ -139,6 +138,7 @@ ament_target_dependencies(gateway_lib ament_index_cpp action_msgs ros2_medkit_msgs + ros2_medkit_serialization ) target_link_libraries(gateway_lib diff --git a/src/ros2_medkit_gateway/README.md b/src/ros2_medkit_gateway/README.md index ac540f142..e99c53444 100644 --- a/src/ros2_medkit_gateway/README.md +++ b/src/ros2_medkit_gateway/README.md @@ -212,9 +212,9 @@ curl http://localhost:8080/api/v1/components/nonexistent/data **Behavior:** - Returns array of all topics under the component's namespace -- Each topic is sampled once with `ros2 topic echo --once` +- Each topic is sampled once using native rclcpp GenericSubscription - Empty array `[]` returned if component has no topics -- 3-second timeout per topic to accommodate slow-publishing topics +- Configurable timeout per topic (default: 2 seconds) **Use Cases:** - Remote diagnostics - Read all sensor values from a component diff --git a/src/ros2_medkit_gateway/config/gateway_params.yaml b/src/ros2_medkit_gateway/config/gateway_params.yaml index 5e97f7ff0..a4cbf1bbb 100644 --- a/src/ros2_medkit_gateway/config/gateway_params.yaml +++ b/src/ros2_medkit_gateway/config/gateway_params.yaml @@ -85,19 +85,20 @@ ros2_medkit_gateway: # Valid range: 1-50 max_parallel_topic_samples: 20 - # Timeout (in seconds) for sampling topic data via ros2 topic echo + # Timeout (in seconds) for sampling topic data via native GenericSubscription # Topics without publishers return immediately with metadata (no timeout wait) # This timeout only applies when topics have active publishers # Valid range: 0.1-30.0 topic_sample_timeout_sec: 2.0 - # NOTE: Native sampling is always enabled - # The gateway uses native rclcpp APIs for topic discovery and sampling: - # - Uses node->get_topic_names_and_types() instead of `ros2 topic list` - # - Checks publisher counts before sampling to skip idle topics immediately - # - Returns metadata instantly for topics without publishers (no CLI timeout wait) - # This significantly improves UX when many topics are idle (e.g., robot waiting for commands) - # CLI is only used for publishing (ros2 topic pub) + # NOTE: Native-only implementation + # The gateway uses native rclcpp APIs for all ROS 2 interactions: + # - Topic discovery: node->get_topic_names_and_types() + # - Topic sampling: rclcpp::GenericSubscription + JsonSerializer + # - Topic publishing: rclcpp::GenericPublisher + JsonSerializer + # - Service calls: rclcpp::GenericClient + JsonSerializer + # - Action operations: Internal action services via GenericClient + # No ROS 2 CLI dependencies - pure C++ implementation using ros2_medkit_serialization. # Authentication Configuration (REQ_INTEROP_086, REQ_INTEROP_087) # JWT-based authentication with Role-Based Access Control (RBAC) diff --git a/src/ros2_medkit_gateway/design/architecture.puml b/src/ros2_medkit_gateway/design/architecture.puml index 4d13c8c18..0d5a581bd 100644 --- a/src/ros2_medkit_gateway/design/architecture.puml +++ b/src/ros2_medkit_gateway/design/architecture.puml @@ -46,15 +46,12 @@ package "ros2_medkit_gateway" { + sample_topics_parallel(): vector } - class ROS2CLIWrapper { - + exec(): string - + is_command_available(): bool - + escape_shell_arg(): string - } - - class OutputParser { - + parse_yaml(): json - - yaml_to_json(): json + class JsonSerializer { + + serialize(): SerializedMessage + + deserialize(): json + + to_json(): json + + from_json(): RosMessage_Cpp + + yaml_to_json(): json {static} } class Area { @@ -103,9 +100,8 @@ DiscoveryManager --> "rclcpp::Node" : uses RESTServer --> GatewayNode : uses RESTServer --> DataAccessManager : uses -' DataAccessManager owns utility classes -DataAccessManager *--> ROS2CLIWrapper : owns (publishing) -DataAccessManager *--> OutputParser : owns +' DataAccessManager owns utility classes (native implementation) +DataAccessManager *--> JsonSerializer : owns (serialization) DataAccessManager *--> NativeTopicSampler : owns ' NativeTopicSampler uses Node interface diff --git a/src/ros2_medkit_gateway/design/index.rst b/src/ros2_medkit_gateway/design/index.rst index 04a104a7e..e6b4d3666 100644 --- a/src/ros2_medkit_gateway/design/index.rst +++ b/src/ros2_medkit_gateway/design/index.rst @@ -85,15 +85,11 @@ The following diagram shows the relationships between the main components of the + sample_topics_parallel(): vector } - class ROS2CLIWrapper { - + exec(): string - + is_command_available(): bool - + escape_shell_arg(): string {static} - } - - class OutputParser { - + parse_yaml(): json - + yaml_to_json(): json {static} + class JsonSerializer { + + serialize(): SerializedMessage + + deserialize(): json + + to_json(): json + + from_json(): RosMessage_Cpp } class Area { @@ -160,13 +156,12 @@ The following diagram shows the relationships between the main components of the RESTServer --> OperationManager : uses RESTServer --> ConfigurationManager : uses - ' OperationManager uses DiscoveryManager and CLI + ' OperationManager uses DiscoveryManager and native serialization OperationManager --> DiscoveryManager : uses - OperationManager *--> ROS2CLIWrapper : owns + OperationManager *--> JsonSerializer : owns - ' DataAccessManager owns utility classes - DataAccessManager *--> ROS2CLIWrapper : owns (publishing) - DataAccessManager *--> OutputParser : owns + ' DataAccessManager owns utility classes and uses native publishing + DataAccessManager *--> JsonSerializer : owns (serialization) DataAccessManager *--> NativeTopicSampler : owns ' NativeTopicSampler uses Node interface @@ -215,13 +210,14 @@ Main Components - Attaches operations (services/actions) to their parent components - Uses O(n+m) algorithm with hash maps for efficient service/action attachment -3. **OperationManager** - Executes ROS 2 operations (services and actions) - - Calls ROS 2 services synchronously via ``ros2 service call`` CLI - - Sends action goals via ``ros2 action send_goal`` CLI (3s timeout for acceptance) +3. **OperationManager** - Executes ROS 2 operations (services and actions) using native APIs + - Calls ROS 2 services via ``rclcpp::GenericClient`` with native serialization + - Sends action goals via native action client interfaces - Tracks active action goals with status, feedback, and timestamps - Subscribes to ``/_action/status`` topics for real-time goal status updates - - Supports goal cancellation via ``ros2 action cancel`` CLI + - Supports goal cancellation via native cancel service calls - Automatically cleans up completed goals older than 5 minutes + - Uses ``ros2_medkit_serialization`` for JSON ↔ ROS 2 message conversion 4. **RESTServer** - Provides the HTTP/REST API - Discovery endpoints: ``/health``, ``/``, ``/areas``, ``/components``, ``/areas/{area_id}/components`` @@ -241,35 +237,30 @@ Main Components - Caches parameter clients per node for efficiency - Converts between JSON and ROS 2 parameter types automatically -6. **DataAccessManager** - Reads runtime data from ROS 2 topics +6. **DataAccessManager** - Reads and writes runtime data from/to ROS 2 topics - Uses native rclcpp APIs for fast topic discovery and sampling - Checks publisher counts before sampling to skip idle topics instantly - Returns metadata (type, schema) for topics without publishers - - Falls back to ROS 2 CLI only for publishing (``ros2 topic pub``) + - Uses native ``rclcpp::GenericPublisher`` for topic publishing with CDR serialization - Returns topic data as JSON with metadata (topic name, timestamp, type info) - Parallel topic sampling with configurable concurrency limit (``max_parallel_topic_samples``, default: 10) 7. **NativeTopicSampler** - Fast topic sampling using native rclcpp APIs - Discovers topics via ``node->get_topic_names_and_types()`` + - Uses ``rclcpp::GenericSubscription`` for type-agnostic message sampling - Checks ``count_publishers()`` before sampling to skip idle topics - - Returns metadata instantly for topics without publishers (no CLI timeout) + - Returns metadata instantly for topics without publishers (no timeout) - Significantly improves UX when robot has many idle topics -8. **ROS2CLIWrapper** - Executes ROS 2 CLI commands safely - - Used for publishing (``ros2 topic pub``), service calls, and action operations - - Wraps ``popen()`` with RAII for exception safety during command execution - - Checks command exit status to detect failures - - Prevents command injection with shell argument escaping - - Validates command availability before execution - -9. **OutputParser** - Converts ROS 2 CLI output to JSON - - Parses YAML output from ``ros2 topic echo`` and ``ros2 service call`` - - Preserves type information (bool → int → double → string precedence) - - Handles multi-document YAML streams correctly - - Converts ROS message structures to nested JSON objects - - Provides static ``yaml_to_json()`` utility for reuse - -10. **Data Models** - Entity representations +8. **JsonSerializer** (ros2_medkit_serialization) - Converts between JSON and ROS 2 messages + - Uses ``dynmsg`` library for dynamic type introspection + - Serializes JSON to CDR format for publishing via ``serialize()`` + - Deserializes CDR to JSON for subscriptions via ``deserialize()`` + - Converts between deserialized ROS 2 messages and JSON via ``to_json()`` / ``from_json()`` + - Provides static ``yaml_to_json()`` utility for YAML to JSON conversion + - Thread-safe and stateless design + +9. **Data Models** - Entity representations - ``Area`` - Physical or logical domain - ``Component`` - Hardware or software component with attached operations - ``ServiceInfo`` - Service metadata (path, name, type) diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/data_access_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/data_access_manager.hpp index f7565a35f..ab7959b3d 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/data_access_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/data_access_manager.hpp @@ -15,15 +15,17 @@ #pragma once #include +#include #include #include +#include #include +#include #include #include "ros2_medkit_gateway/native_topic_sampler.hpp" -#include "ros2_medkit_gateway/output_parser.hpp" -#include "ros2_medkit_gateway/ros2_cli_wrapper.hpp" #include "ros2_medkit_gateway/type_introspection.hpp" +#include "ros2_medkit_serialization/json_serializer.hpp" namespace ros2_medkit_gateway { @@ -100,9 +102,26 @@ class DataAccessManager { */ json sample_result_to_json(const TopicSampleResult & sample); + /** + * @brief Get or create a cached GenericPublisher for a topic + * @param topic_path Full topic path + * @param msg_type ROS 2 message type + * @return Shared pointer to GenericPublisher + */ + rclcpp::GenericPublisher::SharedPtr get_or_create_publisher(const std::string & topic_path, + const std::string & msg_type); + rclcpp::Node * node_; - std::unique_ptr cli_wrapper_; - std::unique_ptr output_parser_; + + /// JSON serializer for native message serialization + std::shared_ptr serializer_; + + /// Cached publishers (topic+type -> publisher) + std::unordered_map publishers_; + + /// Mutex for thread-safe publisher cache access + mutable std::shared_mutex publishers_mutex_; + std::unique_ptr type_introspection_; std::unique_ptr native_sampler_; int max_parallel_samples_; diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/native_topic_sampler.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/native_topic_sampler.hpp index 766012324..dce5bf582 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/native_topic_sampler.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/native_topic_sampler.hpp @@ -26,6 +26,7 @@ #include #include "ros2_medkit_gateway/models.hpp" +#include "ros2_medkit_serialization/json_serializer.hpp" namespace ros2_medkit_gateway { @@ -280,17 +281,15 @@ class NativeTopicSampler { static bool is_system_topic(const std::string & topic_name); private: - /** - * @brief Parse YAML-formatted message string to JSON - */ - json parse_message_yaml(const std::string & yaml_str); - /** * @brief Get the message type for a topic from the graph */ std::string get_topic_type(const std::string & topic_name); rclcpp::Node * node_; + + /// Native JSON serializer for topic deserialization + std::shared_ptr serializer_; }; } // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/operation_manager.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/operation_manager.hpp index e7391fce8..2ae77fe97 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/operation_manager.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/operation_manager.hpp @@ -15,19 +15,24 @@ #pragma once #include +#include #include #include #include #include #include #include +#include +#include #include +#include #include #include #include "ros2_medkit_gateway/discovery_manager.hpp" #include "ros2_medkit_gateway/models.hpp" -#include "ros2_medkit_gateway/ros2_cli_wrapper.hpp" +#include "ros2_medkit_serialization/json_serializer.hpp" +#include "ros2_medkit_serialization/service_action_types.hpp" namespace ros2_medkit_gateway { @@ -126,7 +131,7 @@ class OperationManager { // ==================== ACTION OPERATIONS ==================== - /// Send a goal to an action server using ros2 action send_goal + /// Send a goal to an action server using native rclcpp_action internal services /// @param action_path Full action path (e.g., "/powertrain/engine/long_calibration") /// @param action_type Action type (e.g., "example_interfaces/action/Fibonacci") /// @param goal JSON goal data @@ -203,27 +208,52 @@ class OperationManager { void unsubscribe_from_action_status(const std::string & action_path); private: - /// Convert JSON to YAML string for ros2 service call - std::string json_to_yaml(const json & j); + /// Set of clients for an action (internal services) + struct ActionClientSet { + rclcpp::GenericClient::SharedPtr send_goal_client; + rclcpp::GenericClient::SharedPtr get_result_client; + rclcpp::GenericClient::SharedPtr cancel_goal_client; + std::string action_type; // Store type for later use + }; - /// Parse YAML output from ros2 service call to JSON - json parse_service_response(const std::string & yaml_output); + /// Convert UUID hex string to JSON array of byte values + json uuid_hex_to_json_array(const std::string & uuid_hex); - /// Convert UUID hex string to array of byte values for YAML - std::string uuid_to_yaml_array(const std::string & uuid_hex); + /// Generate a random UUID + std::array generate_uuid(); - /// Parse ros2 action send_goal CLI output to extract goal_id and status - ActionSendGoalResult parse_send_goal_cli_output(const std::string & output); - - /// Parse ros2 action cancel output - ActionCancelResult parse_cancel_output(const std::string & output); + /// Convert UUID bytes to JSON array + json uuid_bytes_to_json_array(const std::array & uuid); /// Track a new goal void track_goal(const std::string & goal_id, const std::string & action_path, const std::string & action_type); + /// Get or create a cached GenericClient for a service + rclcpp::GenericClient::SharedPtr get_or_create_service_client(const std::string & service_path, + const std::string & service_type); + + /// Get or create cached action clients for an action + ActionClientSet & get_or_create_action_clients(const std::string & action_path, const std::string & action_type); + + /// Make cache key from service path and type + static std::string make_client_key(const std::string & service_path, const std::string & service_type); + rclcpp::Node * node_; DiscoveryManager * discovery_manager_; - std::unique_ptr cli_wrapper_; + + /// Random number generator for UUID generation + std::mt19937 rng_; + std::mutex rng_mutex_; + + /// Native JSON serializer for service calls + std::shared_ptr serializer_; + + /// Cache for GenericClient instances (key = "service_path|service_type") + mutable std::shared_mutex clients_mutex_; + std::map generic_clients_; + + /// Cache for action client sets (key = action_path) + std::map action_clients_; /// Timeout for service calls in seconds (configurable via service_call_timeout_sec param) int service_call_timeout_sec_; diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/output_parser.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/output_parser.hpp deleted file mode 100644 index 2be76afcd..000000000 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/output_parser.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2025 mfaferek93 -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include // NOLINT(build/include_order) - -#include -#include - -namespace ros2_medkit_gateway { - -using json = nlohmann::json; - -class OutputParser { - public: - OutputParser() = default; - - /** - * @brief Parse YAML string to JSON - */ - json parse_yaml(const std::string & yaml_str); - - /** - * @brief Convert YAML node to JSON (static utility) - */ - static json yaml_to_json(const YAML::Node & node); -}; - -} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2_cli_wrapper.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2_cli_wrapper.hpp deleted file mode 100644 index 66439793c..000000000 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/ros2_cli_wrapper.hpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2025 mfaferek93 -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include -#include - -namespace ros2_medkit_gateway { - -class ROS2CLIWrapper { - public: - ROS2CLIWrapper() = default; - - /** - * @brief Execute shell command and return stdout - */ - std::string exec(const std::string & command); - - /** - * @brief Check if command exists in PATH - */ - bool is_command_available(const std::string & command); - - /** - * @brief Escape shell arguments - */ - static std::string escape_shell_arg(const std::string & arg); -}; - -} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/type_introspection.hpp b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/type_introspection.hpp index f0e600e84..0ae51c544 100644 --- a/src/ros2_medkit_gateway/include/ros2_medkit_gateway/type_introspection.hpp +++ b/src/ros2_medkit_gateway/include/ros2_medkit_gateway/type_introspection.hpp @@ -20,11 +20,9 @@ #include #include -namespace ros2_medkit_gateway { +#include "ros2_medkit_serialization/json_serializer.hpp" -// Forward declarations -class ROS2CLIWrapper; -class OutputParser; +namespace ros2_medkit_gateway { /** * @brief Information about a ROS 2 message type including schema and template @@ -39,23 +37,23 @@ struct TopicTypeInfo { * @brief Provides type introspection capabilities for ROS 2 message types * * This class allows querying information about ROS 2 topics and message types - * without requiring actual message data. It uses CLI commands and a Python - * helper script to gather type information. + * without requiring actual message data. It uses the native JsonSerializer + * to gather type information via dynmsg. * - * Type information is cached to avoid repeated CLI calls for the same types. + * Type information is cached to avoid repeated introspection calls for the same types. */ class TypeIntrospection { public: /** * @brief Construct a new TypeIntrospection object * - * @param scripts_path Path to the directory containing helper scripts + * The scripts_path parameter is deprecated and ignored - native serialization is used. */ explicit TypeIntrospection(const std::string & scripts_path = ""); ~TypeIntrospection() = default; - // Disable copy (due to mutex and unique_ptrs) + // Disable copy (due to mutex) TypeIntrospection(const TypeIntrospection &) = delete; TypeIntrospection & operator=(const TypeIntrospection &) = delete; @@ -75,7 +73,7 @@ class TypeIntrospection { /** * @brief Get the default value template for a message type * - * Uses `ros2 interface proto` to generate a template with default values. + * Uses native JsonSerializer to generate a template with default values. * * @param type_name Full type name * @return nlohmann::json Template with default field values @@ -86,7 +84,7 @@ class TypeIntrospection { /** * @brief Get the JSON schema for a message type * - * Uses a Python helper script to generate recursive type schema. + * Uses native JsonSerializer to generate type schema. * * @param type_name Full type name * @return nlohmann::json Schema with field types @@ -95,9 +93,8 @@ class TypeIntrospection { nlohmann::json get_type_schema(const std::string & type_name); private: - std::string scripts_path_; ///< Path to helper scripts - std::unique_ptr cli_wrapper_; ///< CLI wrapper for commands - std::unique_ptr output_parser_; ///< Parser for CLI output + /// Native JSON serializer for type introspection + std::shared_ptr serializer_; std::unordered_map type_cache_; ///< Cache for type info mutable std::mutex cache_mutex_; ///< Mutex for thread-safe cache access diff --git a/src/ros2_medkit_gateway/package.xml b/src/ros2_medkit_gateway/package.xml index 5ed2a6136..5bfe149d5 100644 --- a/src/ros2_medkit_gateway/package.xml +++ b/src/ros2_medkit_gateway/package.xml @@ -22,6 +22,7 @@ yaml_cpp_vendor ros2_medkit_msgs + ros2_medkit_serialization rosidl_runtime_py rosidl_parser diff --git a/src/ros2_medkit_gateway/src/data_access_manager.cpp b/src/ros2_medkit_gateway/src/data_access_manager.cpp index ff324143b..dde48d516 100644 --- a/src/ros2_medkit_gateway/src/data_access_manager.cpp +++ b/src/ros2_medkit_gateway/src/data_access_manager.cpp @@ -17,16 +17,17 @@ #include #include #include +#include #include #include "ros2_medkit_gateway/exceptions.hpp" +#include "ros2_medkit_serialization/serialization_error.hpp" namespace ros2_medkit_gateway { DataAccessManager::DataAccessManager(rclcpp::Node * node) : node_(node) - , cli_wrapper_(std::make_unique()) - , output_parser_(std::make_unique()) + , serializer_(std::make_shared()) , type_introspection_( std::make_unique(ament_index_cpp::get_package_share_directory("ros2_medkit_gateway") + "/scr" "ipt" @@ -49,38 +50,57 @@ DataAccessManager::DataAccessManager(rclcpp::Node * node) topic_sample_timeout_sec_ = 1.0; } - // CLI is still needed for publishing (ros2 topic pub) - if (!cli_wrapper_->is_command_available("ros2")) { - RCLCPP_WARN(node_->get_logger(), "ROS 2 CLI not found, publishing will not be available"); - } - RCLCPP_INFO(node_->get_logger(), - "DataAccessManager initialized (native_sampling=enabled, max_parallel_samples=%d, " - "topic_sample_timeout=%.2fs)", + "DataAccessManager initialized (native_sampling=enabled, native_publishing=enabled, " + "max_parallel_samples=%d, topic_sample_timeout=%.2fs)", max_parallel_samples_, topic_sample_timeout_sec_); } +rclcpp::GenericPublisher::SharedPtr DataAccessManager::get_or_create_publisher(const std::string & topic_path, + const std::string & msg_type) { + const std::string key = topic_path + "|" + msg_type; + + // Try read lock first (fast path) + { + std::shared_lock lock(publishers_mutex_); + auto it = publishers_.find(key); + if (it != publishers_.end()) { + return it->second; + } + } + + // Need to create - take exclusive lock + std::unique_lock lock(publishers_mutex_); + + // Double-check (another thread might have created it) + auto it = publishers_.find(key); + if (it != publishers_.end()) { + return it->second; + } + + // Create new publisher with default QoS (reliable, keep last 10) + auto publisher = node_->create_generic_publisher(topic_path, msg_type, rclcpp::QoS(10)); + publishers_[key] = publisher; + + RCLCPP_DEBUG(node_->get_logger(), "Created generic publisher for %s (%s)", topic_path.c_str(), msg_type.c_str()); + + return publisher; +} + json DataAccessManager::publish_to_topic(const std::string & topic_path, const std::string & msg_type, - const json & data, double timeout_sec) { + const json & data, double /* timeout_sec */) { try { - // Convert JSON data to string for ros2 topic pub - // Note: data.dump() produces JSON format, but ROS 2 CLI accepts JSON - // as valid YAML (JSON is a subset of YAML 1.2) - std::string yaml_data = data.dump(); + // Step 1: Get or create cached publisher + auto publisher = get_or_create_publisher(topic_path, msg_type); - // TODO(mfaferek93) #32: Check timeout command availability - // GNU coreutils 'timeout' may not be available on all systems (BSD, containers) - // Should check in constructor or provide fallback mechanism - std::ostringstream cmd; - cmd << "timeout " << static_cast(std::ceil(timeout_sec)) << "s " << "ros2 topic pub --once -w 0 " - << ROS2CLIWrapper::escape_shell_arg(topic_path) << " " << ROS2CLIWrapper::escape_shell_arg(msg_type) << " " - << ROS2CLIWrapper::escape_shell_arg(yaml_data); + // Step 2: Serialize JSON to CDR format + rclcpp::SerializedMessage serialized_msg = serializer_->serialize(msg_type, data); - RCLCPP_INFO(node_->get_logger(), "Executing: %s", cmd.str().c_str()); + // Step 3: Publish serialized message + publisher->publish(serialized_msg); - std::string output = cli_wrapper_->exec(cmd.str()); - - RCLCPP_INFO(node_->get_logger(), "Published to topic '%s' with type '%s'", topic_path.c_str(), msg_type.c_str()); + RCLCPP_INFO(node_->get_logger(), "Published to topic '%s' with type '%s' (native)", topic_path.c_str(), + msg_type.c_str()); json result = {{"topic", topic_path}, {"type", msg_type}, @@ -90,6 +110,15 @@ json DataAccessManager::publish_to_topic(const std::string & topic_path, const s .count()}}; return result; + + } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + RCLCPP_ERROR(node_->get_logger(), "Unknown message type '%s': %s", msg_type.c_str(), e.what()); + throw std::runtime_error("Unknown message type '" + msg_type + "': " + e.what()); + + } catch (const ros2_medkit_serialization::SerializationError & e) { + RCLCPP_ERROR(node_->get_logger(), "Failed to serialize message for '%s': %s", topic_path.c_str(), e.what()); + throw std::runtime_error("Failed to serialize message for '" + topic_path + "': " + e.what()); + } catch (const std::exception & e) { RCLCPP_ERROR(node_->get_logger(), "Failed to publish to topic '%s': %s", topic_path.c_str(), e.what()); throw std::runtime_error("Failed to publish to topic '" + topic_path + "': " + e.what()); diff --git a/src/ros2_medkit_gateway/src/discovery_manager.cpp b/src/ros2_medkit_gateway/src/discovery_manager.cpp index afb95eba0..62ab452bc 100644 --- a/src/ros2_medkit_gateway/src/discovery_manager.cpp +++ b/src/ros2_medkit_gateway/src/discovery_manager.cpp @@ -191,10 +191,15 @@ std::vector DiscoveryManager::discover_services() { info.type = types.empty() ? "" : types[0]; // Enrich with schema info if TypeIntrospection is available + // Service types: pkg/srv/Type -> Request: pkg/srv/Type_Request, Response: pkg/srv/Type_Response if (type_introspection_ && !info.type.empty()) { try { - auto type_info = type_introspection_->get_type_info(info.type); - info.type_info = type_info.schema; + json type_info_json; + auto request_info = type_introspection_->get_type_info(info.type + "_Request"); + auto response_info = type_introspection_->get_type_info(info.type + "_Response"); + type_info_json["request"] = request_info.schema; + type_info_json["response"] = response_info.schema; + info.type_info = type_info_json; } catch (const std::exception & e) { RCLCPP_DEBUG(node_->get_logger(), "Could not get schema for service '%s': %s", info.type.c_str(), e.what()); } @@ -251,10 +256,17 @@ std::vector DiscoveryManager::discover_actions() { } // Enrich with schema info if TypeIntrospection is available + // Action types: pkg/action/Type -> Goal: pkg/action/Type_Goal, etc. if (type_introspection_ && !info.type.empty()) { try { - auto type_info = type_introspection_->get_type_info(info.type); - info.type_info = type_info.schema; + json type_info_json; + auto goal_info = type_introspection_->get_type_info(info.type + "_Goal"); + auto result_info = type_introspection_->get_type_info(info.type + "_Result"); + auto feedback_info = type_introspection_->get_type_info(info.type + "_Feedback"); + type_info_json["goal"] = goal_info.schema; + type_info_json["result"] = result_info.schema; + type_info_json["feedback"] = feedback_info.schema; + info.type_info = type_info_json; } catch (const std::exception & e) { RCLCPP_DEBUG(node_->get_logger(), "Could not get schema for action '%s': %s", info.type.c_str(), e.what()); } diff --git a/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp b/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp index a2bdded23..dcc5cdc36 100644 --- a/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp +++ b/src/ros2_medkit_gateway/src/http/handlers/operation_handlers.cpp @@ -72,13 +72,19 @@ void OperationHandlers::handle_list_operations(const httplib::Request & req, htt for (const auto & svc : services) { json svc_json = {{"name", svc.name}, {"path", svc.full_path}, {"type", svc.type}, {"kind", "service"}}; - // Try to get schema for service type + // Build type_info with request/response schemas for services try { - auto type_info = type_introspection->get_type_info(svc.type); - svc_json["type_info"] = {{"schema", type_info.schema}, {"default_value", type_info.default_value}}; + json type_info_json; + // Service types: pkg/srv/Type -> Request: pkg/srv/Type_Request, Response: pkg/srv/Type_Response + auto request_info = type_introspection->get_type_info(svc.type + "_Request"); + auto response_info = type_introspection->get_type_info(svc.type + "_Response"); + type_info_json["request"] = request_info.schema; + type_info_json["response"] = response_info.schema; + svc_json["type_info"] = type_info_json; } catch (const std::exception & e) { RCLCPP_DEBUG(HandlerContext::logger(), "Could not get type info for service '%s': %s", svc.type.c_str(), e.what()); + svc_json["type_info"] = json::object(); } operations.push_back(svc_json); @@ -87,13 +93,21 @@ void OperationHandlers::handle_list_operations(const httplib::Request & req, htt for (const auto & act : actions) { json act_json = {{"name", act.name}, {"path", act.full_path}, {"type", act.type}, {"kind", "action"}}; - // Try to get schema for action type + // Build type_info with goal/result/feedback schemas for actions try { - auto type_info = type_introspection->get_type_info(act.type); - act_json["type_info"] = {{"schema", type_info.schema}, {"default_value", type_info.default_value}}; + json type_info_json; + // Action types: pkg/action/Type -> Goal: pkg/action/Type_Goal, etc. + auto goal_info = type_introspection->get_type_info(act.type + "_Goal"); + auto result_info = type_introspection->get_type_info(act.type + "_Result"); + auto feedback_info = type_introspection->get_type_info(act.type + "_Feedback"); + type_info_json["goal"] = goal_info.schema; + type_info_json["result"] = result_info.schema; + type_info_json["feedback"] = feedback_info.schema; + act_json["type_info"] = type_info_json; } catch (const std::exception & e) { RCLCPP_DEBUG(HandlerContext::logger(), "Could not get type info for action '%s': %s", act.type.c_str(), e.what()); + act_json["type_info"] = json::object(); } operations.push_back(act_json); diff --git a/src/ros2_medkit_gateway/src/native_topic_sampler.cpp b/src/ros2_medkit_gateway/src/native_topic_sampler.cpp index ff020118d..423d666bb 100644 --- a/src/ros2_medkit_gateway/src/native_topic_sampler.cpp +++ b/src/ros2_medkit_gateway/src/native_topic_sampler.cpp @@ -15,18 +15,20 @@ #include "ros2_medkit_gateway/native_topic_sampler.hpp" #include +#include #include #include #include #include #include +#include #include #include #include #include -#include "ros2_medkit_gateway/output_parser.hpp" -#include "ros2_medkit_gateway/ros2_cli_wrapper.hpp" +#include "ros2_medkit_serialization/json_serializer.hpp" +#include "ros2_medkit_serialization/serialization_error.hpp" namespace ros2_medkit_gateway { @@ -113,11 +115,12 @@ QosProfile qos_to_profile(const rclcpp::QoS & qos) { } // namespace -NativeTopicSampler::NativeTopicSampler(rclcpp::Node * node) : node_(node) { +NativeTopicSampler::NativeTopicSampler(rclcpp::Node * node) + : node_(node), serializer_(std::make_shared()) { if (!node_) { throw std::invalid_argument("NativeTopicSampler requires a valid node pointer"); } - RCLCPP_INFO(node_->get_logger(), "NativeTopicSampler initialized"); + RCLCPP_INFO(node_->get_logger(), "NativeTopicSampler initialized with native serialization"); } std::vector NativeTopicSampler::discover_all_topics() { @@ -195,11 +198,6 @@ std::string NativeTopicSampler::get_topic_type(const std::string & topic_name) { return ""; } -json NativeTopicSampler::parse_message_yaml(const std::string & yaml_str) { - OutputParser parser; - return parser.parse_yaml(yaml_str); -} - TopicSampleResult NativeTopicSampler::sample_topic(const std::string & topic_name, double timeout_sec) { RCLCPP_DEBUG(node_->get_logger(), "sample_topic: START topic='%s', timeout=%.2f", topic_name.c_str(), timeout_sec); TopicSampleResult result; @@ -235,23 +233,8 @@ TopicSampleResult NativeTopicSampler::sample_topic(const std::string & topic_nam return result; } - // Create a generic subscription to receive one message - // We use serialized messages + ros2 topic echo approach via CLI for now, - // as GenericSubscription requires type support which needs runtime loading. - // TODO(mfaferek93): Implement true generic subscription with rosidl_typesupport_introspection_cpp - - // For now, use a hybrid approach: - // 1. Fast metadata via native API (already done above) - // 2. Use ros2 topic echo with very short timeout only for active topics - - // We fall back to CLI sampling but with a much shorter timeout since we know - // there are publishers. This still provides significant UX improvement: - // - Topics without publishers return immediately (no CLI call) - // - Topics with publishers use CLI but we've eliminated ~50-90% of CLI calls - + // Use native GenericSubscription for sampling try { - // Use ros2 topic echo --once with native --timeout flag - // timeout_sec is passed from caller (default 1s for single topic, configurable) constexpr double kDefaultTimeoutSec = 1.0; if (timeout_sec <= 0) { RCLCPP_WARN(node_->get_logger(), @@ -259,58 +242,82 @@ TopicSampleResult NativeTopicSampler::sample_topic(const std::string & topic_nam topic_name.c_str(), kDefaultTimeoutSec); timeout_sec = kDefaultTimeoutSec; } - int timeout_int = static_cast(std::ceil(timeout_sec)); - std::ostringstream cmd; - cmd << "ros2 topic echo " << ROS2CLIWrapper::escape_shell_arg(topic_name) << " --once --no-arr --timeout " - << timeout_int << " 2>/dev/null"; - RCLCPP_DEBUG(node_->get_logger(), "sample_topic: executing CLI: %s", cmd.str().c_str()); + // Set up one-shot subscription with promise/future + std::promise message_promise; + auto message_future = message_promise.get_future(); + std::atomic received{false}; + + // Choose QoS based on publisher QoS (best effort for sensor data, reliable for others) + rclcpp::QoS qos = rclcpp::SensorDataQoS(); // Best effort, keep last 1 + + // Try to match publisher QoS if available + if (!result.publishers.empty()) { + const auto & pub_qos = result.publishers[0].qos; + if (pub_qos.reliability == "reliable") { + qos = rclcpp::QoS(10); // Reliable, keep last 10 + } + } - // Execute command with RAII pipe management to prevent resource leaks - auto pipe_closer = [](FILE * f) { - if (f) { - pclose(f); + // Create generic subscription + // NOLINTNEXTLINE(performance-unnecessary-value-param) - GenericSubscription requires value type in callback + auto callback = [&message_promise, &received](std::shared_ptr msg) { + // Only process first message (one-shot) + bool expected = false; + if (received.compare_exchange_strong(expected, true)) { + message_promise.set_value(*msg); } }; - std::unique_ptr pipe(popen(cmd.str().c_str(), "r"), pipe_closer); - if (!pipe) { - RCLCPP_WARN(node_->get_logger(), "sample_topic: Failed to execute CLI command for topic '%s'", - topic_name.c_str()); + rclcpp::GenericSubscription::SharedPtr subscription; + try { + subscription = node_->create_generic_subscription(topic_name, info->type, qos, callback); + } catch (const std::exception & e) { + RCLCPP_WARN(node_->get_logger(), "Failed to create subscription for '%s': %s", topic_name.c_str(), e.what()); result.has_data = false; return result; } - RCLCPP_DEBUG(node_->get_logger(), "sample_topic: CLI started, reading output..."); + RCLCPP_DEBUG(node_->get_logger(), "sample_topic: Created GenericSubscription for '%s'", topic_name.c_str()); - // Use larger buffer for better performance with large messages - std::array buffer; - std::string output; - output.reserve(4096); // Pre-allocate for typical message sizes + // Spin with timeout + const auto timeout = std::chrono::duration(timeout_sec); + const auto start_time = std::chrono::steady_clock::now(); - while (fgets(buffer.data(), static_cast(buffer.size()), pipe.get()) != nullptr) { - output += buffer.data(); - } - - // Get exit code - release pipe to get exit code before RAII cleanup - int exit_code = pclose(pipe.release()); + while (!received.load()) { + auto elapsed = std::chrono::steady_clock::now() - start_time; + if (elapsed >= timeout) { + RCLCPP_DEBUG(node_->get_logger(), "sample_topic: Timeout waiting for message on '%s'", topic_name.c_str()); + result.has_data = false; + return result; + } - RCLCPP_DEBUG(node_->get_logger(), "sample_topic: CLI finished, exit_code=%d, output_len=%zu", exit_code, - output.length()); + // Spin with small timeout to allow checking received flag + rclcpp::spin_some(node_->get_node_base_interface()); - // Check for success (exit code 0 and non-empty output without warnings) - if (exit_code == 0 && !output.empty() && output.find("WARNING") == std::string::npos) { - result.data = parse_message_yaml(output); - // Check if result.data has a value AND is not null - if (result.data && !result.data->is_null()) { - result.has_data = true; - RCLCPP_DEBUG(node_->get_logger(), "sample_topic: Sampled data from topic '%s'", topic_name.c_str()); + if (!received.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - } else { - RCLCPP_DEBUG(node_->get_logger(), "sample_topic: No data received from topic '%s' (exit_code=%d)", - topic_name.c_str(), exit_code); + } + + // Deserialize message using JsonSerializer + try { + auto serialized_msg = message_future.get(); + result.data = serializer_->deserialize(info->type, serialized_msg); + result.has_data = true; + RCLCPP_DEBUG(node_->get_logger(), "sample_topic: Sampled data from topic '%s'", topic_name.c_str()); + } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + RCLCPP_WARN(node_->get_logger(), "Unknown type '%s' for topic '%s': %s", info->type.c_str(), topic_name.c_str(), + e.what()); + result.has_data = false; + } catch (const ros2_medkit_serialization::SerializationError & e) { + RCLCPP_WARN(node_->get_logger(), "Failed to deserialize message from '%s': %s", topic_name.c_str(), e.what()); + result.has_data = false; + } catch (const std::exception & e) { + RCLCPP_WARN(node_->get_logger(), "Failed to process message from '%s': %s", topic_name.c_str(), e.what()); result.has_data = false; } + } catch (const std::exception & e) { RCLCPP_WARN(node_->get_logger(), "Exception sampling topic '%s': %s", topic_name.c_str(), e.what()); result.has_data = false; diff --git a/src/ros2_medkit_gateway/src/operation_manager.cpp b/src/ros2_medkit_gateway/src/operation_manager.cpp index a71b3c5db..004782d0f 100644 --- a/src/ros2_medkit_gateway/src/operation_manager.cpp +++ b/src/ros2_medkit_gateway/src/operation_manager.cpp @@ -14,16 +14,16 @@ #include "ros2_medkit_gateway/operation_manager.hpp" -#include - #include -#include #include #include #include #include -#include "ros2_medkit_gateway/output_parser.hpp" +#include "ros2_medkit_serialization/json_serializer.hpp" +#include "ros2_medkit_serialization/serialization_error.hpp" +#include "ros2_medkit_serialization/service_action_types.hpp" +#include "ros2_medkit_serialization/type_cache.hpp" namespace ros2_medkit_gateway { @@ -36,14 +36,11 @@ constexpr int kDefaultServiceCallTimeoutSec = 10; OperationManager::OperationManager(rclcpp::Node * node, DiscoveryManager * discovery_manager) : node_(node) , discovery_manager_(discovery_manager) - , cli_wrapper_(std::make_unique()) + , rng_(std::random_device{}()) + , serializer_(std::make_shared()) , service_call_timeout_sec_( static_cast(node->declare_parameter("service_call_timeout_sec", kDefaultServiceCallTimeoutSec))) { - if (!cli_wrapper_->is_command_available("ros2")) { - RCLCPP_WARN(node_->get_logger(), "ROS 2 CLI not found, service calls may not be available"); - } - - RCLCPP_INFO(node_->get_logger(), "OperationManager initialized"); + RCLCPP_INFO(node_->get_logger(), "OperationManager initialized with native serialization"); } bool OperationManager::is_valid_message_type(const std::string & type) { @@ -72,179 +69,39 @@ bool OperationManager::is_valid_uuid_hex(const std::string & uuid_hex) { }); } -std::string OperationManager::json_to_yaml(const json & j) { - // Convert JSON to YAML format for ros2 CLI commands - // ros2 action/service commands expect YAML format: {key: value} not {"key": value} - std::function convert = [&convert](const json & val) -> std::string { - if (val.is_object()) { - std::ostringstream ss; - ss << "{"; - bool first = true; - for (auto it = val.begin(); it != val.end(); ++it) { - if (!first) { - ss << ", "; - } - first = false; - ss << it.key() << ": " << convert(it.value()); - } - ss << "}"; - return ss.str(); - } else if (val.is_array()) { - std::ostringstream ss; - ss << "["; - bool first = true; - for (const auto & item : val) { - if (!first) { - ss << ", "; - } - first = false; - ss << convert(item); - } - ss << "]"; - return ss.str(); - } else if (val.is_string()) { - // Quote strings that contain special YAML characters or are empty - std::string s = val.get(); - // Empty strings must be quoted, otherwise YAML interprets them as null/None - if (s.empty() || s.find_first_of(":{}[],\"'") != std::string::npos) { - return "'" + s + "'"; - } - return s; - } else { - return val.dump(); // numbers, bools, null - } - }; - - return convert(j); +std::string OperationManager::make_client_key(const std::string & service_path, const std::string & service_type) { + return service_path + "|" + service_type; } -json OperationManager::parse_service_response(const std::string & yaml_output) { - try { - // ros2 service call output format: - // requester: making request: ... - // response: - // std_srvs.srv.Trigger_Response(success=True, message='...') - // - // Or for newer ROS2 (YAML format): - // response: - // success: true - // message: '...' - - // Find the response section - std::string response_section; - size_t response_pos = yaml_output.find("response:"); - if (response_pos != std::string::npos) { - // Skip "response:" and any newline - size_t start = response_pos + 9; // length of "response:" - while (start < yaml_output.size() && (yaml_output[start] == '\n' || yaml_output[start] == ' ')) { - start++; - } - response_section = yaml_output.substr(start); - // Trim trailing whitespace and newlines - while (!response_section.empty() && - (response_section.back() == '\n' || response_section.back() == ' ' || response_section.back() == '\r')) { - response_section.pop_back(); - } - } else { - // Try parsing the whole output - response_section = yaml_output; +rclcpp::GenericClient::SharedPtr OperationManager::get_or_create_service_client(const std::string & service_path, + const std::string & service_type) { + const std::string key = make_client_key(service_path, service_type); + + // Try read lock first (fast path) + { + std::shared_lock lock(clients_mutex_); + auto it = generic_clients_.find(key); + if (it != generic_clients_.end()) { + return it->second; } + } - // Check if it's in Python repr format: TypeName(field=value, ...) - // Example: std_srvs.srv.Trigger_Response(success=True, message='...') - static const std::regex repr_regex(R"(^[\w.]+\((.*)\)$)"); - std::smatch match; - if (std::regex_match(response_section, match, repr_regex)) { - // Parse Python repr format - std::string fields_str = match[1].str(); - json result = json::object(); - - // Parse field=value pairs - // Handle nested parentheses and quoted strings - size_t pos = 0; - while (pos < fields_str.length()) { - // Skip whitespace - while (pos < fields_str.length() && (fields_str[pos] == ' ' || fields_str[pos] == ',')) { - pos++; - } - if (pos >= fields_str.length()) { - break; - } + // Need to create - take exclusive lock + std::unique_lock lock(clients_mutex_); - // Find field name (up to '=') - size_t eq_pos = fields_str.find('=', pos); - if (eq_pos == std::string::npos) { - break; - } - std::string field_name = fields_str.substr(pos, eq_pos - pos); - pos = eq_pos + 1; - - // Parse value - std::string value_str; - if (pos < fields_str.length() && fields_str[pos] == '\'') { - // Quoted string - pos++; // Skip opening quote - size_t end_quote = pos; - while (end_quote < fields_str.length()) { - if (fields_str[end_quote] == '\'') { - // Check if it's escaped - if (end_quote > 0 && fields_str[end_quote - 1] == '\\') { - end_quote++; - continue; - } - break; - } - end_quote++; - } - value_str = fields_str.substr(pos, end_quote - pos); - pos = end_quote + 1; - result[field_name] = value_str; - } else { - // Non-quoted value (bool, number, etc.) - size_t comma_pos = fields_str.find(',', pos); - size_t paren_pos = fields_str.find(')', pos); - size_t end_pos = std::min(comma_pos, paren_pos); - if (end_pos == std::string::npos) { - end_pos = fields_str.length(); - } - value_str = fields_str.substr(pos, end_pos - pos); - pos = end_pos; - - // Convert to appropriate JSON type - if (value_str == "True" || value_str == "true") { - result[field_name] = true; - } else if (value_str == "False" || value_str == "false") { - result[field_name] = false; - } else if (value_str == "None" || value_str == "null") { - result[field_name] = nullptr; - } else { - // Try to parse as number - try { - if (value_str.find('.') != std::string::npos) { - result[field_name] = std::stod(value_str); - } else { - result[field_name] = std::stoll(value_str); - } - } catch (...) { - result[field_name] = value_str; - } - } - } - } - return result; - } + // Double-check (another thread might have created it) + auto it = generic_clients_.find(key); + if (it != generic_clients_.end()) { + return it->second; + } - // Try to parse as YAML (for newer ROS2 format) - YAML::Node yaml_node = YAML::Load(response_section); + // Create new client + auto client = node_->create_generic_client(service_path, service_type); + generic_clients_[key] = client; - // Convert YAML to JSON using shared utility - return OutputParser::yaml_to_json(yaml_node); + RCLCPP_DEBUG(node_->get_logger(), "Created generic client for %s (%s)", service_path.c_str(), service_type.c_str()); - } catch (const std::exception & e) { - RCLCPP_WARN(node_->get_logger(), "Failed to parse service response: %s", e.what()); - // Return raw output as string - return json{{"raw_response", yaml_output}}; - } + return client; } ServiceCallResult OperationManager::call_service(const std::string & service_path, const std::string & service_type, @@ -252,39 +109,120 @@ ServiceCallResult OperationManager::call_service(const std::string & service_pat ServiceCallResult result; try { - // Build ros2 service call command with timeout - // -1 = single call (don't wait for response if service unavailable) - // timeout = overall timeout to prevent hanging (configurable via service_call_timeout_sec param) - std::ostringstream cmd; - cmd << "timeout " << service_call_timeout_sec_ << " ros2 service call " - << ROS2CLIWrapper::escape_shell_arg(service_path) << " " << ROS2CLIWrapper::escape_shell_arg(service_type); - - // Add request data if not empty - if (!request.empty() && !request.is_null()) { - cmd << " " << ROS2CLIWrapper::escape_shell_arg(json_to_yaml(request)); + using ros2_medkit_serialization::ServiceActionTypes; + + // Step 1: Get or create cached client + auto client = get_or_create_service_client(service_path, service_type); + + // Step 2: Wait for service availability + if (!client->wait_for_service(std::chrono::seconds(5))) { + result.success = false; + result.error_message = "Service not available: " + service_path; + return result; + } + + // Step 3: Build request/response type names + std::string request_type = ServiceActionTypes::get_service_request_type(service_type); + std::string response_type = ServiceActionTypes::get_service_response_type(service_type); + + // Step 4: Create request message from JSON + json request_data = request.empty() || request.is_null() ? json::object() : request; + + // Get defaults for request type and merge with provided data + try { + json defaults = serializer_->get_defaults(request_type); + // Merge: request overrides defaults + for (auto it = request_data.begin(); it != request_data.end(); ++it) { + defaults[it.key()] = it.value(); + } + request_data = defaults; + } catch (const ros2_medkit_serialization::TypeNotFoundError &) { + // If we can't get defaults, just use provided data } + // Convert JSON to ROS message (deserialized form, not CDR) + // Note: GenericClient expects void* pointing to deserialized message structure + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + RosMessage_Cpp ros_request = serializer_->from_json(request_type, request_data); + RCLCPP_INFO(node_->get_logger(), "Calling service: %s (type: %s)", service_path.c_str(), service_type.c_str()); - std::string output = cli_wrapper_->exec(cmd.str()); + // Step 5: Send request using GenericClient (expects void* to deserialized message) + auto future_and_id = client->async_send_request(ros_request.data); - RCLCPP_DEBUG(node_->get_logger(), "Service call output: %s", output.c_str()); + // Step 6: Wait for response with timeout + auto timeout = std::chrono::seconds(service_call_timeout_sec_); + auto future_status = future_and_id.wait_for(timeout); - result.success = true; - result.response = parse_service_response(output); + if (future_status != std::future_status::ready) { + // Clean up pending request on timeout + client->remove_pending_request(future_and_id.request_id); + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + result.success = false; + result.error_message = + "Service call timed out (" + std::to_string(service_call_timeout_sec_) + "s): " + service_path; + return result; + } - } catch (const std::exception & e) { - std::string error_msg = e.what(); - // Check if it was a timeout - if (error_msg.find("exit code 124") != std::string::npos) { - RCLCPP_WARN(node_->get_logger(), "Service call timed out for '%s'", service_path.c_str()); - result.error_message = "Service call timed out (" + std::to_string(service_call_timeout_sec_) + - "s). The service may be unavailable or slow to respond."; + // Clean up request message after sending + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + + // Step 7: Get response and deserialize + auto response_ptr = future_and_id.get(); + + // The response is a void* pointing to the deserialized response message data + // We need to convert it back to JSON via the serializer + // GenericClient returns a SharedResponse (shared_ptr) + // The data is already deserialized by rclcpp internally + + // For now, we need to serialize the response back to get JSON + // This is a bit roundabout but works with the current GenericClient API + // TODO: Optimize by accessing response data directly when possible + + if (response_ptr != nullptr) { + // Create a temporary serialized message to deserialize from + // The response_ptr points to deserialized message data + // We need to re-serialize it to use our deserializer + + // Alternative approach: use the type info to read fields directly + // For now, try to get the response via YAML (simpler but less efficient) + + // Actually, GenericClient gives us a deserialized message already + // We can use dynmsg to convert it to JSON directly + try { + auto type_info = ros2_medkit_serialization::TypeCache::instance().get_message_type_info(response_type); + if (type_info != nullptr) { + result.response = serializer_->to_json(type_info, response_ptr.get()); + result.success = true; + RCLCPP_DEBUG(node_->get_logger(), "Service call succeeded: %s", result.response.dump().c_str()); + } else { + result.success = false; + result.error_message = "Unknown response type: " + response_type; + } + } catch (const std::exception & e) { + result.success = false; + result.error_message = std::string("Failed to deserialize response: ") + e.what(); + } } else { - RCLCPP_ERROR(node_->get_logger(), "Service call failed for '%s': %s", service_path.c_str(), e.what()); - result.error_message = error_msg; + result.success = false; + result.error_message = "Service returned null response"; } + + } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + RCLCPP_ERROR(node_->get_logger(), "Type not found for service '%s': %s", service_path.c_str(), e.what()); result.success = false; + result.error_message = std::string("Unknown service type: ") + e.what(); + + } catch (const ros2_medkit_serialization::SerializationError & e) { + RCLCPP_ERROR(node_->get_logger(), "Serialization failed for service '%s': %s", service_path.c_str(), e.what()); + result.success = false; + result.error_message = std::string("Invalid request format: ") + e.what(); + + } catch (const std::exception & e) { + std::string error_msg = e.what(); + RCLCPP_ERROR(node_->get_logger(), "Service call failed for '%s': %s", service_path.c_str(), e.what()); + result.success = false; + result.error_message = error_msg; } return result; @@ -360,72 +298,70 @@ std::string action_status_to_string(ActionGoalStatus status) { } } -std::string OperationManager::uuid_to_yaml_array(const std::string & uuid_hex) { - // Convert hex string to YAML array of byte values [b1, b2, ...] - std::ostringstream ss; - ss << "["; - for (size_t i = 0; i < uuid_hex.length() && i < kUuidHexLength; i += 2) { - if (i > 0) { - ss << ", "; - } - int byte_val = std::stoi(uuid_hex.substr(i, 2), nullptr, 16); - ss << byte_val; +std::array OperationManager::generate_uuid() { + std::lock_guard lock(rng_mutex_); + std::array uuid; + std::uniform_int_distribution dist(0, 255); + for (auto & byte : uuid) { + byte = static_cast(dist(rng_)); } - ss << "]"; - return ss.str(); + // Set version 4 (random UUID) + uuid[6] = (uuid[6] & 0x0f) | 0x40; + // Set variant bits + uuid[8] = (uuid[8] & 0x3f) | 0x80; + return uuid; } -ActionSendGoalResult OperationManager::parse_send_goal_cli_output(const std::string & output) { - ActionSendGoalResult result; - result.success = false; - result.goal_accepted = false; - - // Look for "Goal accepted with ID: " - static const std::regex goal_id_regex(R"(Goal accepted with ID:\s*([a-f0-9]+))"); - std::smatch match; - if (std::regex_search(output, match, goal_id_regex)) { - result.goal_id = match[1].str(); - result.goal_accepted = true; - result.success = true; +json OperationManager::uuid_bytes_to_json_array(const std::array & uuid) { + json array = json::array(); + for (auto byte : uuid) { + array.push_back(static_cast(byte)); } + return array; +} - // Check for rejection - if (output.find("Goal rejected") != std::string::npos) { - result.success = true; // Command worked, but goal was rejected - result.goal_accepted = false; - result.error_message = "Goal rejected by action server"; +json OperationManager::uuid_hex_to_json_array(const std::string & uuid_hex) { + json array = json::array(); + for (size_t i = 0; i + 1 < uuid_hex.length() && i < kUuidHexLength; i += 2) { + int byte_val = std::stoi(uuid_hex.substr(i, 2), nullptr, 16); + array.push_back(byte_val); } + return array; +} - // Check for server not available - if (output.find("Action server is not available") != std::string::npos || - output.find("not available after waiting") != std::string::npos) { - result.success = false; - result.error_message = "Action server not available"; +OperationManager::ActionClientSet & OperationManager::get_or_create_action_clients(const std::string & action_path, + const std::string & action_type) { + std::unique_lock lock(clients_mutex_); + + auto it = action_clients_.find(action_path); + if (it != action_clients_.end()) { + return it->second; } - return result; -} + using namespace ros2_medkit_serialization; -ActionCancelResult OperationManager::parse_cancel_output(const std::string & output) { - ActionCancelResult result; - result.success = true; - result.return_code = 0; // ERROR_NONE - - // Check output for success/failure - if (output.find("Cancel request accepted") != std::string::npos || output.find("canceling") != std::string::npos) { - result.return_code = 0; - } else if (output.find("not found") != std::string::npos || output.find("unknown") != std::string::npos) { - result.return_code = 2; // ERROR_UNKNOWN_GOAL_ID - result.error_message = "Unknown goal ID"; - } else if (output.find("rejected") != std::string::npos) { - result.return_code = 1; // ERROR_REJECTED - result.error_message = "Cancel request rejected"; - } else if (output.find("terminated") != std::string::npos || output.find("already finished") != std::string::npos) { - result.return_code = 3; // ERROR_GOAL_TERMINATED - result.error_message = "Goal already terminated"; - } + ActionClientSet clients; + clients.action_type = action_type; - return result; + // Send goal service: {action}/_action/send_goal + std::string send_goal_service = action_path + "/_action/send_goal"; + std::string send_goal_type = ServiceActionTypes::get_action_send_goal_service_type(action_type); + clients.send_goal_client = node_->create_generic_client(send_goal_service, send_goal_type); + + // Get result service: {action}/_action/get_result + std::string get_result_service = action_path + "/_action/get_result"; + std::string get_result_type = ServiceActionTypes::get_action_get_result_service_type(action_type); + clients.get_result_client = node_->create_generic_client(get_result_service, get_result_type); + + // Cancel goal service (standard type for all actions) + std::string cancel_service = action_path + "/_action/cancel_goal"; + clients.cancel_goal_client = node_->create_generic_client(cancel_service, "action_msgs/srv/CancelGoal"); + + RCLCPP_DEBUG(node_->get_logger(), "Created action clients for %s (type: %s)", action_path.c_str(), + action_type.c_str()); + + action_clients_[action_path] = std::move(clients); + return action_clients_[action_path]; } void OperationManager::track_goal(const std::string & goal_id, const std::string & action_path, @@ -444,56 +380,91 @@ void OperationManager::track_goal(const std::string & goal_id, const std::string ActionSendGoalResult OperationManager::send_action_goal(const std::string & action_path, const std::string & action_type, const json & goal) { ActionSendGoalResult result; + result.success = false; + result.goal_accepted = false; try { - // Use ros2 action send_goal with short timeout (3s) - // This is enough for discovery + goal acceptance, but not for long-running actions - // The action continues running in background after we get the goal_id - std::ostringstream cmd; - cmd << "ros2 action send_goal " << ROS2CLIWrapper::escape_shell_arg(action_path) << " " - << ROS2CLIWrapper::escape_shell_arg(action_type); - - // Add goal data - if (!goal.empty() && !goal.is_null()) { - cmd << " " << ROS2CLIWrapper::escape_shell_arg(json_to_yaml(goal)); - } else { - cmd << " '{}'"; + using namespace ros2_medkit_serialization; + + // Step 1: Get or create action clients + auto & clients = get_or_create_action_clients(action_path, action_type); + + // Step 2: Wait for send_goal service + if (!clients.send_goal_client->wait_for_service(std::chrono::seconds(5))) { + result.error_message = "Action server not available: " + action_path; + return result; } - // Short timeout: enough for discovery + acceptance, action continues in background - cmd << " -t 3"; + // Step 3: Generate UUID for goal + auto uuid_bytes = generate_uuid(); + + // Step 4: Build SendGoal request + // The request has structure: {goal_id: {uuid: [...]}, goal: {...}} + json send_goal_request; + send_goal_request["goal_id"]["uuid"] = uuid_bytes_to_json_array(uuid_bytes); + send_goal_request["goal"] = goal.empty() || goal.is_null() ? json::object() : goal; + + // Step 5: Create request message from JSON and send + std::string request_type = ServiceActionTypes::get_action_send_goal_request_type(action_type); + + RCLCPP_INFO(node_->get_logger(), "SendGoal request type: %s, JSON: %s", request_type.c_str(), + send_goal_request.dump().c_str()); + + // Convert JSON to ROS message (deserialized form, not CDR) + // Note: GenericClient expects void* pointing to deserialized message structure + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + RosMessage_Cpp ros_request = serializer_->from_json(request_type, send_goal_request); RCLCPP_INFO(node_->get_logger(), "Sending action goal: %s (type: %s)", action_path.c_str(), action_type.c_str()); - std::string output = cli_wrapper_->exec(cmd.str()); - RCLCPP_DEBUG(node_->get_logger(), "Action send_goal output: %s", output.c_str()); + // Send using GenericClient (expects void* to deserialized message) + auto future_and_id = clients.send_goal_client->async_send_request(ros_request.data); - // Parse goal_id from CLI output - result = parse_send_goal_cli_output(output); + // Wait for response with timeout + auto timeout = std::chrono::seconds(service_call_timeout_sec_); + auto future_status = future_and_id.wait_for(timeout); - // Track the goal if accepted - if (result.goal_accepted && !result.goal_id.empty()) { - track_goal(result.goal_id, action_path, action_type); - RCLCPP_INFO(node_->get_logger(), "Action goal accepted with ID: %s", result.goal_id.c_str()); + if (future_status != std::future_status::ready) { + clients.send_goal_client->remove_pending_request(future_and_id.request_id); + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + result.error_message = "Send goal timed out"; + return result; + } - // Subscribe to status topic for real-time updates - subscribe_to_action_status(action_path); + // Clean up request message after sending + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); - // Check if action already completed (fast actions) or still running - if (output.find("Goal finished with status: SUCCEEDED") != std::string::npos) { - update_goal_status(result.goal_id, ActionGoalStatus::SUCCEEDED); - } else if (output.find("Goal finished with status: CANCELED") != std::string::npos) { - update_goal_status(result.goal_id, ActionGoalStatus::CANCELED); - } else if (output.find("Goal finished with status: ABORTED") != std::string::npos) { - update_goal_status(result.goal_id, ActionGoalStatus::ABORTED); - } else { - // Still executing (timed out waiting for result, which is expected) - update_goal_status(result.goal_id, ActionGoalStatus::EXECUTING); - } + // Step 6: Deserialize response + auto response_ptr = future_and_id.get(); + if (response_ptr == nullptr) { + result.error_message = "Send goal returned null response"; + return result; + } + + std::string response_type = ServiceActionTypes::get_action_send_goal_response_type(action_type); + auto type_info = ros2_medkit_serialization::TypeCache::instance().get_message_type_info(response_type); + if (type_info == nullptr) { + result.error_message = "Unknown response type: " + response_type; + return result; + } + json response = serializer_->to_json(type_info, response_ptr.get()); + + // Step 7: Extract goal_id and acceptance + result.success = true; + result.goal_accepted = response.value("accepted", false); + + if (result.goal_accepted) { + result.goal_id = uuid_bytes_to_hex(uuid_bytes); + track_goal(result.goal_id, action_path, action_type); + subscribe_to_action_status(action_path); + update_goal_status(result.goal_id, ActionGoalStatus::EXECUTING); + RCLCPP_INFO(node_->get_logger(), "Action goal accepted with ID: %s", result.goal_id.c_str()); + } else { + result.error_message = "Goal rejected by action server"; } } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Action send_goal failed for '%s': %s", action_path.c_str(), e.what()); + RCLCPP_ERROR(node_->get_logger(), "Send action goal failed for '%s': %s", action_path.c_str(), e.what()); result.success = false; result.error_message = e.what(); } @@ -550,40 +521,93 @@ ActionSendGoalResult OperationManager::send_component_action_goal(const std::str ActionCancelResult OperationManager::cancel_action_goal(const std::string & action_path, const std::string & goal_id) { ActionCancelResult result; + result.success = false; // Validate goal_id format if (!is_valid_uuid_hex(goal_id)) { - result.success = false; result.error_message = "Invalid goal_id format: must be 32 hex characters"; return result; } try { - // Build cancel command - ros2 action doesn't have a direct cancel by goal_id - // We need to use the internal service - std::ostringstream cmd; - cmd << "ros2 service call " << ROS2CLIWrapper::escape_shell_arg(action_path + "/_action/cancel_goal") - << " action_msgs/srv/CancelGoal "; + // Get the tracked goal to find action type + auto goal_info = get_tracked_goal(goal_id); + if (!goal_info) { + result.error_message = "Unknown goal_id - not tracked"; + return result; + } - // Convert goal_id hex string to UUID bytes array - // Format: {goal_info: {goal_id: {uuid: [b1, b2, ...]}}} - cmd << "'{goal_info: {goal_id: {uuid: " << uuid_to_yaml_array(goal_id) << "}}}'"; + // Get or create action clients (use tracked type) + auto & clients = get_or_create_action_clients(action_path, goal_info->action_type); + + if (!clients.cancel_goal_client->wait_for_service(std::chrono::seconds(2))) { + result.error_message = "Cancel service not available"; + return result; + } + + // Build cancel request: {goal_info: {goal_id: {uuid: [...]}}} + json cancel_request; + cancel_request["goal_info"]["goal_id"]["uuid"] = uuid_hex_to_json_array(goal_id); + + // Convert JSON to ROS message (deserialized form, not CDR) + // Note: GenericClient expects void* pointing to deserialized message structure + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + RosMessage_Cpp ros_request = serializer_->from_json("action_msgs/srv/CancelGoal_Request", cancel_request); RCLCPP_INFO(node_->get_logger(), "Canceling action goal: %s (goal_id: %s)", action_path.c_str(), goal_id.c_str()); - std::string output = cli_wrapper_->exec(cmd.str()); - RCLCPP_DEBUG(node_->get_logger(), "Action cancel output: %s", output.c_str()); + // Send using GenericClient (expects void* to deserialized message) + auto future_and_id = clients.cancel_goal_client->async_send_request(ros_request.data); + + auto future_status = future_and_id.wait_for(std::chrono::seconds(5)); + if (future_status != std::future_status::ready) { + clients.cancel_goal_client->remove_pending_request(future_and_id.request_id); + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + result.error_message = "Cancel request timed out"; + return result; + } + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); - result = parse_cancel_output(output); + auto response_ptr = future_and_id.get(); + if (response_ptr == nullptr) { + result.error_message = "Cancel returned null response"; + return result; + } - // Update goal status if cancel was accepted - if (result.success && result.return_code == 0) { + auto type_info = + ros2_medkit_serialization::TypeCache::instance().get_message_type_info("action_msgs/srv/CancelGoal_Response"); + if (type_info == nullptr) { + result.error_message = "Unknown response type: action_msgs/srv/CancelGoal_Response"; + return result; + } + json response = serializer_->to_json(type_info, response_ptr.get()); + + result.success = true; + result.return_code = static_cast(response.value("return_code", 0)); + + if (result.return_code == 0) { update_goal_status(goal_id, ActionGoalStatus::CANCELING); + RCLCPP_INFO(node_->get_logger(), "Cancel request accepted for goal: %s", goal_id.c_str()); + } else { + // Map return codes to error messages + switch (result.return_code) { + case 1: + result.error_message = "Cancel request rejected"; + break; + case 2: + result.error_message = "Unknown goal ID"; + break; + case 3: + result.error_message = "Goal already terminated"; + break; + default: + result.error_message = "Unknown cancel error"; + break; + } } } catch (const std::exception & e) { RCLCPP_ERROR(node_->get_logger(), "Action cancel failed for '%s': %s", action_path.c_str(), e.what()); - result.success = false; result.error_message = e.what(); } @@ -604,27 +628,55 @@ ActionGetResultResult OperationManager::get_action_result(const std::string & ac } try { - // Build get_result service call - // Service type is {action_type}_GetResult - std::string get_result_type = action_type; - // Replace /action/ with action:: for service type naming - // e.g., example_interfaces/action/Fibonacci -> example_interfaces/action/Fibonacci_GetResult - get_result_type += "_GetResult"; + using namespace ros2_medkit_serialization; + + // Get or create action clients + auto & clients = get_or_create_action_clients(action_path, action_type); + + if (!clients.get_result_client->wait_for_service(std::chrono::seconds(2))) { + result.error_message = "Get result service not available"; + return result; + } + + // Build request: {goal_id: {uuid: [...]}} + json get_result_request; + get_result_request["goal_id"]["uuid"] = uuid_hex_to_json_array(goal_id); - std::ostringstream cmd; - cmd << "ros2 service call " << ROS2CLIWrapper::escape_shell_arg(action_path + "/_action/get_result") << " " - << ROS2CLIWrapper::escape_shell_arg(get_result_type) << " "; + std::string request_type = ServiceActionTypes::get_action_get_result_request_type(action_type); - // Convert goal_id to UUID bytes array - cmd << "'{goal_id: {uuid: " << uuid_to_yaml_array(goal_id) << "}}'"; + // Convert JSON to ROS message (deserialized form, not CDR) + // Note: GenericClient expects void* pointing to deserialized message structure + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + RosMessage_Cpp ros_request = serializer_->from_json(request_type, get_result_request); RCLCPP_INFO(node_->get_logger(), "Getting action result: %s (goal_id: %s)", action_path.c_str(), goal_id.c_str()); - std::string output = cli_wrapper_->exec(cmd.str()); - RCLCPP_DEBUG(node_->get_logger(), "Action get_result output: %s", output.c_str()); + // Send using GenericClient (expects void* to deserialized message) + auto future_and_id = clients.get_result_client->async_send_request(ros_request.data); + + auto future_status = future_and_id.wait_for(std::chrono::seconds(service_call_timeout_sec_)); + if (future_status != std::future_status::ready) { + clients.get_result_client->remove_pending_request(future_and_id.request_id); + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + result.error_message = "Get result timed out"; + return result; + } + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + + auto response_ptr = future_and_id.get(); + if (response_ptr == nullptr) { + result.error_message = "Get result returned null response"; + return result; + } + + std::string response_type = ServiceActionTypes::get_action_get_result_response_type(action_type); + auto type_info = ros2_medkit_serialization::TypeCache::instance().get_message_type_info(response_type); + if (type_info == nullptr) { + result.error_message = "Unknown response type: " + response_type; + return result; + } + json response = serializer_->to_json(type_info, response_ptr.get()); - // Parse the response - json response = parse_service_response(output); result.success = true; // Extract status from response @@ -643,9 +695,11 @@ ActionGetResultResult OperationManager::get_action_result(const std::string & ac // Update local tracking update_goal_status(goal_id, result.status); + RCLCPP_INFO(node_->get_logger(), "Got action result for %s: status=%s", goal_id.c_str(), + action_status_to_string(result.status).c_str()); + } catch (const std::exception & e) { RCLCPP_ERROR(node_->get_logger(), "Get action result failed for '%s': %s", action_path.c_str(), e.what()); - result.success = false; result.error_message = e.what(); } @@ -684,9 +738,9 @@ std::vector OperationManager::get_goals_for_action(const std::st } } // Release lock before sorting - // Sort by last_update, newest first + // Sort by created_at, newest first (most recently created goal) std::sort(goals.begin(), goals.end(), [](const ActionGoalInfo & a, const ActionGoalInfo & b) { - return a.last_update > b.last_update; + return a.created_at > b.created_at; }); return goals; } diff --git a/src/ros2_medkit_gateway/src/output_parser.cpp b/src/ros2_medkit_gateway/src/output_parser.cpp deleted file mode 100644 index 23214e171..000000000 --- a/src/ros2_medkit_gateway/src/output_parser.cpp +++ /dev/null @@ -1,147 +0,0 @@ -// Copyright 2025 mfaferek93 -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ros2_medkit_gateway/output_parser.hpp" - -#include -#include - -namespace ros2_medkit_gateway { - -namespace { - -/** - * @brief Preprocess ros2 topic echo output to extract clean YAML - * - * ros2 topic echo can output warning messages like: - * - "A message was lost!!!" with count info - * - Other diagnostic messages - * - * This function extracts the last valid YAML document from the output. - */ -std::string preprocess_topic_echo_output(const std::string & raw_output) { - // Split by document separator "---" - std::vector documents; - std::istringstream stream(raw_output); - std::string line; - std::string current_doc; - - while (std::getline(stream, line)) { - // Skip warning lines from ros2 topic echo - if (line.find("A message was lost") != std::string::npos || line.find("total count") != std::string::npos) { - continue; - } - - // Document separator - if (line == "---") { - if (!current_doc.empty()) { - documents.push_back(current_doc); - current_doc.clear(); - } - continue; - } - - // Accumulate non-empty lines - if (!line.empty()) { - current_doc += line + "\n"; - } - } - - // Don't forget the last document if there's no trailing --- - if (!current_doc.empty()) { - documents.push_back(current_doc); - } - - // Return the last valid document (the actual message data) - if (documents.empty()) { - return ""; - } - - return documents.back(); -} - -} // namespace - -json OutputParser::parse_yaml(const std::string & yaml_str) { - try { - // Preprocess to remove warning messages and get clean YAML - std::string clean_yaml = preprocess_topic_echo_output(yaml_str); - - if (clean_yaml.empty()) { - throw std::runtime_error("No valid YAML content found in input"); - } - - std::vector docs = YAML::LoadAll(clean_yaml); - - if (docs.empty()) { - throw std::runtime_error("No YAML documents found in input"); - } - - // Parse only the first document (should be the message data after preprocessing) - return yaml_to_json(docs[0]); - } catch (const YAML::Exception & e) { - throw std::runtime_error("Failed to parse YAML: " + std::string(e.what())); - } -} - -json OutputParser::yaml_to_json(const YAML::Node & node) { - switch (node.Type()) { - case YAML::NodeType::Null: - return nullptr; - - case YAML::NodeType::Scalar: { - // Try bool first (most specific) - try { - return node.as(); - } catch (...) { - } - - // Try int first (preserves exact integer values) - try { - return node.as(); - } catch (...) { - } - - // Try double for floating-point numbers - try { - return node.as(); - } catch (...) { - } - - // Fallback to string - return node.as(); - } - - case YAML::NodeType::Sequence: { - json arr = json::array(); - for (const auto & item : node) { - arr.push_back(yaml_to_json(item)); - } - return arr; - } - - case YAML::NodeType::Map: { - json obj = json::object(); - for (const auto & pair : node) { - obj[pair.first.as()] = yaml_to_json(pair.second); - } - return obj; - } - - default: - return nullptr; - } -} - -} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/ros2_cli_wrapper.cpp b/src/ros2_medkit_gateway/src/ros2_cli_wrapper.cpp deleted file mode 100644 index e7be65372..000000000 --- a/src/ros2_medkit_gateway/src/ros2_cli_wrapper.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2025 mfaferek93 -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ros2_medkit_gateway/ros2_cli_wrapper.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -namespace ros2_medkit_gateway { - -// Custom deleter for FILE* from popen -struct PipeDeleter { - void operator()(FILE * fp) const { - if (fp) { - pclose(fp); - } - } -}; - -std::string ROS2CLIWrapper::exec(const std::string & command) { - std::array buffer; - std::string result; - - std::unique_ptr pipe(popen(command.c_str(), "r")); - - if (!pipe) { - throw std::runtime_error("Failed to execute command: " + command); - } - - while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) { - result += buffer.data(); - } - - // Check command exit status (release prevents double-close in deleter) - int exit_code = pclose(pipe.release()); - if (exit_code != 0) { - throw std::runtime_error("Command failed with exit code " + std::to_string(WEXITSTATUS(exit_code)) + ": " + - command); - } - - return result; -} - -bool ROS2CLIWrapper::is_command_available(const std::string & command) { - // Search in PATH for the command using access() instead of system() - // This avoids shell execution and potential command injection - const char * path_env = std::getenv("PATH"); - if (!path_env) { - return false; - } - - std::string path(path_env); - std::istringstream ss(path); - std::string dir; - - while (std::getline(ss, dir, ':')) { - if (dir.empty()) { - continue; - } - std::string full_path = std::string(dir).append("/").append(command); - if (access(full_path.c_str(), X_OK) == 0) { - return true; - } - } - - return false; -} - -// TODO(mfaferek93): Improve command injection protection -// Current: Single-quote escaping (reasonably safe, prevents shell expansion) -// Future options: -// 1. Add input validation: whitelist ROS 2 naming conventions (alphanumeric, /, _) -// 2. Reject topic names with suspicious characters -// 3. Use fork/exec or posix_spawn instead of shell execution (best solution) -std::string ROS2CLIWrapper::escape_shell_arg(const std::string & arg) { - std::string escaped = "'"; - for (char c : arg) { - if (c == '\'') { - escaped += "'\\''"; - } else { - escaped += c; - } - } - escaped += "'"; - return escaped; -} - -} // namespace ros2_medkit_gateway diff --git a/src/ros2_medkit_gateway/src/type_introspection.cpp b/src/ros2_medkit_gateway/src/type_introspection.cpp index 9cc2f4b87..8483e007a 100644 --- a/src/ros2_medkit_gateway/src/type_introspection.cpp +++ b/src/ros2_medkit_gateway/src/type_introspection.cpp @@ -17,91 +17,33 @@ #include #include -#include "ros2_medkit_gateway/output_parser.hpp" -#include "ros2_medkit_gateway/ros2_cli_wrapper.hpp" +#include "ros2_medkit_serialization/json_serializer.hpp" +#include "ros2_medkit_serialization/serialization_error.hpp" namespace ros2_medkit_gateway { -TypeIntrospection::TypeIntrospection(const std::string & scripts_path) - : scripts_path_(scripts_path) - , cli_wrapper_(std::make_unique()) - , output_parser_(std::make_unique()) { +TypeIntrospection::TypeIntrospection(const std::string & /* scripts_path */) + : serializer_(std::make_shared()) { + // scripts_path is deprecated and ignored - we use native serialization now } nlohmann::json TypeIntrospection::get_type_template(const std::string & type_name) { - // Call: ros2 interface proto {type_name} - std::string cmd = "ros2 interface proto " + ROS2CLIWrapper::escape_shell_arg(type_name); - std::string output = cli_wrapper_->exec(cmd); - - // The output is a quoted YAML string, e.g.: - // "header:\n stamp:\n sec: 0\n..." - // We need to remove the surrounding quotes and parse as YAML - - // Remove surrounding quotes if present - if (output.size() >= 2 && output.front() == '"' && output.back() == '"') { - output = output.substr(1, output.size() - 2); - } - - // Unescape common escape sequences (\n, \t, \r, \\, \") - std::string unescaped; - for (size_t i = 0; i < output.size(); ++i) { - if (i + 1 < output.size() && output[i] == '\\') { - char next = output[i + 1]; - switch (next) { - case 'n': - unescaped += '\n'; - ++i; - break; - case 't': - unescaped += '\t'; - ++i; - break; - case 'r': - unescaped += '\r'; - ++i; - break; - case '\\': - unescaped += '\\'; - ++i; - break; - case '"': - unescaped += '"'; - ++i; - break; - default: - unescaped += output[i]; - break; - } - } else { - unescaped += output[i]; - } + try { + return serializer_->get_defaults(type_name); + } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + throw std::runtime_error("Unknown type: " + type_name + " (" + e.what() + ")"); + } catch (const std::exception & e) { + throw std::runtime_error("Failed to get type template: " + std::string(e.what())); } - - return output_parser_->parse_yaml(unescaped); } nlohmann::json TypeIntrospection::get_type_schema(const std::string & type_name) { - // Call Python script for recursive schema - if (scripts_path_.empty()) { - throw std::runtime_error("scripts_path not configured for TypeIntrospection"); - } - - std::string script_path = "python3 " + ROS2CLIWrapper::escape_shell_arg(scripts_path_ + "/get_type_schema.py") + " " + - ROS2CLIWrapper::escape_shell_arg(type_name); - - std::string output = cli_wrapper_->exec(script_path); - try { - nlohmann::json result = nlohmann::json::parse(output); - - // Check for error in result - if (result.contains("error")) { - throw std::runtime_error(result["error"].get()); - } - - return result; - } catch (const nlohmann::json::parse_error & e) { - throw std::runtime_error("Failed to parse schema output: " + std::string(e.what())); + return serializer_->get_schema(type_name); + } catch (const ros2_medkit_serialization::TypeNotFoundError & e) { + throw std::runtime_error("Unknown type: " + type_name + " (" + e.what() + ")"); + } catch (const std::exception & e) { + throw std::runtime_error("Failed to get type schema: " + std::string(e.what())); } } @@ -115,7 +57,7 @@ TopicTypeInfo TypeIntrospection::get_type_info(const std::string & type_name) { } } - // Build type info + // Build type info using native serializer TopicTypeInfo info; info.name = type_name; @@ -127,14 +69,9 @@ TopicTypeInfo TypeIntrospection::get_type_info(const std::string & type_name) { info.default_value = nlohmann::json::object(); } - // Get schema (recursive type info) + // Get schema (type structure) try { - nlohmann::json schema_result = get_type_schema(type_name); - if (schema_result.contains("schema")) { - info.schema = schema_result["schema"]; - } else { - info.schema = nlohmann::json::object(); - } + info.schema = get_type_schema(type_name); } catch (const std::exception & e) { // If schema fails, use empty object info.schema = nlohmann::json::object(); diff --git a/src/ros2_medkit_gateway/test/test_integration.test.py b/src/ros2_medkit_gateway/test/test_integration.test.py index 85978260f..e44944329 100644 --- a/src/ros2_medkit_gateway/test/test_integration.test.py +++ b/src/ros2_medkit_gateway/test/test_integration.test.py @@ -1810,8 +1810,11 @@ def test_53_service_operation_has_type_info_schema(self): self.assertIsInstance(type_info['response'], dict) # std_srvs/srv/Trigger has empty request and response with success+message - self.assertIn('success', type_info['response']) - self.assertIn('message', type_info['response']) + # Schema format is JSON Schema: {"type": "object", "properties": {...}} + response_schema = type_info['response'] + self.assertIn('properties', response_schema, 'Response schema should have properties') + self.assertIn('success', response_schema['properties']) + self.assertIn('message', response_schema['properties']) print(f'✓ Service operation type_info test passed: {type_info}') @@ -1854,9 +1857,16 @@ def test_54_action_operation_has_type_info_schema(self): self.assertIsInstance(type_info['feedback'], dict) # example_interfaces/action/Fibonacci has order in goal, sequence in result/feedback - self.assertIn('order', type_info['goal']) - self.assertIn('sequence', type_info['result']) - self.assertIn('sequence', type_info['feedback']) + # Schema format is JSON Schema: {"type": "object", "properties": {...}} + goal_schema = type_info['goal'] + result_schema = type_info['result'] + feedback_schema = type_info['feedback'] + self.assertIn('properties', goal_schema, 'Goal schema should have properties') + self.assertIn('order', goal_schema['properties']) + self.assertIn('properties', result_schema, 'Result schema should have properties') + self.assertIn('sequence', result_schema['properties']) + self.assertIn('properties', feedback_schema, 'Feedback schema should have properties') + self.assertIn('sequence', feedback_schema['properties']) print(f'✓ Action operation type_info test passed: {type_info}') diff --git a/src/ros2_medkit_serialization/CMakeLists.txt b/src/ros2_medkit_serialization/CMakeLists.txt new file mode 100644 index 000000000..fe52dc4d5 --- /dev/null +++ b/src/ros2_medkit_serialization/CMakeLists.txt @@ -0,0 +1,144 @@ +cmake_minimum_required(VERSION 3.8) +project(ros2_medkit_serialization) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Code coverage option +option(ENABLE_COVERAGE "Enable code coverage reporting" OFF) +if(ENABLE_COVERAGE) + message(STATUS "Code coverage enabled") + add_compile_options(--coverage -O0 -g) + add_link_options(--coverage) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(dynmsg REQUIRED) +find_package(rosidl_typesupport_introspection_cpp REQUIRED) +find_package(rosidl_runtime_cpp REQUIRED) +find_package(rcpputils REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(nlohmann_json REQUIRED) + +# Library +add_library(${PROJECT_NAME} + src/json_serializer.cpp + src/type_cache.cpp + src/service_action_types.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + dynmsg + rosidl_typesupport_introspection_cpp + rosidl_runtime_cpp + rcpputils + yaml_cpp_vendor +) + +target_link_libraries(${PROJECT_NAME} + nlohmann_json::nlohmann_json + yaml-cpp::yaml-cpp +) + +# Apply coverage flags to library +if(ENABLE_COVERAGE) + target_compile_options(${PROJECT_NAME} PRIVATE --coverage -O0 -g) + target_link_options(${PROJECT_NAME} PRIVATE --coverage) +endif() + +# Export the library +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + dynmsg + rosidl_typesupport_introspection_cpp + rosidl_runtime_cpp + rcpputils + yaml_cpp_vendor + nlohmann_json +) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_clang_tidy REQUIRED) + find_package(std_msgs REQUIRED) + find_package(std_srvs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(test_msgs REQUIRED) + + # Use custom clang-format and clang-tidy configs from repo root + set(ament_cmake_clang_format_CONFIG_FILE "${CMAKE_CURRENT_SOURCE_DIR}/../../.clang-format") + set(ament_cmake_clang_tidy_CONFIG_FILE "${CMAKE_CURRENT_SOURCE_DIR}/../../.clang-tidy") + + # Limit clang-tidy to only report issues from our source files + set(ament_cmake_clang_tidy_HEADER_FILTER "^${CMAKE_CURRENT_SOURCE_DIR}/(include|src|test)/") + + # Exclude linters that conflict with clang-format + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_uncrustify + ament_cmake_cpplint + ament_cmake_clang_tidy + ) + ament_lint_auto_find_test_dependencies() + + # Configure clang-tidy manually with increased timeout + ament_clang_tidy( + "${CMAKE_CURRENT_BINARY_DIR}" + CONFIG_FILE "${ament_cmake_clang_tidy_CONFIG_FILE}" + HEADER_FILTER "${ament_cmake_clang_tidy_HEADER_FILTER}" + TIMEOUT 300 + ) + + # Unit tests + ament_add_gtest(test_type_cache test/test_type_cache.cpp) + target_link_libraries(test_type_cache ${PROJECT_NAME}) + ament_target_dependencies(test_type_cache std_msgs std_srvs) + + ament_add_gtest(test_json_serializer test/test_json_serializer.cpp) + target_link_libraries(test_json_serializer ${PROJECT_NAME}) + ament_target_dependencies(test_json_serializer std_msgs std_srvs geometry_msgs sensor_msgs test_msgs) + + ament_add_gtest(test_service_action_types test/test_service_action_types.cpp) + target_link_libraries(test_service_action_types ${PROJECT_NAME}) + ament_target_dependencies(test_service_action_types std_srvs) + + # Apply coverage flags to test targets + if(ENABLE_COVERAGE) + target_compile_options(test_type_cache PRIVATE --coverage -O0 -g) + target_link_options(test_type_cache PRIVATE --coverage) + target_compile_options(test_json_serializer PRIVATE --coverage -O0 -g) + target_link_options(test_json_serializer PRIVATE --coverage) + target_compile_options(test_service_action_types PRIVATE --coverage -O0 -g) + target_link_options(test_service_action_types PRIVATE --coverage) + endif() +endif() + +ament_package() diff --git a/src/ros2_medkit_serialization/README.md b/src/ros2_medkit_serialization/README.md new file mode 100644 index 000000000..7fe5c83f7 --- /dev/null +++ b/src/ros2_medkit_serialization/README.md @@ -0,0 +1,96 @@ +# ros2_medkit_serialization + +Runtime JSON ↔ ROS 2 message serialization library for the ros2_medkit gateway. + +## Overview + +This package provides dynamic message serialization using [dynmsg](https://github.com/osrf/dynamic_message_introspection), enabling JSON ↔ ROS 2 message conversion at runtime without compile-time type dependencies. It eliminates CLI fallbacks in the gateway by providing native C++ APIs for all message operations. + +## Features + +- **JSON Serialization**: Convert ROS 2 messages to/from JSON (via YAML bridge) +- **CDR Serialization**: Serialize JSON to CDR format for `GenericPublisher` and `GenericClient` +- **CDR Deserialization**: Deserialize CDR data from `GenericSubscription` to JSON +- **Type Caching**: Thread-safe caching of type introspection data with LRU eviction +- **Service/Action Types**: Helper functions for request/response/goal type derivation +- **Schema Generation**: Generate JSON schemas for ROS 2 message types + +## Dependencies + +- `dynmsg` - OSRF dynamic message introspection library +- `nlohmann_json` - JSON library +- `yaml_cpp_vendor` - YAML library (for dynmsg compatibility) +- `rclcpp` - ROS 2 client library (for SerializedMessage) + +## Usage + +```cpp +#include "ros2_medkit_serialization/json_serializer.hpp" +#include "ros2_medkit_serialization/type_cache.hpp" + +// Get type info (cached) +auto& cache = ros2_medkit_serialization::TypeCache::instance(); +auto type_info = cache.get("std_msgs", "String"); + +// Create serializer +ros2_medkit_serialization::JsonSerializer serializer; + +// Serialize JSON to CDR for GenericPublisher +nlohmann::json msg_json = {{"data", "Hello World"}}; +auto serialized = serializer.serialize("std_msgs/msg/String", msg_json); +// Use with: generic_publisher->publish(serialized); + +// Deserialize CDR to JSON from GenericSubscription +nlohmann::json result = serializer.deserialize("std_msgs/msg/String", serialized_msg); + +// Convert in-memory message to JSON +nlohmann::json json = serializer.to_json(type_info, message_ptr); + +// Generate schema for a type +nlohmann::json schema = serializer.get_schema("geometry_msgs/msg/Twist"); +``` + +## API Reference + +### JsonSerializer + +| Method | Description | +|--------|-------------| +| `serialize(type, json)` | Serialize JSON to CDR `SerializedMessage` | +| `deserialize(type, msg)` | Deserialize CDR to JSON | +| `to_json(type_info, ptr)` | Convert in-memory message to JSON | +| `from_json(type, json)` | Create message from JSON | +| `get_schema(type)` | Get JSON schema for type | +| `get_defaults(type)` | Get default values as JSON | +| `yaml_to_json(yaml)` | Convert YAML node to JSON (static) | + +### TypeCache + +| Method | Description | +|--------|-------------| +| `instance()` | Get singleton instance | +| `get(pkg, name)` | Get cached type info | +| `get(type_string)` | Get cached type info from full type string | +| `clear()` | Clear all cached entries | + +## Building + +```bash +cd /path/to/ros2_medkit +colcon build --packages-select ros2_medkit_serialization +``` + +## Testing + +```bash +colcon test --packages-select ros2_medkit_serialization +colcon test-result --verbose +``` + +## Design Documentation + +See the [design documentation](../../docs/design/ros2_medkit_serialization/index.rst) for architecture details. + +## License + +Apache-2.0 diff --git a/src/ros2_medkit_serialization/design/architecture.puml b/src/ros2_medkit_serialization/design/architecture.puml new file mode 100644 index 000000000..9305b87ea --- /dev/null +++ b/src/ros2_medkit_serialization/design/architecture.puml @@ -0,0 +1,97 @@ +@startuml ros2_medkit_serialization_architecture + +skinparam linetype ortho +skinparam classAttributeIconSize 0 + +title ROS 2 Medkit Serialization - Class Architecture + +package "External Libraries" { + class "dynmsg" { + +ros_message_init() + +ros_message_set_from_yaml() + +ros_message_to_yaml() + +ros_message_serialize() + +ros_message_deserialize() + } + + class "nlohmann::json" as JSON + + class "YAML::Node" as YAML + + class "rclcpp::SerializedMessage" as SerializedMessage +} + +package "ros2_medkit_serialization" { + + class JsonSerializer { + + to_json(type_info, data): json + + to_json(type_string, data): json + + from_json(type_info, json): RosMessage_Cpp + + from_json(type_string, json): RosMessage_Cpp + + from_json_to_message(type_info, json, data): void + + serialize(type_string, json): SerializedMessage + + deserialize(type_string, msg): json + + get_schema(type_string): json + + get_defaults(type_string): json + + {static} yaml_to_json(yaml): json + + {static} json_to_yaml(json): YAML::Node + } + + class TypeCache <> { + + {static} instance(): TypeCache& + + get(pkg, name): TypeInfo_Cpp* + + get(type_string): TypeInfo_Cpp* + + clear(): void + - cache_: map + - mutex_: shared_mutex + } + + class ServiceActionTypes <> { + + {static} get_request_type(srv_type): string + + {static} get_response_type(srv_type): string + + {static} get_goal_type(action_type): string + + {static} get_result_type(action_type): string + + {static} get_feedback_type(action_type): string + + {static} is_service_type(type): bool + + {static} is_action_type(type): bool + } + + class SerializationError <> { + + what(): string + } + + class TypeNotFoundError <> { + + what(): string + } + + class JsonConversionError <> { + + what(): string + } +} + +' Relationships + +' JsonSerializer uses TypeCache +JsonSerializer --> TypeCache : uses + +' JsonSerializer uses dynmsg +JsonSerializer --> "dynmsg" : uses + +' JsonSerializer converts between formats +JsonSerializer ..> JSON : produces/consumes +JsonSerializer ..> YAML : internal conversion +JsonSerializer ..> SerializedMessage : produces/consumes + +' TypeCache caches dynmsg type info +TypeCache --> "dynmsg" : caches TypeInfo_Cpp + +' Exception hierarchy +TypeNotFoundError -up-|> SerializationError : extends +JsonConversionError -up-|> SerializationError : extends + +' JsonSerializer throws exceptions +JsonSerializer ..> SerializationError : throws +JsonSerializer ..> TypeNotFoundError : throws +JsonSerializer ..> JsonConversionError : throws + +@enduml diff --git a/src/ros2_medkit_serialization/design/index.rst b/src/ros2_medkit_serialization/design/index.rst new file mode 100644 index 000000000..54de86884 --- /dev/null +++ b/src/ros2_medkit_serialization/design/index.rst @@ -0,0 +1,248 @@ +ros2_medkit_serialization +========================= + +This section contains design documentation for the ros2_medkit_serialization package. + +Overview +-------- + +The ``ros2_medkit_serialization`` package provides runtime JSON ↔ ROS 2 message conversion +using the `dynmsg `_ library. +It enables the gateway to work with any ROS 2 message type without compile-time dependencies. + +Architecture +------------ + +The following diagram shows the relationships between the main components. + +.. plantuml:: + :caption: ROS 2 Medkit Serialization Class Architecture + + @startuml ros2_medkit_serialization_architecture + + skinparam linetype ortho + skinparam classAttributeIconSize 0 + + title ROS 2 Medkit Serialization - Class Architecture + + package "External Libraries" { + class "dynmsg" { + +ros_message_init() + +ros_message_set_from_yaml() + +ros_message_to_yaml() + +ros_message_serialize() + +ros_message_deserialize() + } + + class "nlohmann::json" as JSON + + class "YAML::Node" as YAML + + class "rclcpp::SerializedMessage" as SerializedMessage + } + + package "ros2_medkit_serialization" { + + class JsonSerializer { + + to_json(type_info, data): json + + to_json(type_string, data): json + + from_json(type_info, json): RosMessage_Cpp + + from_json(type_string, json): RosMessage_Cpp + + from_json_to_message(type_info, json, data): void + + serialize(type_string, json): SerializedMessage + + deserialize(type_string, msg): json + + get_schema(type_string): json + + get_defaults(type_string): json + + {static} yaml_to_json(yaml): json + + {static} json_to_yaml(json): YAML::Node + } + + class TypeCache <> { + + {static} instance(): TypeCache& + + get(pkg, name): TypeInfo_Cpp* + + get(type_string): TypeInfo_Cpp* + + clear(): void + - cache_: map + - mutex_: shared_mutex + } + + class ServiceActionTypes <> { + + {static} get_request_type(srv_type): string + + {static} get_response_type(srv_type): string + + {static} get_goal_type(action_type): string + + {static} get_result_type(action_type): string + + {static} get_feedback_type(action_type): string + + {static} is_service_type(type): bool + + {static} is_action_type(type): bool + } + + class SerializationError <> { + + what(): string + } + + class TypeNotFoundError <> { + + what(): string + } + + class JsonConversionError <> { + + what(): string + } + } + + ' Relationships + + ' JsonSerializer uses TypeCache + JsonSerializer --> TypeCache : uses + + ' JsonSerializer uses dynmsg + JsonSerializer --> "dynmsg" : uses + + ' JsonSerializer converts between formats + JsonSerializer ..> JSON : produces/consumes + JsonSerializer ..> YAML : internal conversion + JsonSerializer ..> SerializedMessage : produces/consumes + + ' TypeCache caches dynmsg type info + TypeCache --> "dynmsg" : caches TypeInfo_Cpp + + ' Exception hierarchy + TypeNotFoundError -up-|> SerializationError : extends + JsonConversionError -up-|> SerializationError : extends + + ' JsonSerializer throws exceptions + JsonSerializer ..> SerializationError : throws + JsonSerializer ..> TypeNotFoundError : throws + JsonSerializer ..> JsonConversionError : throws + + @enduml + +Main Components +--------------- + +1. **JsonSerializer** - The main API for JSON ↔ ROS 2 message conversion + - Stateless and thread-safe design + - Provides ``serialize()`` / ``deserialize()`` for CDR format (used with GenericPublisher/GenericSubscription) + - Provides ``to_json()`` / ``from_json()`` for in-memory message conversion + - Uses dynmsg YAML bridge internally (JSON ↔ YAML ↔ ROS 2 message) + - Generates JSON schemas and default values for any message type + - Static utilities for YAML ↔ JSON conversion + +2. **TypeCache** - Thread-safe singleton cache for type introspection data + - Caches ``TypeInfo_Cpp*`` pointers from dynmsg + - Uses ``std::shared_mutex`` for concurrent read access + - Automatic cache miss loading via dynmsg ``ros_type_info_get()`` + - Key format: ``"package/msg/Type"`` or ``"package/srv/Type"`` + +3. **ServiceActionTypes** - Utility functions for service/action type manipulation + - Derives request/response types from service type (e.g., ``std_srvs/srv/SetBool`` → ``std_srvs/srv/SetBool_Request``) + - Derives goal/result/feedback types from action type + - Type validation helpers (``is_service_type()``, ``is_action_type()``) + +4. **Exception Classes** - Typed exceptions for error handling + - ``SerializationError`` - Base class for all serialization errors + - ``TypeNotFoundError`` - Message type not found in ROS 2 type system + - ``JsonConversionError`` - JSON/YAML conversion failed + +Data Flow +--------- + +Publishing (JSON → CDR) +~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: text + + JSON → json_to_yaml() → YAML::Node → dynmsg (set_from_yaml + serialize) → SerializedMessage → GenericPublisher + +Subscribing (CDR → JSON) +~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: text + + GenericSubscription → SerializedMessage → dynmsg (deserialize + to_yaml) → YAML::Node → yaml_to_json() → JSON + +Service Calls (JSON → CDR → JSON) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: text + + Request: JSON → serialize() → SerializedMessage → GenericClient + Response: GenericClient → SerializedMessage → deserialize() → JSON + +Design Decisions +---------------- + +YAML Bridge +~~~~~~~~~~~ + +The package uses dynmsg's YAML interface rather than direct binary manipulation because: + +1. **Stability**: dynmsg's YAML API is well-tested and handles complex nested types +2. **Type Safety**: YAML preserves type information during conversion +3. **Debugging**: YAML is human-readable for troubleshooting +4. **Performance**: Conversion overhead is negligible compared to network I/O + +The JSON ↔ YAML conversion layer adds minimal overhead since both formats +have similar data models (objects, arrays, scalars). + +Singleton TypeCache +~~~~~~~~~~~~~~~~~~~ + +The TypeCache uses the singleton pattern because: + +1. **Memory Efficiency**: Type introspection data is shared across all serializers +2. **Thread Safety**: Single mutex protects all cache operations +3. **Lifetime Management**: Cache lives for the process duration, matching dynmsg's design + +Thread Safety +~~~~~~~~~~~~~ + +- ``JsonSerializer`` is stateless and can be used from multiple threads +- ``TypeCache`` uses ``std::shared_mutex`` for concurrent reads +- All public methods are safe to call from any thread + +Usage in Gateway +---------------- + +The serialization package is used by: + +1. **DataAccessManager** - Publishing to topics via ``GenericPublisher`` +2. **OperationManager** - Calling services/actions via ``GenericClient`` +3. **NativeTopicSampler** - Deserializing messages from ``GenericSubscription`` + +Example: Publishing to a Topic +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: cpp + + JsonSerializer serializer; + + // Create publisher + auto publisher = node->create_generic_publisher( + "/cmd_vel", "geometry_msgs/msg/Twist", qos); + + // Serialize JSON to CDR + nlohmann::json twist_json = { + {"linear", {{"x", 1.0}, {"y", 0.0}, {"z", 0.0}}}, + {"angular", {{"x", 0.0}, {"y", 0.0}, {"z", 0.5}}} + }; + auto serialized = serializer.serialize("geometry_msgs/msg/Twist", twist_json); + + // Publish + publisher->publish(serialized); + +Example: Deserializing from Subscription +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: cpp + + JsonSerializer serializer; + + auto callback = [&](std::shared_ptr msg) { + nlohmann::json json = serializer.deserialize( + "sensor_msgs/msg/LaserScan", *msg); + // Process json... + }; + + auto subscription = node->create_generic_subscription( + "/scan", "sensor_msgs/msg/LaserScan", qos, callback); + diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/json_serializer.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/json_serializer.hpp new file mode 100644 index 000000000..95b419ea4 --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/json_serializer.hpp @@ -0,0 +1,160 @@ +// Copyright 2026 Selfpatch +// +// 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. + +#ifndef ROS2_MEDKIT_SERIALIZATION__JSON_SERIALIZER_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__JSON_SERIALIZER_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include "ros2_medkit_serialization/serialization_error.hpp" +#include "ros2_medkit_serialization/type_cache.hpp" + +namespace ros2_medkit_serialization { + +/// JSON serializer for ROS 2 messages using dynmsg. +/// +/// This class provides the main API for converting between JSON and ROS 2 +/// messages at runtime. It uses dynmsg's YAML capabilities internally and +/// converts between JSON and YAML formats. +/// +/// @note This class is stateless and thread-safe. +class JsonSerializer { + public: + JsonSerializer() = default; + ~JsonSerializer() = default; + + // Non-copyable but movable + JsonSerializer(const JsonSerializer &) = delete; + JsonSerializer & operator=(const JsonSerializer &) = delete; + JsonSerializer(JsonSerializer &&) = default; + JsonSerializer & operator=(JsonSerializer &&) = default; + + /// Convert a ROS 2 message to JSON + /// + /// @param type_info Type introspection info + /// @param message_data Pointer to the message data + /// @return JSON representation of the message + /// @throws JsonConversionError if conversion fails + nlohmann::json to_json(const TypeInfo_Cpp * type_info, const void * message_data) const; + + /// Convert a ROS 2 message to JSON using type string + /// + /// @param type_string Full type string (e.g., "std_msgs/msg/String") + /// @param message_data Pointer to the message data + /// @return JSON representation of the message + /// @throws TypeNotFoundError if type cannot be loaded + /// @throws JsonConversionError if conversion fails + nlohmann::json to_json(const std::string & type_string, const void * message_data) const; + + /// Convert JSON to a ROS 2 message + /// + /// @param type_info Type introspection info + /// @param json JSON data to convert + /// @return RosMessage_Cpp containing allocated message data + /// @throws JsonConversionError if conversion fails + /// @note Caller is responsible for calling ros_message_destroy_with_allocator + RosMessage_Cpp from_json(const TypeInfo_Cpp * type_info, const nlohmann::json & json) const; + + /// Convert JSON to a ROS 2 message using type string + /// + /// @param type_string Full type string (e.g., "std_msgs/msg/String") + /// @param json JSON data to convert + /// @return RosMessage_Cpp containing allocated message data + /// @throws TypeNotFoundError if type cannot be loaded + /// @throws JsonConversionError if conversion fails + /// @note Caller is responsible for calling ros_message_destroy_with_allocator + RosMessage_Cpp from_json(const std::string & type_string, const nlohmann::json & json) const; + + /// Populate an existing message from JSON + /// + /// @param type_info Type introspection info + /// @param json JSON data to convert + /// @param message_data Pointer to pre-allocated message to populate + /// @throws JsonConversionError if conversion fails + void from_json_to_message(const TypeInfo_Cpp * type_info, const nlohmann::json & json, void * message_data) const; + + /// Generate a JSON schema for a ROS 2 type + /// + /// @param type_info Type introspection info + /// @return JSON schema object + nlohmann::json get_schema(const TypeInfo_Cpp * type_info) const; + + /// Generate a JSON schema using type string + /// + /// @param type_string Full type string + /// @return JSON schema object + /// @throws TypeNotFoundError if type cannot be loaded + nlohmann::json get_schema(const std::string & type_string) const; + + /// Get default values for a ROS 2 type as JSON + /// + /// @param type_info Type introspection info + /// @return JSON object with default values + nlohmann::json get_defaults(const TypeInfo_Cpp * type_info) const; + + /// Get default values using type string + /// + /// @param type_string Full type string + /// @return JSON object with default values + /// @throws TypeNotFoundError if type cannot be loaded + nlohmann::json get_defaults(const std::string & type_string) const; + + // Serialization for GenericClient/GenericSubscription + + /// Serialize JSON to CDR format for use with GenericClient + /// + /// @param type_string Full type string (e.g., "std_srvs/srv/SetBool_Request") + /// @param json JSON data to serialize + /// @return SerializedMessage containing CDR data + /// @throws TypeNotFoundError if type cannot be loaded + /// @throws SerializationError if serialization fails + rclcpp::SerializedMessage serialize(const std::string & type_string, const nlohmann::json & json) const; + + /// Deserialize CDR data to JSON + /// + /// @param type_string Full type string + /// @param serialized_msg SerializedMessage containing CDR data + /// @return JSON representation of the message + /// @throws TypeNotFoundError if type cannot be loaded + /// @throws SerializationError if deserialization fails + nlohmann::json deserialize(const std::string & type_string, const rclcpp::SerializedMessage & serialized_msg) const; + + // YAML ↔ JSON conversion utilities + + /// Convert YAML node to JSON + /// + /// @param yaml YAML node to convert + /// @return JSON representation + static nlohmann::json yaml_to_json(const YAML::Node & yaml); + + /// Convert JSON to YAML node + /// + /// @param json JSON to convert + /// @return YAML node representation + static YAML::Node json_to_yaml(const nlohmann::json & json); + + private: + /// Get type info from cache, throwing if not found + const TypeInfo_Cpp * get_type_info_or_throw(const std::string & type_string) const; +}; + +} // namespace ros2_medkit_serialization + +#endif // ROS2_MEDKIT_SERIALIZATION__JSON_SERIALIZER_HPP_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/serialization_error.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/serialization_error.hpp new file mode 100644 index 000000000..7f9687d5e --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/serialization_error.hpp @@ -0,0 +1,103 @@ +// Copyright 2026 Selfpatch +// +// 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. + +#ifndef ROS2_MEDKIT_SERIALIZATION__SERIALIZATION_ERROR_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__SERIALIZATION_ERROR_HPP_ + +#include +#include + +namespace ros2_medkit_serialization { + +/// Base class for all serialization errors +class SerializationError : public std::runtime_error { + public: + explicit SerializationError(const std::string & message) : std::runtime_error(message) { + } +}; + +/// Error when type cannot be found or loaded +class TypeNotFoundError : public SerializationError { + public: + explicit TypeNotFoundError(const std::string & type_name) + : SerializationError("Type not found: " + type_name), type_name_(type_name) { + } + + const std::string & type_name() const noexcept { + return type_name_; + } + + private: + std::string type_name_; +}; + +/// Error during JSON parsing or conversion +class JsonConversionError : public SerializationError { + public: + explicit JsonConversionError(const std::string & message) : SerializationError("JSON conversion error: " + message) { + } +}; + +/// Error during YAML parsing or conversion +class YamlConversionError : public SerializationError { + public: + explicit YamlConversionError(const std::string & message) : SerializationError("YAML conversion error: " + message) { + } +}; + +/// Error when a required field is missing +class MissingFieldError : public SerializationError { + public: + explicit MissingFieldError(const std::string & field_name) + : SerializationError("Missing required field: " + field_name), field_name_(field_name) { + } + + const std::string & field_name() const noexcept { + return field_name_; + } + + private: + std::string field_name_; +}; + +/// Error when field type doesn't match expected type +class TypeMismatchError : public SerializationError { + public: + TypeMismatchError(const std::string & field_name, const std::string & expected_type, const std::string & actual_type) + : SerializationError("Type mismatch for field '" + field_name + "': expected " + expected_type + ", got " + + actual_type) + , field_name_(field_name) + , expected_type_(expected_type) + , actual_type_(actual_type) { + } + + const std::string & field_name() const noexcept { + return field_name_; + } + const std::string & expected_type() const noexcept { + return expected_type_; + } + const std::string & actual_type() const noexcept { + return actual_type_; + } + + private: + std::string field_name_; + std::string expected_type_; + std::string actual_type_; +}; + +} // namespace ros2_medkit_serialization + +#endif // ROS2_MEDKIT_SERIALIZATION__SERIALIZATION_ERROR_HPP_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/service_action_types.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/service_action_types.hpp new file mode 100644 index 000000000..92fd45302 --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/service_action_types.hpp @@ -0,0 +1,138 @@ +// Copyright 2026 Selfpatch +// +// 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. + +#ifndef ROS2_MEDKIT_SERIALIZATION__SERVICE_ACTION_TYPES_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__SERVICE_ACTION_TYPES_HPP_ + +#include +#include +#include + +namespace ros2_medkit_serialization { + +/// Helper functions for working with service and action types. +/// +/// ROS 2 services have Request and Response types, while actions have +/// Goal, Result, and Feedback types. This class provides utilities to +/// derive these sub-types from the main service/action type. +class ServiceActionTypes { + public: + /// Get the request type for a service + /// + /// @param service_type Full service type (e.g., "std_srvs/srv/SetBool") + /// @return Request type string (e.g., "std_srvs/srv/SetBool_Request") + static std::string get_service_request_type(const std::string & service_type); + + /// Get the response type for a service + /// + /// @param service_type Full service type (e.g., "std_srvs/srv/SetBool") + /// @return Response type string (e.g., "std_srvs/srv/SetBool_Response") + static std::string get_service_response_type(const std::string & service_type); + + /// Get the goal type for an action + /// + /// @param action_type Full action type (e.g., "action_tutorials_interfaces/action/Fibonacci") + /// @return Goal type string + static std::string get_action_goal_type(const std::string & action_type); + + /// Get the result type for an action + /// + /// @param action_type Full action type + /// @return Result type string + static std::string get_action_result_type(const std::string & action_type); + + /// Get the feedback type for an action + /// + /// @param action_type Full action type + /// @return Feedback type string + static std::string get_action_feedback_type(const std::string & action_type); + + // ==================== Action Internal Service Types ==================== + // Actions are implemented as a set of internal services: + // - {action}/_action/send_goal (type: {ActionType}_SendGoal) + // - {action}/_action/cancel_goal (type: action_msgs/srv/CancelGoal) + // - {action}/_action/get_result (type: {ActionType}_GetResult) + + /// Get the SendGoal service type for an action + /// + /// @param action_type Full action type (e.g., "example_interfaces/action/Fibonacci") + /// @return Service type string (e.g., "example_interfaces/action/Fibonacci_SendGoal") + static std::string get_action_send_goal_service_type(const std::string & action_type); + + /// Get the GetResult service type for an action + /// + /// @param action_type Full action type + /// @return Service type string (e.g., "example_interfaces/action/Fibonacci_GetResult") + static std::string get_action_get_result_service_type(const std::string & action_type); + + /// Get the SendGoal request type for an action (used with GenericClient) + /// + /// @param action_type Full action type + /// @return Request message type string (e.g., "example_interfaces/action/Fibonacci_SendGoal_Request") + static std::string get_action_send_goal_request_type(const std::string & action_type); + + /// Get the SendGoal response type for an action + /// + /// @param action_type Full action type + /// @return Response message type string (e.g., "example_interfaces/action/Fibonacci_SendGoal_Response") + static std::string get_action_send_goal_response_type(const std::string & action_type); + + /// Get the GetResult request type for an action (used with GenericClient) + /// + /// @param action_type Full action type + /// @return Request message type string (e.g., "example_interfaces/action/Fibonacci_GetResult_Request") + static std::string get_action_get_result_request_type(const std::string & action_type); + + /// Get the GetResult response type for an action + /// + /// @param action_type Full action type + /// @return Response message type string (e.g., "example_interfaces/action/Fibonacci_GetResult_Response") + static std::string get_action_get_result_response_type(const std::string & action_type); + + /// Get the FeedbackMessage type for an action (used for feedback subscription) + /// + /// @param action_type Full action type + /// @return Message type string (e.g., "example_interfaces/action/Fibonacci_FeedbackMessage") + static std::string get_action_feedback_message_type(const std::string & action_type); + + /// Parse a service/action type into components + /// + /// @param full_type Full type string (e.g., "std_srvs/srv/SetBool") + /// @return Tuple of (package, interface_type, name), or nullopt if invalid + /// interface_type is "msg", "srv", or "action" + static std::optional> + parse_interface_type(const std::string & full_type); + + /// Check if a type is a service type + /// + /// @param full_type Full type string + /// @return true if the type contains "/srv/" + static bool is_service_type(const std::string & full_type); + + /// Check if a type is an action type + /// + /// @param full_type Full type string + /// @return true if the type contains "/action/" + static bool is_action_type(const std::string & full_type); + + /// Check if a type is a message type + /// + /// @param full_type Full type string + /// @return true if the type contains "/msg/" + static bool is_message_type(const std::string & full_type); +}; + +} // namespace ros2_medkit_serialization + +#endif // ROS2_MEDKIT_SERIALIZATION__SERVICE_ACTION_TYPES_HPP_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/type_cache.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/type_cache.hpp new file mode 100644 index 000000000..4e13da3de --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/type_cache.hpp @@ -0,0 +1,107 @@ +// Copyright 2026 Selfpatch +// +// 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. + +#ifndef ROS2_MEDKIT_SERIALIZATION__TYPE_CACHE_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__TYPE_CACHE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dynmsg/typesupport.hpp" + +namespace ros2_medkit_serialization { + +/// Thread-safe cache for ROS 2 type introspection data. +/// +/// This class wraps dynmsg's type loading functions and caches the results +/// to avoid repeated dynamic library lookups. It is implemented as a singleton +/// for global access throughout the gateway. +/// +/// @note All public methods are thread-safe. +class TypeCache { + public: + /// Get the singleton instance + static TypeCache & instance(); + + /// Delete copy constructor + TypeCache(const TypeCache &) = delete; + + /// Delete copy assignment + TypeCache & operator=(const TypeCache &) = delete; + + /// Get type info for a message type (C++ introspection) + /// + /// @param package_name Package name (e.g., "std_msgs") + /// @param type_name Type name without prefix (e.g., "String") + /// @return Pointer to type info, or nullptr if not found + const TypeInfo_Cpp * get_message_type_info(const std::string & package_name, const std::string & type_name); + + /// Get type info for any interface type (msg/srv/action) + /// + /// @param package_name Package name (e.g., "std_srvs") + /// @param interface_type Interface type ("msg", "srv", or "action") + /// @param type_name Type name (e.g., "Trigger_Request") + /// @return Pointer to type info, or nullptr if not found + const TypeInfo_Cpp * get_message_type_info(const std::string & package_name, const std::string & interface_type, + const std::string & type_name); + + /// Get type info from full type string + /// + /// @param full_type Full type string (e.g., "std_msgs/msg/String" or "std_srvs/srv/Trigger_Request") + /// @return Pointer to type info, or nullptr if not found + const TypeInfo_Cpp * get_message_type_info(const std::string & full_type); + + /// Parse a full type string into package, interface type, and type name + /// + /// @param full_type Full type string (e.g., "std_msgs/msg/String") + /// @return Tuple of (package_name, interface_type, type_name), or nullopt if invalid format + static std::optional> + parse_type_string(const std::string & full_type); + + /// Check if a type is cached + /// + /// @param package_name Package name + /// @param type_name Type name + /// @return true if the type is in the cache + bool is_cached(const std::string & package_name, const std::string & type_name) const; + + /// Clear the cache + void clear(); + + /// Get the number of cached types + size_t size() const; + + private: + TypeCache() = default; + + /// Generate cache key from package and type name (assumes "msg" interface type) + static std::string make_key(const std::string & package_name, const std::string & type_name); + + /// Generate cache key from package, interface type, and type name + static std::string make_key(const std::string & package_name, const std::string & interface_type, + const std::string & type_name); + + mutable std::shared_mutex mutex_; + std::unordered_map cache_; +}; + +} // namespace ros2_medkit_serialization + +#endif // ROS2_MEDKIT_SERIALIZATION__TYPE_CACHE_HPP_ diff --git a/src/ros2_medkit_serialization/package.xml b/src/ros2_medkit_serialization/package.xml new file mode 100644 index 000000000..0440af73b --- /dev/null +++ b/src/ros2_medkit_serialization/package.xml @@ -0,0 +1,34 @@ + + + + ros2_medkit_serialization + 0.1.0 + Runtime JSON to ROS 2 message serialization library + bburda + Apache-2.0 + + ament_cmake + + rclcpp + dynmsg + rosidl_typesupport_introspection_cpp + rosidl_runtime_cpp + rcpputils + yaml_cpp_vendor + nlohmann-json-dev + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + ament_cmake_clang_format + ament_cmake_clang_tidy + std_msgs + std_srvs + geometry_msgs + sensor_msgs + test_msgs + + + ament_cmake + + diff --git a/src/ros2_medkit_serialization/src/json_serializer.cpp b/src/ros2_medkit_serialization/src/json_serializer.cpp new file mode 100644 index 000000000..dcf5025b8 --- /dev/null +++ b/src/ros2_medkit_serialization/src/json_serializer.cpp @@ -0,0 +1,446 @@ +// Copyright 2026 Selfpatch +// +// 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_serialization/json_serializer.hpp" + +#include + +#include "dynmsg/message_reading.hpp" +#include "dynmsg/msg_parser.hpp" +#include "rcpputils/shared_library.hpp" +#include "rmw/rmw.h" +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" + +namespace ros2_medkit_serialization { + +nlohmann::json JsonSerializer::to_json(const TypeInfo_Cpp * type_info, const void * message_data) const { + if (type_info == nullptr || message_data == nullptr) { + throw JsonConversionError("Null type_info or message_data"); + } + + // Create a RosMessage_Cpp structure for dynmsg + RosMessage_Cpp ros_msg; + ros_msg.type_info = type_info; + ros_msg.data = const_cast(static_cast(message_data)); + + // Use dynmsg to convert to YAML + YAML::Node yaml_node = dynmsg::cpp::message_to_yaml(ros_msg); + + // Convert YAML to JSON + return yaml_to_json(yaml_node); +} + +nlohmann::json JsonSerializer::to_json(const std::string & type_string, const void * message_data) const { + const TypeInfo_Cpp * type_info = get_type_info_or_throw(type_string); + return to_json(type_info, message_data); +} + +RosMessage_Cpp JsonSerializer::from_json(const TypeInfo_Cpp * type_info, const nlohmann::json & json) const { + if (type_info == nullptr) { + throw JsonConversionError("Null type_info"); + } + + // Convert JSON to YAML + YAML::Node yaml_node = json_to_yaml(json); + + // Convert YAML to string for dynmsg + std::stringstream ss; + ss << yaml_node; + std::string yaml_str = ss.str(); + + // Use dynmsg to parse YAML into ROS message + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + return dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(type_info, yaml_str, &allocator); +} + +RosMessage_Cpp JsonSerializer::from_json(const std::string & type_string, const nlohmann::json & json) const { + const TypeInfo_Cpp * type_info = get_type_info_or_throw(type_string); + return from_json(type_info, json); +} + +void JsonSerializer::from_json_to_message(const TypeInfo_Cpp * type_info, const nlohmann::json & json, + void * message_data) const { + if (type_info == nullptr || message_data == nullptr) { + throw JsonConversionError("Null type_info or message_data"); + } + + // Convert JSON to YAML + YAML::Node yaml_node = json_to_yaml(json); + + // Convert YAML to string for dynmsg + std::stringstream ss; + ss << yaml_node; + std::string yaml_str = ss.str(); + + // Use dynmsg to populate existing message + dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(type_info, yaml_str, message_data); +} + +nlohmann::json JsonSerializer::get_schema(const TypeInfo_Cpp * type_info) const { + if (type_info == nullptr) { + throw JsonConversionError("Null type_info"); + } + + nlohmann::json schema; + schema["type"] = "object"; + schema["properties"] = nlohmann::json::object(); + + // Iterate over message members to build schema + for (uint32_t i = 0; i < type_info->member_count_; ++i) { + const auto & member = type_info->members_[i]; + nlohmann::json prop; + + // Map ROS types to JSON schema types + switch (member.type_id_) { + case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: + prop["type"] = "boolean"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + prop["type"] = "integer"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: + prop["type"] = "number"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + prop["type"] = "string"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + // Nested message - recursively get schema + if (member.members_ != nullptr) { + prop = get_schema(static_cast(member.members_->data)); + } else { + prop["type"] = "object"; + } + break; + default: + prop["type"] = "string"; // Fallback + break; + } + + // Handle arrays/sequences + if (member.is_array_) { + nlohmann::json array_schema; + array_schema["type"] = "array"; + array_schema["items"] = prop; + if (member.array_size_ > 0 && !member.is_upper_bound_) { + // Fixed-size array + array_schema["minItems"] = member.array_size_; + array_schema["maxItems"] = member.array_size_; + } + prop = array_schema; + } + + schema["properties"][member.name_] = prop; + } + + return schema; +} + +nlohmann::json JsonSerializer::get_schema(const std::string & type_string) const { + const TypeInfo_Cpp * type_info = get_type_info_or_throw(type_string); + return get_schema(type_info); +} + +nlohmann::json JsonSerializer::get_defaults(const TypeInfo_Cpp * type_info) const { + if (type_info == nullptr) { + throw JsonConversionError("Null type_info"); + } + + // Allocate a default-initialized message + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + RosMessage_Cpp ros_msg; + dynmsg::cpp::ros_message_with_typeinfo_init(type_info, &ros_msg, &allocator); + + // Convert the default message to JSON + nlohmann::json result = to_json(type_info, ros_msg.data); + + // Clean up + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); + + return result; +} + +nlohmann::json JsonSerializer::get_defaults(const std::string & type_string) const { + const TypeInfo_Cpp * type_info = get_type_info_or_throw(type_string); + return get_defaults(type_info); +} + +nlohmann::json JsonSerializer::yaml_to_json(const YAML::Node & yaml) { + switch (yaml.Type()) { + case YAML::NodeType::Null: + return nullptr; + + case YAML::NodeType::Scalar: { + // Try to parse as different types using type detection heuristics. + // Empty catch blocks are intentional - we try each type in order and + // fall through to the next on failure. This is a common pattern for + // YAML scalar type inference. + + // First try boolean + try { + bool b = yaml.as(); + // YAML::as succeeds for "true"/"false"/"yes"/"no" etc. + std::string str = yaml.as(); + if (str == "true" || str == "false" || str == "yes" || str == "no" || str == "True" || str == "False" || + str == "Yes" || str == "No") { + return b; + } + } catch (...) { + // Not a boolean - try next type + } + + // Try integer + try { + int64_t i = yaml.as(); + std::string str = yaml.as(); + // Check if it looks like an integer + if (!str.empty() && (std::isdigit(str[0]) || str[0] == '-')) { + try { + size_t pos; + std::stoll(str, &pos); + if (pos == str.length()) { + return i; + } + } catch (...) { + // String doesn't parse as integer - continue + } + } + } catch (...) { + // Not an integer - try next type + } + + // Try floating point + try { + double d = yaml.as(); + std::string str = yaml.as(); + // Check if it looks like a number + if (!str.empty() && (std::isdigit(str[0]) || str[0] == '-' || str[0] == '.')) { + try { + size_t pos; + std::stod(str, &pos); + if (pos == str.length()) { + return d; + } + } catch (...) { + // String doesn't parse as double - continue + } + } + } catch (...) { + // Not a double - fall back to string + } + + // Fall back to string + return yaml.as(); + } + + case YAML::NodeType::Sequence: { + nlohmann::json arr = nlohmann::json::array(); + for (const auto & elem : yaml) { + arr.push_back(yaml_to_json(elem)); + } + return arr; + } + + case YAML::NodeType::Map: { + nlohmann::json obj = nlohmann::json::object(); + for (const auto & pair : yaml) { + obj[pair.first.as()] = yaml_to_json(pair.second); + } + return obj; + } + + default: + return nullptr; + } +} + +YAML::Node JsonSerializer::json_to_yaml(const nlohmann::json & json) { + YAML::Node yaml; + + if (json.is_null()) { + return yaml; // Null node + } else if (json.is_boolean()) { + yaml = json.get(); + } else if (json.is_number_integer()) { + yaml = json.get(); + } else if (json.is_number_unsigned()) { + yaml = json.get(); + } else if (json.is_number_float()) { + yaml = json.get(); + } else if (json.is_string()) { + yaml = json.get(); + } else if (json.is_array()) { + for (const auto & elem : json) { + yaml.push_back(json_to_yaml(elem)); + } + } else if (json.is_object()) { + for (auto it = json.begin(); it != json.end(); ++it) { + yaml[it.key()] = json_to_yaml(it.value()); + } + } + + return yaml; +} + +const TypeInfo_Cpp * JsonSerializer::get_type_info_or_throw(const std::string & type_string) const { + const TypeInfo_Cpp * type_info = TypeCache::instance().get_message_type_info(type_string); + + if (type_info == nullptr) { + throw TypeNotFoundError(type_string); + } + + return type_info; +} + +rclcpp::SerializedMessage JsonSerializer::serialize(const std::string & type_string, + const nlohmann::json & json) const { + // Get type info for introspection + const TypeInfo_Cpp * type_info = get_type_info_or_throw(type_string); + + // Create a ROS message from JSON using dynmsg + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + RosMessage_Cpp ros_msg = from_json(type_info, json); + + // Get the type support for serialization + // We need to load the type support handle for rmw_serialize + auto parsed = TypeCache::parse_type_string(type_string); + if (!parsed) { + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); + throw SerializationError("Invalid type string format: " + type_string); + } + + const std::string & pkg_name = std::get<0>(*parsed); + const std::string & iface_type = std::get<1>(*parsed); + const std::string & type_name = std::get<2>(*parsed); + + // Get generic type support (for serialization) + // Use platform-independent library name resolution + std::string ts_lib_name = rcpputils::get_platform_library_name(pkg_name + "__rosidl_typesupport_cpp"); + std::string ts_func_name = + "rosidl_typesupport_cpp__get_message_type_support_handle__" + pkg_name + "__" + iface_type + "__" + type_name; + + // Use rcpputils to load the library + try { + auto ts_lib = std::make_shared(ts_lib_name); + auto get_ts_func = reinterpret_cast(ts_lib->get_symbol(ts_func_name)); + const rosidl_message_type_support_t * type_support = get_ts_func(); + + // Create serialized message + rclcpp::SerializedMessage serialized_msg; + + // Serialize using rmw + rmw_ret_t ret = rmw_serialize(ros_msg.data, type_support, &serialized_msg.get_rcl_serialized_message()); + + // Clean up ROS message + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); + + if (ret != RMW_RET_OK) { + throw SerializationError("rmw_serialize failed for type: " + type_string); + } + + return serialized_msg; + } catch (const TypeNotFoundError &) { + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); + throw; + } catch (const SerializationError &) { + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); + throw; + } catch (const std::runtime_error & e) { + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); + // Library load errors from SharedLibrary + std::string msg = e.what(); + if (msg.find("library") != std::string::npos || msg.find("symbol") != std::string::npos) { + throw TypeNotFoundError(type_string + " (" + msg + ")"); + } + throw SerializationError(std::string("Serialization failed: ") + e.what()); + } catch (const std::exception & e) { + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); + throw SerializationError(std::string("Serialization failed: ") + e.what()); + } +} + +nlohmann::json JsonSerializer::deserialize(const std::string & type_string, + const rclcpp::SerializedMessage & serialized_msg) const { + // Get type info for introspection + const TypeInfo_Cpp * type_info = get_type_info_or_throw(type_string); + + // Parse type string + auto parsed = TypeCache::parse_type_string(type_string); + if (!parsed) { + throw SerializationError("Invalid type string format: " + type_string); + } + + const std::string & pkg_name = std::get<0>(*parsed); + const std::string & iface_type = std::get<1>(*parsed); + const std::string & type_name = std::get<2>(*parsed); + + // Load the type support + // Use platform-independent library name resolution + std::string ts_lib_name = rcpputils::get_platform_library_name(pkg_name + "__rosidl_typesupport_cpp"); + std::string ts_func_name = + "rosidl_typesupport_cpp__get_message_type_support_handle__" + pkg_name + "__" + iface_type + "__" + type_name; + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + try { + auto ts_lib = std::make_shared(ts_lib_name); + auto get_ts_func = reinterpret_cast(ts_lib->get_symbol(ts_func_name)); + const rosidl_message_type_support_t * type_support = get_ts_func(); + + // Allocate a message to deserialize into + RosMessage_Cpp ros_msg; + dynmsg::cpp::ros_message_with_typeinfo_init(type_info, &ros_msg, &allocator); + + // Deserialize using rmw + rmw_ret_t ret = rmw_deserialize(&serialized_msg.get_rcl_serialized_message(), type_support, ros_msg.data); + + if (ret != RMW_RET_OK) { + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); + throw SerializationError("rmw_deserialize failed for type: " + type_string); + } + + // Convert to JSON + nlohmann::json result = to_json(type_info, ros_msg.data); + + // Clean up + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); + + return result; + } catch (const TypeNotFoundError &) { + throw; + } catch (const SerializationError &) { + throw; + } catch (const std::runtime_error & e) { + // Library load errors from SharedLibrary + std::string msg = e.what(); + if (msg.find("library") != std::string::npos || msg.find("symbol") != std::string::npos) { + throw TypeNotFoundError(type_string + " (" + msg + ")"); + } + throw SerializationError(std::string("Deserialization failed: ") + e.what()); + } catch (const std::exception & e) { + throw SerializationError(std::string("Deserialization failed: ") + e.what()); + } +} + +} // namespace ros2_medkit_serialization diff --git a/src/ros2_medkit_serialization/src/service_action_types.cpp b/src/ros2_medkit_serialization/src/service_action_types.cpp new file mode 100644 index 000000000..c43107e39 --- /dev/null +++ b/src/ros2_medkit_serialization/src/service_action_types.cpp @@ -0,0 +1,87 @@ +// Copyright 2026 Selfpatch +// +// 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_serialization/service_action_types.hpp" + +#include "ros2_medkit_serialization/type_cache.hpp" + +namespace ros2_medkit_serialization { + +std::string ServiceActionTypes::get_service_request_type(const std::string & service_type) { + return service_type + "_Request"; +} + +std::string ServiceActionTypes::get_service_response_type(const std::string & service_type) { + return service_type + "_Response"; +} + +std::string ServiceActionTypes::get_action_goal_type(const std::string & action_type) { + return action_type + "_Goal"; +} + +std::string ServiceActionTypes::get_action_result_type(const std::string & action_type) { + return action_type + "_Result"; +} + +std::string ServiceActionTypes::get_action_feedback_type(const std::string & action_type) { + return action_type + "_Feedback"; +} + +std::string ServiceActionTypes::get_action_send_goal_service_type(const std::string & action_type) { + return action_type + "_SendGoal"; +} + +std::string ServiceActionTypes::get_action_get_result_service_type(const std::string & action_type) { + return action_type + "_GetResult"; +} + +std::string ServiceActionTypes::get_action_send_goal_request_type(const std::string & action_type) { + return action_type + "_SendGoal_Request"; +} + +std::string ServiceActionTypes::get_action_send_goal_response_type(const std::string & action_type) { + return action_type + "_SendGoal_Response"; +} + +std::string ServiceActionTypes::get_action_get_result_request_type(const std::string & action_type) { + return action_type + "_GetResult_Request"; +} + +std::string ServiceActionTypes::get_action_get_result_response_type(const std::string & action_type) { + return action_type + "_GetResult_Response"; +} + +std::string ServiceActionTypes::get_action_feedback_message_type(const std::string & action_type) { + return action_type + "_FeedbackMessage"; +} + +std::optional> +ServiceActionTypes::parse_interface_type(const std::string & full_type) { + // Delegate to TypeCache::parse_type_string to avoid duplicate regex + return TypeCache::parse_type_string(full_type); +} + +bool ServiceActionTypes::is_service_type(const std::string & full_type) { + return full_type.find("/srv/") != std::string::npos; +} + +bool ServiceActionTypes::is_action_type(const std::string & full_type) { + return full_type.find("/action/") != std::string::npos; +} + +bool ServiceActionTypes::is_message_type(const std::string & full_type) { + return full_type.find("/msg/") != std::string::npos; +} + +} // namespace ros2_medkit_serialization diff --git a/src/ros2_medkit_serialization/src/type_cache.cpp b/src/ros2_medkit_serialization/src/type_cache.cpp new file mode 100644 index 000000000..0a73647ac --- /dev/null +++ b/src/ros2_medkit_serialization/src/type_cache.cpp @@ -0,0 +1,109 @@ +// Copyright 2026 Selfpatch +// +// 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_serialization/type_cache.hpp" + +#include +#include + +namespace ros2_medkit_serialization { + +TypeCache & TypeCache::instance() { + static TypeCache instance; + return instance; +} + +const TypeInfo_Cpp * TypeCache::get_message_type_info(const std::string & package_name, const std::string & type_name) { + // This overload assumes "msg" interface type for backward compatibility + return get_message_type_info(package_name, "msg", type_name); +} + +const TypeInfo_Cpp * TypeCache::get_message_type_info(const std::string & package_name, + const std::string & interface_type, + const std::string & type_name) { + const std::string key = make_key(package_name, interface_type, type_name); + + // Try read lock first (fast path for cache hits) + { + std::shared_lock lock(mutex_); + auto it = cache_.find(key); + if (it != cache_.end()) { + return it->second; + } + } + + // Load the type info using dynmsg with full interface type + FullInterfaceTypeName full_interface_type{package_name, interface_type, type_name}; + const TypeInfo_Cpp * type_info = dynmsg::cpp::get_type_info(full_interface_type); + + if (type_info != nullptr) { + std::unique_lock lock(mutex_); + cache_[key] = type_info; + } + + return type_info; +} + +const TypeInfo_Cpp * TypeCache::get_message_type_info(const std::string & full_type) { + auto parsed = parse_type_string(full_type); + if (!parsed.has_value()) { + return nullptr; + } + // Now we have (package, interface_type, type_name) + return get_message_type_info(std::get<0>(*parsed), std::get<1>(*parsed), std::get<2>(*parsed)); +} + +std::optional> +TypeCache::parse_type_string(const std::string & full_type) { + // Pattern: package/msg/TypeName or package/srv/TypeName or package/action/TypeName + static const std::regex type_regex(R"(^([a-zA-Z_][a-zA-Z0-9_]*)/(msg|srv|action)/(\w+)$)"); + + std::smatch match; + if (!std::regex_match(full_type, match, type_regex)) { + return std::nullopt; + } + + // Return (package, interface_type, type_name) + std::string package = match[1].str(); + std::string interface_type = match[2].str(); + std::string type_name = match[3].str(); + + return std::make_tuple(package, interface_type, type_name); +} + +bool TypeCache::is_cached(const std::string & package_name, const std::string & type_name) const { + std::shared_lock lock(mutex_); + return cache_.find(make_key(package_name, type_name)) != cache_.end(); +} + +void TypeCache::clear() { + std::unique_lock lock(mutex_); + cache_.clear(); +} + +size_t TypeCache::size() const { + std::shared_lock lock(mutex_); + return cache_.size(); +} + +std::string TypeCache::make_key(const std::string & package_name, const std::string & type_name) { + return package_name + "/msg/" + type_name; +} + +std::string TypeCache::make_key(const std::string & package_name, const std::string & interface_type, + const std::string & type_name) { + return package_name + "/" + interface_type + "/" + type_name; +} + +} // namespace ros2_medkit_serialization diff --git a/src/ros2_medkit_serialization/test/test_json_serializer.cpp b/src/ros2_medkit_serialization/test/test_json_serializer.cpp new file mode 100644 index 000000000..93f824844 --- /dev/null +++ b/src/ros2_medkit_serialization/test/test_json_serializer.cpp @@ -0,0 +1,338 @@ +// Copyright 2026 Selfpatch +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "ros2_medkit_serialization/json_serializer.hpp" +#include "ros2_medkit_serialization/type_cache.hpp" + +namespace ros2_medkit_serialization { + +class JsonSerializerTest : public ::testing::Test { + protected: + void SetUp() override { + TypeCache::instance().clear(); + } + + JsonSerializer serializer_; +}; + +// YAML ↔ JSON conversion tests + +TEST_F(JsonSerializerTest, YamlToJsonNull) { + YAML::Node yaml; + auto json = JsonSerializer::yaml_to_json(yaml); + EXPECT_TRUE(json.is_null()); +} + +TEST_F(JsonSerializerTest, YamlToJsonBool) { + YAML::Node yaml; + yaml = true; + auto json = JsonSerializer::yaml_to_json(yaml); + EXPECT_TRUE(json.is_boolean()); + EXPECT_TRUE(json.get()); +} + +TEST_F(JsonSerializerTest, YamlToJsonInteger) { + YAML::Node yaml; + yaml = 42; + auto json = JsonSerializer::yaml_to_json(yaml); + EXPECT_TRUE(json.is_number_integer()); + EXPECT_EQ(json.get(), 42); +} + +TEST_F(JsonSerializerTest, YamlToJsonFloat) { + YAML::Node yaml; + yaml = 3.14; + auto json = JsonSerializer::yaml_to_json(yaml); + EXPECT_TRUE(json.is_number_float()); + EXPECT_NEAR(json.get(), 3.14, 0.001); +} + +TEST_F(JsonSerializerTest, YamlToJsonString) { + YAML::Node yaml; + yaml = "hello"; + auto json = JsonSerializer::yaml_to_json(yaml); + EXPECT_TRUE(json.is_string()); + EXPECT_EQ(json.get(), "hello"); +} + +TEST_F(JsonSerializerTest, YamlToJsonArray) { + YAML::Node yaml; + yaml.push_back(1); + yaml.push_back(2); + yaml.push_back(3); + auto json = JsonSerializer::yaml_to_json(yaml); + EXPECT_TRUE(json.is_array()); + EXPECT_EQ(json.size(), 3U); + EXPECT_EQ(json[0].get(), 1); + EXPECT_EQ(json[1].get(), 2); + EXPECT_EQ(json[2].get(), 3); +} + +TEST_F(JsonSerializerTest, YamlToJsonObject) { + YAML::Node yaml; + yaml["name"] = "test"; + yaml["value"] = 123; + auto json = JsonSerializer::yaml_to_json(yaml); + EXPECT_TRUE(json.is_object()); + EXPECT_EQ(json["name"].get(), "test"); + EXPECT_EQ(json["value"].get(), 123); +} + +TEST_F(JsonSerializerTest, JsonToYamlNull) { + nlohmann::json json = nullptr; + auto yaml = JsonSerializer::json_to_yaml(json); + EXPECT_TRUE(yaml.IsNull()); +} + +TEST_F(JsonSerializerTest, JsonToYamlBool) { + nlohmann::json json = true; + auto yaml = JsonSerializer::json_to_yaml(json); + EXPECT_TRUE(yaml.IsScalar()); + EXPECT_TRUE(yaml.as()); +} + +TEST_F(JsonSerializerTest, JsonToYamlInteger) { + nlohmann::json json = 42; + auto yaml = JsonSerializer::json_to_yaml(json); + EXPECT_TRUE(yaml.IsScalar()); + EXPECT_EQ(yaml.as(), 42); +} + +TEST_F(JsonSerializerTest, JsonToYamlFloat) { + nlohmann::json json = 3.14; + auto yaml = JsonSerializer::json_to_yaml(json); + EXPECT_TRUE(yaml.IsScalar()); + EXPECT_NEAR(yaml.as(), 3.14, 0.001); +} + +TEST_F(JsonSerializerTest, JsonToYamlString) { + nlohmann::json json = "hello"; + auto yaml = JsonSerializer::json_to_yaml(json); + EXPECT_TRUE(yaml.IsScalar()); + EXPECT_EQ(yaml.as(), "hello"); +} + +TEST_F(JsonSerializerTest, JsonToYamlArray) { + nlohmann::json json = {1, 2, 3}; + auto yaml = JsonSerializer::json_to_yaml(json); + EXPECT_TRUE(yaml.IsSequence()); + EXPECT_EQ(yaml.size(), 3U); +} + +TEST_F(JsonSerializerTest, JsonToYamlObject) { + nlohmann::json json = {{"name", "test"}, {"value", 123}}; + auto yaml = JsonSerializer::json_to_yaml(json); + EXPECT_TRUE(yaml.IsMap()); + EXPECT_EQ(yaml["name"].as(), "test"); + EXPECT_EQ(yaml["value"].as(), 123); +} + +// Message serialization tests + +TEST_F(JsonSerializerTest, ToJsonStdMsgsString) { + std_msgs::msg::String msg; + msg.data = "hello world"; + + auto type_info = TypeCache::instance().get_message_type_info("std_msgs", "String"); + ASSERT_NE(type_info, nullptr); + + auto json = serializer_.to_json(type_info, &msg); + EXPECT_TRUE(json.is_object()); + EXPECT_EQ(json["data"].get(), "hello world"); +} + +TEST_F(JsonSerializerTest, ToJsonStdMsgsInt32) { + std_msgs::msg::Int32 msg; + msg.data = 42; + + auto type_info = TypeCache::instance().get_message_type_info("std_msgs", "Int32"); + ASSERT_NE(type_info, nullptr); + + auto json = serializer_.to_json(type_info, &msg); + EXPECT_TRUE(json.is_object()); + EXPECT_EQ(json["data"].get(), 42); +} + +TEST_F(JsonSerializerTest, ToJsonGeometryMsgsPoint) { + geometry_msgs::msg::Point msg; + msg.x = 1.0; + msg.y = 2.0; + msg.z = 3.0; + + auto type_info = TypeCache::instance().get_message_type_info("geometry_msgs", "Point"); + ASSERT_NE(type_info, nullptr); + + auto json = serializer_.to_json(type_info, &msg); + EXPECT_TRUE(json.is_object()); + EXPECT_NEAR(json["x"].get(), 1.0, 0.001); + EXPECT_NEAR(json["y"].get(), 2.0, 0.001); + EXPECT_NEAR(json["z"].get(), 3.0, 0.001); +} + +TEST_F(JsonSerializerTest, ToJsonWithTypeString) { + std_msgs::msg::String msg; + msg.data = "test"; + + auto json = serializer_.to_json("std_msgs/msg/String", &msg); + EXPECT_EQ(json["data"].get(), "test"); +} + +TEST_F(JsonSerializerTest, FromJsonStdMsgsString) { + nlohmann::json json = {{"data", "hello"}}; + + auto type_info = TypeCache::instance().get_message_type_info("std_msgs", "String"); + ASSERT_NE(type_info, nullptr); + + auto ros_msg = serializer_.from_json(type_info, json); + ASSERT_NE(ros_msg.data, nullptr); + + auto * msg = reinterpret_cast(ros_msg.data); + EXPECT_EQ(msg->data, "hello"); + + // Clean up + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); +} + +TEST_F(JsonSerializerTest, FromJsonGeometryMsgsPoint) { + nlohmann::json json = {{"x", 1.5}, {"y", 2.5}, {"z", 3.5}}; + + auto ros_msg = serializer_.from_json("geometry_msgs/msg/Point", json); + ASSERT_NE(ros_msg.data, nullptr); + + auto * msg = reinterpret_cast(ros_msg.data); + EXPECT_NEAR(msg->x, 1.5, 0.001); + EXPECT_NEAR(msg->y, 2.5, 0.001); + EXPECT_NEAR(msg->z, 3.5, 0.001); + + // Clean up + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); +} + +TEST_F(JsonSerializerTest, FromJsonToExistingMessage) { + nlohmann::json json = {{"data", "populated"}}; + std_msgs::msg::String msg; + + auto type_info = TypeCache::instance().get_message_type_info("std_msgs", "String"); + ASSERT_NE(type_info, nullptr); + + serializer_.from_json_to_message(type_info, json, &msg); + EXPECT_EQ(msg.data, "populated"); +} + +// Schema tests + +TEST_F(JsonSerializerTest, GetSchemaStdMsgsString) { + auto schema = serializer_.get_schema("std_msgs/msg/String"); + EXPECT_EQ(schema["type"].get(), "object"); + EXPECT_TRUE(schema.contains("properties")); + EXPECT_TRUE(schema["properties"].contains("data")); + EXPECT_EQ(schema["properties"]["data"]["type"].get(), "string"); +} + +TEST_F(JsonSerializerTest, GetSchemaGeometryMsgsPoint) { + auto schema = serializer_.get_schema("geometry_msgs/msg/Point"); + EXPECT_EQ(schema["type"].get(), "object"); + EXPECT_TRUE(schema["properties"].contains("x")); + EXPECT_TRUE(schema["properties"].contains("y")); + EXPECT_TRUE(schema["properties"].contains("z")); + EXPECT_EQ(schema["properties"]["x"]["type"].get(), "number"); +} + +// Default values tests + +TEST_F(JsonSerializerTest, GetDefaultsStdMsgsString) { + auto defaults = serializer_.get_defaults("std_msgs/msg/String"); + EXPECT_TRUE(defaults.is_object()); + EXPECT_TRUE(defaults.contains("data")); + EXPECT_EQ(defaults["data"].get(), ""); +} + +TEST_F(JsonSerializerTest, GetDefaultsStdMsgsInt32) { + auto defaults = serializer_.get_defaults("std_msgs/msg/Int32"); + EXPECT_TRUE(defaults.is_object()); + EXPECT_TRUE(defaults.contains("data")); + EXPECT_EQ(defaults["data"].get(), 0); +} + +// Error handling tests + +TEST_F(JsonSerializerTest, ToJsonNullTypeInfoThrows) { + int dummy = 0; + EXPECT_THROW(serializer_.to_json(nullptr, &dummy), JsonConversionError); +} + +TEST_F(JsonSerializerTest, ToJsonNullDataThrows) { + auto type_info = TypeCache::instance().get_message_type_info("std_msgs", "String"); + ASSERT_NE(type_info, nullptr); + EXPECT_THROW(serializer_.to_json(type_info, nullptr), JsonConversionError); +} + +TEST_F(JsonSerializerTest, FromJsonNullTypeInfoThrows) { + nlohmann::json json = {{"data", "test"}}; + EXPECT_THROW(serializer_.from_json(nullptr, json), JsonConversionError); +} + +TEST_F(JsonSerializerTest, InvalidTypeStringThrows) { + std_msgs::msg::String msg; + EXPECT_THROW(serializer_.to_json("invalid/type/Name", &msg), TypeNotFoundError); +} + +// Round-trip tests + +TEST_F(JsonSerializerTest, RoundTripStdMsgsString) { + std_msgs::msg::String original; + original.data = "round trip test"; + + auto json = serializer_.to_json("std_msgs/msg/String", &original); + auto ros_msg = serializer_.from_json("std_msgs/msg/String", json); + + auto * restored = reinterpret_cast(ros_msg.data); + EXPECT_EQ(restored->data, original.data); + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); +} + +TEST_F(JsonSerializerTest, RoundTripGeometryMsgsPoint) { + geometry_msgs::msg::Point original; + original.x = 1.234; + original.y = 5.678; + original.z = 9.012; + + auto json = serializer_.to_json("geometry_msgs/msg/Point", &original); + auto ros_msg = serializer_.from_json("geometry_msgs/msg/Point", json); + + auto * restored = reinterpret_cast(ros_msg.data); + EXPECT_NEAR(restored->x, original.x, 0.0001); + EXPECT_NEAR(restored->y, original.y, 0.0001); + EXPECT_NEAR(restored->z, original.z, 0.0001); + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + dynmsg::cpp::ros_message_destroy_with_allocator(&ros_msg, &allocator); +} + +} // namespace ros2_medkit_serialization + +int main(int argc, char ** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/ros2_medkit_serialization/test/test_service_action_types.cpp b/src/ros2_medkit_serialization/test/test_service_action_types.cpp new file mode 100644 index 000000000..beaebef88 --- /dev/null +++ b/src/ros2_medkit_serialization/test/test_service_action_types.cpp @@ -0,0 +1,99 @@ +// Copyright 2026 Selfpatch +// +// 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 "ros2_medkit_serialization/service_action_types.hpp" + +namespace ros2_medkit_serialization { + +class ServiceActionTypesTest : public ::testing::Test {}; + +TEST_F(ServiceActionTypesTest, GetServiceRequestType) { + EXPECT_EQ(ServiceActionTypes::get_service_request_type("std_srvs/srv/SetBool"), "std_srvs/srv/SetBool_Request"); +} + +TEST_F(ServiceActionTypesTest, GetServiceResponseType) { + EXPECT_EQ(ServiceActionTypes::get_service_response_type("std_srvs/srv/SetBool"), "std_srvs/srv/SetBool_Response"); +} + +TEST_F(ServiceActionTypesTest, GetActionGoalType) { + EXPECT_EQ(ServiceActionTypes::get_action_goal_type("example_interfaces/action/Fibonacci"), + "example_interfaces/action/Fibonacci_Goal"); +} + +TEST_F(ServiceActionTypesTest, GetActionResultType) { + EXPECT_EQ(ServiceActionTypes::get_action_result_type("example_interfaces/action/Fibonacci"), + "example_interfaces/action/Fibonacci_Result"); +} + +TEST_F(ServiceActionTypesTest, GetActionFeedbackType) { + EXPECT_EQ(ServiceActionTypes::get_action_feedback_type("example_interfaces/action/Fibonacci"), + "example_interfaces/action/Fibonacci_Feedback"); +} + +TEST_F(ServiceActionTypesTest, ParseInterfaceTypeMsg) { + auto result = ServiceActionTypes::parse_interface_type("std_msgs/msg/String"); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(std::get<0>(*result), "std_msgs"); + EXPECT_EQ(std::get<1>(*result), "msg"); + EXPECT_EQ(std::get<2>(*result), "String"); +} + +TEST_F(ServiceActionTypesTest, ParseInterfaceTypeSrv) { + auto result = ServiceActionTypes::parse_interface_type("std_srvs/srv/SetBool"); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(std::get<0>(*result), "std_srvs"); + EXPECT_EQ(std::get<1>(*result), "srv"); + EXPECT_EQ(std::get<2>(*result), "SetBool"); +} + +TEST_F(ServiceActionTypesTest, ParseInterfaceTypeAction) { + auto result = ServiceActionTypes::parse_interface_type("example_interfaces/action/Fibonacci"); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(std::get<0>(*result), "example_interfaces"); + EXPECT_EQ(std::get<1>(*result), "action"); + EXPECT_EQ(std::get<2>(*result), "Fibonacci"); +} + +TEST_F(ServiceActionTypesTest, ParseInterfaceTypeInvalid) { + EXPECT_FALSE(ServiceActionTypes::parse_interface_type("invalid").has_value()); + EXPECT_FALSE(ServiceActionTypes::parse_interface_type("").has_value()); + EXPECT_FALSE(ServiceActionTypes::parse_interface_type("pkg/type").has_value()); +} + +TEST_F(ServiceActionTypesTest, IsServiceType) { + EXPECT_TRUE(ServiceActionTypes::is_service_type("std_srvs/srv/SetBool")); + EXPECT_FALSE(ServiceActionTypes::is_service_type("std_msgs/msg/String")); + EXPECT_FALSE(ServiceActionTypes::is_service_type("example_interfaces/action/Fibonacci")); +} + +TEST_F(ServiceActionTypesTest, IsActionType) { + EXPECT_TRUE(ServiceActionTypes::is_action_type("example_interfaces/action/Fibonacci")); + EXPECT_FALSE(ServiceActionTypes::is_action_type("std_msgs/msg/String")); + EXPECT_FALSE(ServiceActionTypes::is_action_type("std_srvs/srv/SetBool")); +} + +TEST_F(ServiceActionTypesTest, IsMessageType) { + EXPECT_TRUE(ServiceActionTypes::is_message_type("std_msgs/msg/String")); + EXPECT_FALSE(ServiceActionTypes::is_message_type("std_srvs/srv/SetBool")); + EXPECT_FALSE(ServiceActionTypes::is_message_type("example_interfaces/action/Fibonacci")); +} + +} // namespace ros2_medkit_serialization + +int main(int argc, char ** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/ros2_medkit_serialization/test/test_type_cache.cpp b/src/ros2_medkit_serialization/test/test_type_cache.cpp new file mode 100644 index 000000000..886b585a0 --- /dev/null +++ b/src/ros2_medkit_serialization/test/test_type_cache.cpp @@ -0,0 +1,183 @@ +// Copyright 2026 Selfpatch +// +// 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 "ros2_medkit_serialization/type_cache.hpp" + +namespace ros2_medkit_serialization { + +class TypeCacheTest : public ::testing::Test { + protected: + void SetUp() override { + // Clear cache before each test + TypeCache::instance().clear(); + } +}; + +TEST_F(TypeCacheTest, SingletonInstance) { + auto & cache1 = TypeCache::instance(); + auto & cache2 = TypeCache::instance(); + EXPECT_EQ(&cache1, &cache2); +} + +TEST_F(TypeCacheTest, ParseTypeStringValid) { + auto result = TypeCache::parse_type_string("std_msgs/msg/String"); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(std::get<0>(*result), "std_msgs"); + EXPECT_EQ(std::get<1>(*result), "msg"); + EXPECT_EQ(std::get<2>(*result), "String"); +} + +TEST_F(TypeCacheTest, ParseTypeStringSrv) { + auto result = TypeCache::parse_type_string("std_srvs/srv/SetBool"); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(std::get<0>(*result), "std_srvs"); + EXPECT_EQ(std::get<1>(*result), "srv"); + EXPECT_EQ(std::get<2>(*result), "SetBool"); +} + +TEST_F(TypeCacheTest, ParseTypeStringAction) { + auto result = TypeCache::parse_type_string("example_interfaces/action/Fibonacci"); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(std::get<0>(*result), "example_interfaces"); + EXPECT_EQ(std::get<1>(*result), "action"); + EXPECT_EQ(std::get<2>(*result), "Fibonacci"); +} + +TEST_F(TypeCacheTest, ParseTypeStringInvalid) { + EXPECT_FALSE(TypeCache::parse_type_string("invalid").has_value()); + EXPECT_FALSE(TypeCache::parse_type_string("").has_value()); + EXPECT_FALSE(TypeCache::parse_type_string("pkg/type").has_value()); + EXPECT_FALSE(TypeCache::parse_type_string("pkg/invalid/Type").has_value()); +} + +TEST_F(TypeCacheTest, GetMessageTypeInfoStdMsgsString) { + const auto * type_info = TypeCache::instance().get_message_type_info("std_msgs", "String"); + ASSERT_NE(type_info, nullptr); + EXPECT_STREQ(type_info->message_name_, "String"); + EXPECT_STREQ(type_info->message_namespace_, "std_msgs::msg"); +} + +TEST_F(TypeCacheTest, GetMessageTypeInfoFromFullType) { + const auto * type_info = TypeCache::instance().get_message_type_info("std_msgs/msg/String"); + ASSERT_NE(type_info, nullptr); + EXPECT_STREQ(type_info->message_name_, "String"); +} + +TEST_F(TypeCacheTest, CachingWorks) { + auto & cache = TypeCache::instance(); + EXPECT_EQ(cache.size(), 0U); + + // First load + const auto * type_info1 = cache.get_message_type_info("std_msgs", "String"); + ASSERT_NE(type_info1, nullptr); + EXPECT_EQ(cache.size(), 1U); + EXPECT_TRUE(cache.is_cached("std_msgs", "String")); + + // Second load - should return cached + const auto * type_info2 = cache.get_message_type_info("std_msgs", "String"); + EXPECT_EQ(type_info1, type_info2); + EXPECT_EQ(cache.size(), 1U); +} + +TEST_F(TypeCacheTest, ClearCache) { + auto & cache = TypeCache::instance(); + cache.get_message_type_info("std_msgs", "String"); + EXPECT_GT(cache.size(), 0U); + + cache.clear(); + EXPECT_EQ(cache.size(), 0U); + EXPECT_FALSE(cache.is_cached("std_msgs", "String")); +} + +TEST_F(TypeCacheTest, NonExistentTypeReturnsNull) { + const auto * type_info = TypeCache::instance().get_message_type_info("nonexistent_pkg", "FakeType"); + EXPECT_EQ(type_info, nullptr); +} + +TEST_F(TypeCacheTest, GetServiceRequestType) { + // Service request types use srv interface type + const auto * type_info = TypeCache::instance().get_message_type_info("std_srvs/srv/Trigger_Request"); + ASSERT_NE(type_info, nullptr); + EXPECT_STREQ(type_info->message_name_, "Trigger_Request"); +} + +TEST_F(TypeCacheTest, GetServiceResponseType) { + // Service response types use srv interface type + const auto * type_info = TypeCache::instance().get_message_type_info("std_srvs/srv/Trigger_Response"); + ASSERT_NE(type_info, nullptr); + EXPECT_STREQ(type_info->message_name_, "Trigger_Response"); + // Trigger_Response has success (bool) and message (string) + EXPECT_EQ(type_info->member_count_, 2U); +} + +TEST_F(TypeCacheTest, GetActionGoalType) { + // Action goal types use action interface type + const auto * type_info = TypeCache::instance().get_message_type_info("example_interfaces/action/Fibonacci_Goal"); + ASSERT_NE(type_info, nullptr); + EXPECT_STREQ(type_info->message_name_, "Fibonacci_Goal"); + // Fibonacci_Goal has order (int32) + EXPECT_EQ(type_info->member_count_, 1U); +} + +TEST_F(TypeCacheTest, GetActionResultType) { + const auto * type_info = TypeCache::instance().get_message_type_info("example_interfaces/action/Fibonacci_Result"); + ASSERT_NE(type_info, nullptr); + EXPECT_STREQ(type_info->message_name_, "Fibonacci_Result"); +} + +TEST_F(TypeCacheTest, GetActionFeedbackType) { + const auto * type_info = TypeCache::instance().get_message_type_info("example_interfaces/action/Fibonacci_Feedback"); + ASSERT_NE(type_info, nullptr); + EXPECT_STREQ(type_info->message_name_, "Fibonacci_Feedback"); +} + +TEST_F(TypeCacheTest, ThreadSafety) { + auto & cache = TypeCache::instance(); + constexpr int NUM_THREADS = 10; + constexpr int ITERATIONS = 100; + + std::vector threads; + std::atomic success_count{0}; + + for (int i = 0; i < NUM_THREADS; ++i) { + threads.emplace_back([&cache, &success_count]() { + for (int j = 0; j < ITERATIONS; ++j) { + const auto * type_info = cache.get_message_type_info("std_msgs", "String"); + if (type_info != nullptr) { + ++success_count; + } + } + }); + } + + for (auto & t : threads) { + t.join(); + } + + EXPECT_EQ(success_count.load(), NUM_THREADS * ITERATIONS); + // Cache should have exactly one entry despite concurrent access + EXPECT_EQ(cache.size(), 1U); +} + +} // namespace ros2_medkit_serialization + +int main(int argc, char ** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}