diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 437e24b4..b4a33c4f 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -24,8 +24,6 @@ jobs: - name: Checkout repository uses: actions/checkout@v4 - with: - submodules: recursive - name: Set up ROS 2 Jazzy uses: ros-tooling/setup-ros@v0.7 @@ -45,8 +43,7 @@ jobs: source /opt/ros/jazzy/setup.bash colcon build --symlink-install \ --cmake-args -DCMAKE_BUILD_TYPE=Release \ - --event-handlers console_direct+ \ - --packages-ignore test_dynmsg dynmsg_demo + --event-handlers console_direct+ - name: Run linters (clang-format, clang-tidy, etc.) run: | @@ -54,8 +51,7 @@ jobs: source install/setup.bash colcon test --return-code-on-test-failure \ --ctest-args -L linter \ - --event-handlers console_direct+ \ - --packages-ignore test_dynmsg dynmsg_demo + --event-handlers console_direct+ - name: Run unit and integration tests timeout-minutes: 15 @@ -64,8 +60,7 @@ jobs: source install/setup.bash colcon test --return-code-on-test-failure \ --ctest-args -LE linter \ - --event-handlers console_direct+ \ - --packages-ignore test_dynmsg dynmsg_demo + --event-handlers console_direct+ - name: Show test results if: always() @@ -97,8 +92,6 @@ jobs: - name: Checkout repository uses: actions/checkout@v4 - with: - submodules: recursive - name: Set up ROS 2 Jazzy uses: ros-tooling/setup-ros@v0.7 @@ -118,8 +111,7 @@ jobs: source /opt/ros/jazzy/setup.bash colcon build --symlink-install \ --cmake-args -DCMAKE_BUILD_TYPE=Debug -DENABLE_COVERAGE=ON \ - --event-handlers console_direct+ \ - --packages-ignore test_dynmsg dynmsg_demo + --event-handlers console_direct+ - name: Run unit and integration tests for coverage run: | @@ -127,8 +119,7 @@ jobs: source install/setup.bash colcon test \ --ctest-args -LE linter \ - --event-handlers console_direct+ \ - --packages-ignore test_dynmsg dynmsg_demo + --event-handlers console_direct+ - name: Generate coverage report run: | diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 466fe057..00000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "src/dynamic_message_introspection"] - path = src/dynamic_message_introspection - url = https://github.com/selfpatch/dynamic_message_introspection.git diff --git a/src/dynamic_message_introspection b/src/dynamic_message_introspection deleted file mode 160000 index b38e36c2..00000000 --- a/src/dynamic_message_introspection +++ /dev/null @@ -1 +0,0 @@ -Subproject commit b38e36c2e2ff2a490d1b1e9db99c55ecb5dd5a07 diff --git a/src/ros2_medkit_gateway/src/operation_manager.cpp b/src/ros2_medkit_gateway/src/operation_manager.cpp index 004782d0..bc9c1075 100644 --- a/src/ros2_medkit_gateway/src/operation_manager.cpp +++ b/src/ros2_medkit_gateway/src/operation_manager.cpp @@ -21,6 +21,7 @@ #include #include "ros2_medkit_serialization/json_serializer.hpp" +#include "ros2_medkit_serialization/message_cleanup.hpp" #include "ros2_medkit_serialization/serialization_error.hpp" #include "ros2_medkit_serialization/service_action_types.hpp" #include "ros2_medkit_serialization/type_cache.hpp" @@ -142,7 +143,6 @@ ServiceCallResult OperationManager::call_service(const std::string & service_pat // 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()); @@ -157,7 +157,7 @@ ServiceCallResult OperationManager::call_service(const std::string & service_pat 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); + ros2_medkit_serialization::destroy_ros_message(&ros_request); result.success = false; result.error_message = "Service call timed out (" + std::to_string(service_call_timeout_sec_) + "s): " + service_path; @@ -165,7 +165,7 @@ ServiceCallResult OperationManager::call_service(const std::string & service_pat } // Clean up request message after sending - dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + ros2_medkit_serialization::destroy_ros_message(&ros_request); // Step 7: Get response and deserialize auto response_ptr = future_and_id.get(); @@ -412,7 +412,6 @@ ActionSendGoalResult OperationManager::send_action_goal(const std::string & acti // 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()); @@ -426,13 +425,13 @@ ActionSendGoalResult OperationManager::send_action_goal(const std::string & acti 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); + ros2_medkit_serialization::destroy_ros_message(&ros_request); result.error_message = "Send goal timed out"; return result; } // Clean up request message after sending - dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + ros2_medkit_serialization::destroy_ros_message(&ros_request); // Step 6: Deserialize response auto response_ptr = future_and_id.get(); @@ -551,7 +550,6 @@ ActionCancelResult OperationManager::cancel_action_goal(const std::string & acti // 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()); @@ -562,11 +560,11 @@ ActionCancelResult OperationManager::cancel_action_goal(const std::string & acti 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); + ros2_medkit_serialization::destroy_ros_message(&ros_request); result.error_message = "Cancel request timed out"; return result; } - dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + ros2_medkit_serialization::destroy_ros_message(&ros_request); auto response_ptr = future_and_id.get(); if (response_ptr == nullptr) { @@ -646,7 +644,6 @@ ActionGetResultResult OperationManager::get_action_result(const std::string & ac // 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()); @@ -657,11 +654,11 @@ ActionGetResultResult OperationManager::get_action_result(const std::string & ac 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); + ros2_medkit_serialization::destroy_ros_message(&ros_request); result.error_message = "Get result timed out"; return result; } - dynmsg::cpp::ros_message_destroy_with_allocator(&ros_request, &allocator); + ros2_medkit_serialization::destroy_ros_message(&ros_request); auto response_ptr = future_and_id.get(); if (response_ptr == nullptr) { diff --git a/src/ros2_medkit_serialization/CMakeLists.txt b/src/ros2_medkit_serialization/CMakeLists.txt index fe52dc4d..d59bbeef 100644 --- a/src/ros2_medkit_serialization/CMakeLists.txt +++ b/src/ros2_medkit_serialization/CMakeLists.txt @@ -20,7 +20,9 @@ endif() # Find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(dynmsg REQUIRED) +find_package(rcutils REQUIRED) +find_package(rosidl_runtime_c REQUIRED) +find_package(rosidl_typesupport_introspection_c REQUIRED) find_package(rosidl_typesupport_introspection_cpp REQUIRED) find_package(rosidl_runtime_cpp REQUIRED) find_package(rcpputils REQUIRED) @@ -33,6 +35,14 @@ add_library(${PROJECT_NAME} src/json_serializer.cpp src/type_cache.cpp src/service_action_types.cpp + src/message_cleanup.cpp + # Vendored dynmsg sources (C++ API only) + src/vendored/dynmsg/typesupport.cpp + src/vendored/dynmsg/message_reading_cpp.cpp + src/vendored/dynmsg/msg_parser_cpp.cpp + src/vendored/dynmsg/string_utils.cpp + src/vendored/dynmsg/vector_utils.cpp + src/vendored/dynmsg/yaml_utils.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC @@ -42,7 +52,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC ament_target_dependencies(${PROJECT_NAME} rclcpp - dynmsg + rcutils + rosidl_runtime_c + rosidl_typesupport_introspection_c rosidl_typesupport_introspection_cpp rosidl_runtime_cpp rcpputils @@ -64,7 +76,9 @@ endif() ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_dependencies( rclcpp - dynmsg + rcutils + rosidl_runtime_c + rosidl_typesupport_introspection_c rosidl_typesupport_introspection_cpp rosidl_runtime_cpp rcpputils @@ -94,26 +108,40 @@ if(BUILD_TESTING) 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 + # Exclude conflicting linters and those we configure manually list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify ament_cmake_cpplint ament_cmake_clang_tidy + ament_cmake_clang_format ) ament_lint_auto_find_test_dependencies() - # Configure clang-tidy manually with increased timeout + # Configure clang-format manually for non-vendored files only + find_package(ament_cmake_clang_format REQUIRED) + set(_clang_format_config "${CMAKE_CURRENT_SOURCE_DIR}/../../.clang-format") + ament_clang_format( + CONFIG_FILE "${_clang_format_config}" + "include/ros2_medkit_serialization/json_serializer.hpp" + "include/ros2_medkit_serialization/message_cleanup.hpp" + "include/ros2_medkit_serialization/serialization_error.hpp" + "include/ros2_medkit_serialization/service_action_types.hpp" + "include/ros2_medkit_serialization/type_cache.hpp" + "src/json_serializer.cpp" + "src/message_cleanup.cpp" + "src/service_action_types.cpp" + "src/type_cache.cpp" + "test/test_json_serializer.cpp" + "test/test_service_action_types.cpp" + "test/test_type_cache.cpp" + ) + + # Configure clang-tidy manually for non-vendored files only + set(_clang_tidy_config "${CMAKE_CURRENT_SOURCE_DIR}/../../.clang-tidy") ament_clang_tidy( "${CMAKE_CURRENT_BINARY_DIR}" - CONFIG_FILE "${ament_cmake_clang_tidy_CONFIG_FILE}" - HEADER_FILTER "${ament_cmake_clang_tidy_HEADER_FILTER}" + CONFIG_FILE "${_clang_tidy_config}" + HEADER_FILTER "^${CMAKE_CURRENT_SOURCE_DIR}/include/ros2_medkit_serialization/[^v].*" TIMEOUT 300 ) diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/message_cleanup.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/message_cleanup.hpp new file mode 100644 index 00000000..e405d738 --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/message_cleanup.hpp @@ -0,0 +1,33 @@ +// 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__MESSAGE_CLEANUP_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__MESSAGE_CLEANUP_HPP_ + +#include "ros2_medkit_serialization/vendored/dynmsg/typesupport.hpp" + +namespace ros2_medkit_serialization { + +/// Destroy a dynamically allocated ROS message created by JsonSerializer::from_json() +/// +/// This function properly cleans up messages allocated via the serialization library. +/// It calls the message's finalization function and deallocates the memory. +/// +/// @param ros_msg Pointer to the RosMessage_Cpp structure to destroy. +/// If nullptr or if ros_msg->data is nullptr, this is a no-op. +void destroy_ros_message(RosMessage_Cpp * ros_msg); + +} // namespace ros2_medkit_serialization + +#endif // ROS2_MEDKIT_SERIALIZATION__MESSAGE_CLEANUP_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 index 4e13da3d..b4255279 100644 --- a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/type_cache.hpp +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/type_cache.hpp @@ -24,7 +24,7 @@ #include #include -#include "dynmsg/typesupport.hpp" +#include "ros2_medkit_serialization/vendored/dynmsg/typesupport.hpp" namespace ros2_medkit_serialization { diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/config.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/config.hpp new file mode 100644 index 00000000..56170e24 --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/config.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 Christophe Bedard +// +// 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__VENDORED__DYNMSG__CONFIG_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__CONFIG_HPP_ + +// Vendored from dynmsg - configuration hardcoded for ros2_medkit_serialization + +// If DYNMSG_VALUE_ONLY is defined, the member's value will be used directly, e.g. +// frame_id: "my_frame" +// instead of providing the type name as well as the value, e.g. +// frame_id: +// type: "string" +// value: "my_frame" +// This also means that, with DYNMSG_VALUE_ONLY, the resulting +// YAML object can be converted back into a message. +#define DYNMSG_VALUE_ONLY + +// [u]int8_t is handled/parsed as a char by yaml-cpp, so force to an intermediate/other type. +// We convert them to string for msg->YAML and then back from string for YAML->msg. +// https://github.com/jbeder/yaml-cpp/issues/201 +// (does not appear to be fixed in 0.6.3 or 0.7.0) +#define DYNMSG_YAML_CPP_BAD_INT8_HANDLING + +// If DYNMSG_PARSER_DEBUG is defined, some debugging-related stuff will be printed. +// #define DYNMSG_PARSER_DEBUG + +#ifdef DYNMSG_PARSER_DEBUG +# define DYNMSG_DEBUG(code) code +#else +# define DYNMSG_DEBUG(code) ((void) (0)) +#endif // DYNMSG_PARSER_DEBUG + +#endif // ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__CONFIG_HPP_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/message_reading.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/message_reading.hpp new file mode 100644 index 00000000..0682bec8 --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/message_reading.hpp @@ -0,0 +1,59 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2021 Christophe Bedard +// +// 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__VENDORED__DYNMSG__MESSAGE_READING_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__MESSAGE_READING_HPP_ + +#include + +#include "ros2_medkit_serialization/vendored/dynmsg/typesupport.hpp" + +namespace dynmsg +{ + +namespace c +{ + +/// Parse a ROS message stored in a raw buffer into a YAML representation. +/** + * The "message" argument contains both a pointer to the raw buffer and a pointer to the ROS type's + * introspection information as retrieved from an introspection type support library. (See the + * "get_type_info" function in typesupport_utils.hpp for loading introspection type support.) + * + * This function will use the provided introspection information to read the binary data out of the + * ROS message-containing raw buffer and convert it into a YAML representation. The YAML + * representation is a tree structure, with each node in the tree being a field in the message. + * Each field is represented by two values: the ROS type of the field, in a textual representation, + * and the value. For an example of the YAML structure, run the CLI tool and echo a topic; the + * resulting YAML printed to the terminal is the structure used. + */ +YAML::Node message_to_yaml(const RosMessage & message); + +} // namespace c + +namespace cpp +{ + +/// C++ version of dynmsg::c::message_to_yaml(). +/** + * \see dynmsg::c::message_to_yaml() + */ +YAML::Node message_to_yaml(const RosMessage_Cpp & message); + +} // namespace cpp + +} // namespace dynmsg + +#endif // ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__MESSAGE_READING_HPP_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/msg_parser.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/msg_parser.hpp new file mode 100644 index 00000000..ef29ab68 --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/msg_parser.hpp @@ -0,0 +1,88 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2021 Christophe Bedard +// +// 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__VENDORED__DYNMSG__MSG_PARSER_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__MSG_PARSER_HPP_ + +#include + +#include "rcutils/allocator.h" + +#include "ros2_medkit_serialization/vendored/dynmsg/typesupport.hpp" + +namespace dynmsg +{ + +namespace c +{ + +/// Parse a YAML representation of a message into a ROS message and store it in a raw bytes buffer. +/** + * The introspection information is used to convert the YAML representation into the correct binary + * representation for the given ROS message. + * + * It is an error for the YAML representation to contain a field that is not in the ROS message. + * It is not an error for a field of the ROS message to not be specified in the YAML + * representation; that field will be left uninitialised. + */ +RosMessage yaml_to_rosmsg(const InterfaceTypeName & interface_type, const std::string & yaml_str); + +/// Version of yaml_to_rosmsg() with TypeInfo provided directly, and an allocator. +/** + * \see dynmsg::c::yaml_to_rosmsg() + */ +RosMessage yaml_and_typeinfo_to_rosmsg( + const TypeInfo * type_info, + const std::string & yaml_str, + rcutils_allocator_t * allocator); + +} // namespace c + +namespace cpp +{ + +/// C++ version of dynmsg::c::yaml_to_rosmsg(). +/** + * \see dynmsg::c::yaml_to_rosmsg() + */ +RosMessage_Cpp yaml_to_rosmsg( + const InterfaceTypeName & interface_type, + const std::string & yaml_str); + +/// C++ version of dynmsg::c::yaml_and_typeinfo_to_rosmsg(). +/** + * \see dynmsg::c::yaml_and_typeinfo_to_rosmsg() + */ +RosMessage_Cpp yaml_and_typeinfo_to_rosmsg( + const TypeInfo_Cpp * type_info, + const std::string & yaml_str, + rcutils_allocator_t * allocator); + +/// Version of dynmsg::cpp::yaml_and_typeinfo_to_rosmsg() using an existing empty message. +/** + * Takes a pointer to an existing empty message. + * + * \see dynmsg::cpp::yaml_and_typeinfo_to_rosmsg() + */ +void yaml_and_typeinfo_to_rosmsg( + const TypeInfo_Cpp * type_info, + const std::string & yaml_str, + void * ros_message); + +} // namespace cpp + +} // namespace dynmsg + +#endif // ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__MSG_PARSER_HPP_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/string_utils.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/string_utils.hpp new file mode 100644 index 00000000..d242ddf0 --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/string_utils.hpp @@ -0,0 +1,29 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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__VENDORED__DYNMSG__STRING_UTILS_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__STRING_UTILS_HPP_ + +#include + +extern "C" +{ +/// Convert a std::string (8-bit characters) to a std::u16string (16-bit characters). +std::u16string string_to_u16string(const std::string & input); + +/// Convert a std::u16string (16-bit characters) to a std::string (8-bit characters). +std::string u16string_to_string(const std::u16string & input); +} // extern "C" + +#endif // ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__STRING_UTILS_HPP_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/types.h b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/types.h new file mode 100644 index 00000000..49d45078 --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/types.h @@ -0,0 +1,29 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2021 Christophe Bedard +// +// 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__VENDORED__DYNMSG__TYPES_H_ +#define ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__TYPES_H_ + +#include + +/// The type that holds a dynmsg return code. +typedef rcutils_ret_t dynmsg_ret_t; + +/// Success return code. +#define DYNMSG_RET_OK RCUTILS_RET_OK +/// Unspecified error return code. +#define DYNMSG_RET_ERROR RCUTILS_RET_ERROR + +#endif // ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__TYPES_H_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/typesupport.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/typesupport.hpp new file mode 100644 index 00000000..43aad97b --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/typesupport.hpp @@ -0,0 +1,163 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2021 Christophe Bedard +// +// 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__VENDORED__DYNMSG__TYPESUPPORT_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__TYPESUPPORT_HPP_ + +#include +#include +#include + +#include "rcutils/allocator.h" +#include "rosidl_typesupport_introspection_c/message_introspection.h" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" + +#include "ros2_medkit_serialization/vendored/dynmsg/types.h" + +extern "C" +{ +// Structure used to store the type support for a single interface type +using TypeSupport = rosidl_message_type_support_t; +// Structure used to store the introspection information for a single interface type +using TypeInfo_C = rosidl_typesupport_introspection_c__MessageMembers; +// Structure used to store the introspection information for a single field of a interface type +using MemberInfo_C = rosidl_typesupport_introspection_c__MessageMember; + +using TypeInfo_Cpp = rosidl_typesupport_introspection_cpp::MessageMembers; +using MemberInfo_Cpp = rosidl_typesupport_introspection_cpp::MessageMember; + +// A ROS message, stored in a binary buffer with attached introspection information +typedef struct RosMessage_C +{ + const TypeInfo_C * type_info; + uint8_t * data; +} RosMessage_C; + +typedef struct RosMessage_Cpp +{ + const TypeInfo_Cpp * type_info; + uint8_t * data; +} RosMessage_Cpp; + +using TypeInfo = TypeInfo_C; +using MemberInfo = MemberInfo_C; +using RosMessage = RosMessage_C; + +typedef const rosidl_message_type_support_t * (* get_message_ts_func)(); + +// An interface type can be identified by its namespace (i.e. the package that stores it) and its +// type name +using InterfaceTypeName = std::pair; + +// Full interface type name including the interface category (msg, srv, action) +// Format: (package_name, interface_type, type_name) +// Example: ("std_srvs", "srv", "Trigger_Request") +using FullInterfaceTypeName = std::tuple; +} // extern "C" + +namespace dynmsg +{ + +namespace c +{ + +/// Search for and load the introspection library for a single interface type. +/** + * This function will search the system's configured library search paths (which should include the + * ROS paths) to find a dynamic library named following the pattern + * "lib[namespace]__rosidl_typesupport_introspection_c.so". + * When found, it opens that library and loads a function named following the pattern + * "rosidl_typesupport_introspection_c__get_message_type_support_handle__[namespace]__msg__[type]". + * This function, when called, provides a pointer to the introspection structure for the specified + * interface type. This pointer is returned. The information contained in this structure can be + * used to understand a ROS message stored in a binary buffer, or to construct a ROS message in a + * binary buffer. + */ +const TypeInfo * get_type_info(const InterfaceTypeName & interface_type); + +/// Search for and load the introspection library for a full interface type (msg/srv/action). +/** + * This overload accepts a FullInterfaceTypeName which includes the interface category. + * This allows loading type info for services and actions in addition to messages. + * \see get_type_info(const InterfaceTypeName &) + */ +const TypeInfo * get_type_info(const FullInterfaceTypeName & interface_type); + +/// Initialise a RosMessage structure. +/** + * The introspection information for the specified interface type is loaded from its shared library + * and stored in the type_info field. The ros_msg buffer is allocated with enough space to store + * one ROS message of the specified type. + * When finshed with the RosMessage instance, call ros_message_destroy() to clean up allocated + * memory. + */ +dynmsg_ret_t ros_message_init(const InterfaceTypeName & interface_type, RosMessage * ros_msg); + +/// Version of ros_message_init() but with TypeInfo directly and an allocator. +/** + * \see ros_message_init() + */ +dynmsg_ret_t ros_message_with_typeinfo_init( + const TypeInfo * type_info, + RosMessage * ros_msg, + rcutils_allocator_t * allocator); + +/// Clean up a RosMessage instance by freeing its resources. +void ros_message_destroy(RosMessage * ros_msg); + +/// Version of ros_message_destroy but with an allocator. +/** + * \see ros_message_destroy() + */ +void ros_message_destroy_with_allocator(RosMessage * ros_msg, rcutils_allocator_t * allocator); + +} // namespace c + +namespace cpp +{ + +/// C++ version of dynmsg::c::get_type_info() +/** + * \see dynmsg::c::get_type_info() + */ +const TypeInfo_Cpp * get_type_info(const InterfaceTypeName & interface_type); + +/// C++ version of dynmsg::c::get_type_info() for full interface types (msg/srv/action) +/** + * \see dynmsg::c::get_type_info(const FullInterfaceTypeName &) + */ +const TypeInfo_Cpp * get_type_info(const FullInterfaceTypeName & interface_type); + +/// C++ version of dynmsg::c::ros_message_with_typeinfo_init() +/** + * \see dynmsg::c::ros_message_with_typeinfo_init() + */ +dynmsg_ret_t ros_message_with_typeinfo_init( + const TypeInfo_Cpp * type_info, + RosMessage_Cpp * ros_msg, + rcutils_allocator_t * allocator); + + +/// C++ version of dynmsg::c::ros_message_destroy_with_allocator() +/** + * \see dynmsg::c::ros_message_destroy_with_allocator() + */ +void ros_message_destroy_with_allocator(RosMessage_Cpp * ros_msg, rcutils_allocator_t * allocator); + +} // namespace cpp + +} // namespace dynmsg + +#endif // ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__TYPESUPPORT_HPP_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/vector_utils.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/vector_utils.hpp new file mode 100644 index 00000000..c6089606 --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/vector_utils.hpp @@ -0,0 +1,36 @@ +// Copyright 2021 Christophe Bedard +// +// 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__VENDORED__DYNMSG__VECTOR_UTILS_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__VECTOR_UTILS_HPP_ + +#include +#include + +namespace dynmsg +{ + +/// Get the number of elements in/size of a vector given a pointer to it and its element size. +/** + * This uses some knowledge/assumption about the std::vector implementation. + * See the `get_vector_size` implementation for more info on how it works. + * + * Note that std::vector is a special optimized std::vector + * specialization and the "trick" used in get_vector_size() does not work. + */ +size_t get_vector_size(const uint8_t * vector, size_t element_size); + +} // namespace dynmsg + +#endif // ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__VECTOR_UTILS_HPP_ diff --git a/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/yaml_utils.hpp b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/yaml_utils.hpp new file mode 100644 index 00000000..a5f8e92b --- /dev/null +++ b/src/ros2_medkit_serialization/include/ros2_medkit_serialization/vendored/dynmsg/yaml_utils.hpp @@ -0,0 +1,42 @@ +// Copyright 2021 Christophe Bedard +// +// 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__VENDORED__DYNMSG__YAML_UTILS_HPP_ +#define ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__YAML_UTILS_HPP_ + +#include + +#include + +namespace dynmsg +{ + +/// Convert YAML node to string. +/** + * Flow style is one-line, JSON-like. + * Non-flow style is block style, which is the normal multi-line style. + * + * \param yaml the YAML node + * \param double_quoted whether to double quote all keys and values or not + * \param flow_style whether to use flow-style or not + * \return the YAML node as a string + */ +std::string yaml_to_string( + const YAML::Node & yaml, + const bool double_quoted = false, + const bool flow_style = false); + +} // namespace dynmsg + +#endif // ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__YAML_UTILS_HPP_ diff --git a/src/ros2_medkit_serialization/package.xml b/src/ros2_medkit_serialization/package.xml index 0440af73..3131a6de 100644 --- a/src/ros2_medkit_serialization/package.xml +++ b/src/ros2_medkit_serialization/package.xml @@ -10,7 +10,9 @@ ament_cmake rclcpp - dynmsg + rcutils + rosidl_runtime_c + rosidl_typesupport_introspection_c rosidl_typesupport_introspection_cpp rosidl_runtime_cpp rcpputils diff --git a/src/ros2_medkit_serialization/src/json_serializer.cpp b/src/ros2_medkit_serialization/src/json_serializer.cpp index dcf5025b..19d5772f 100644 --- a/src/ros2_medkit_serialization/src/json_serializer.cpp +++ b/src/ros2_medkit_serialization/src/json_serializer.cpp @@ -16,10 +16,10 @@ #include -#include "dynmsg/message_reading.hpp" -#include "dynmsg/msg_parser.hpp" #include "rcpputils/shared_library.hpp" #include "rmw/rmw.h" +#include "ros2_medkit_serialization/vendored/dynmsg/message_reading.hpp" +#include "ros2_medkit_serialization/vendored/dynmsg/msg_parser.hpp" #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rosidl_typesupport_introspection_cpp/field_types.hpp" diff --git a/src/ros2_medkit_serialization/src/message_cleanup.cpp b/src/ros2_medkit_serialization/src/message_cleanup.cpp new file mode 100644 index 00000000..b6dd4184 --- /dev/null +++ b/src/ros2_medkit_serialization/src/message_cleanup.cpp @@ -0,0 +1,28 @@ +// 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/message_cleanup.hpp" + +#include "rcutils/allocator.h" + +namespace ros2_medkit_serialization { + +void destroy_ros_message(RosMessage_Cpp * ros_msg) { + if (ros_msg != nullptr && ros_msg->data != nullptr) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + dynmsg::cpp::ros_message_destroy_with_allocator(ros_msg, &allocator); + } +} + +} // namespace ros2_medkit_serialization diff --git a/src/ros2_medkit_serialization/src/vendored/dynmsg/NOTICE.md b/src/ros2_medkit_serialization/src/vendored/dynmsg/NOTICE.md new file mode 100644 index 00000000..0e814481 --- /dev/null +++ b/src/ros2_medkit_serialization/src/vendored/dynmsg/NOTICE.md @@ -0,0 +1,39 @@ +# dynmsg - Vendored Source + +This directory contains vendored source code from the `dynmsg` library, part of the +`dynamic_message_introspection` project. + +## Original Project + +- **Repository**: https://github.com/osrf/dynamic_message_introspection +- **License**: Apache License 2.0 + +## Copyright + +- Copyright 2020 Open Source Robotics Foundation, Inc. +- Copyright 2021 Christophe Bedard + +## Modifications + +The following modifications were made for integration into ros2_medkit_serialization: + +1. Include paths updated from `"dynmsg/..."` to `"ros2_medkit_serialization/vendored/dynmsg/..."` +2. Include guards updated to use `ROS2_MEDKIT_SERIALIZATION__VENDORED__DYNMSG__` prefix +3. Configuration defines hardcoded in `config.hpp` (instead of CMake-generated): + - `DYNMSG_VALUE_ONLY` enabled + - `DYNMSG_YAML_CPP_BAD_INT8_HANDLING` enabled + - `DYNMSG_PARSER_DEBUG` disabled +4. C API files excluded (only C++ API used) + +## Excluded Files + +The following files from the original dynmsg library were not vendored as they are +not used by ros2_medkit_serialization: + +- `message_reading_c.cpp` (C API) +- `msg_parser_c.cpp` (C API) + +## License + +Licensed under the Apache License, Version 2.0. See the LICENSE file in the +ros2_medkit_serialization package root for the full license text. diff --git a/src/ros2_medkit_serialization/src/vendored/dynmsg/message_reading_cpp.cpp b/src/ros2_medkit_serialization/src/vendored/dynmsg/message_reading_cpp.cpp new file mode 100644 index 00000000..16a3cd1c --- /dev/null +++ b/src/ros2_medkit_serialization/src/vendored/dynmsg/message_reading_cpp.cpp @@ -0,0 +1,641 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2021 Christophe Bedard +// +// 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 "rcutils/logging_macros.h" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" + +#include "ros2_medkit_serialization/vendored/dynmsg/config.hpp" +#include "ros2_medkit_serialization/vendored/dynmsg/message_reading.hpp" +#include "ros2_medkit_serialization/vendored/dynmsg/string_utils.hpp" +#include "ros2_medkit_serialization/vendored/dynmsg/typesupport.hpp" +#include "ros2_medkit_serialization/vendored/dynmsg/vector_utils.hpp" + +namespace dynmsg +{ +namespace cpp +{ + +namespace impl +{ + +#ifndef DYNMSG_VALUE_ONLY +// Convert primitive ROS types to a string representation +std::string +member_type_to_string(const MemberInfo_Cpp & member_info) +{ + std::stringstream result; + switch (member_info.type_id_) { + case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + result << "float"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: + result << "double"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: + result << "long double"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + result << "char"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: + result << "wchar"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: + result << "boolean"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: + result << "octet"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + result << "uint8"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + result << "int8"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + result << "uint16"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + result << "int16"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + result << "uint32"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + result << "int32"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + result << "uint64"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + result << "int64"; + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + result << "string"; + // Strings may have an upper bound + if (member_info.string_upper_bound_ > 0) { + result << "<=" << member_info.string_upper_bound_; + } + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + result << "wstring"; + // WStrings may have an upper bound + if (member_info.string_upper_bound_ > 0) { + result << "<=" << member_info.string_upper_bound_; + } + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + // For nested types, the string representation must include the name space as well as the + // type name + result << + reinterpret_cast(member_info.members_->data)->message_namespace_ << + "/" << + reinterpret_cast(member_info.members_->data)->message_name_; + break; + default: + // Don't throw an error, just print out "UNKNOWN" then keep persevering through the message + result << "UNKNOWN"; + break; + } + // If this member is a sequence of some kind, indicate that in the type + if (member_info.is_array_) { + result << '['; + if (member_info.is_upper_bound_) { + result << "<="; + } + if (member_info.array_size_ > 0) { + result << member_info.array_size_; + } + result << ']'; + } + return result.str(); +} +#endif // DYNMSG_VALUE_ONLY + +// Get the size of primitive types +size_t +size_of_member_type(uint8_t type_id) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: size_of_member_type: " << (int)type_id << std::endl); + switch (type_id) { + case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + return sizeof(float); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: + return sizeof(double); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: + return sizeof(long double); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + return sizeof(uint8_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: + return sizeof(uint16_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: + return sizeof(bool); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: + return sizeof(uint8_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + return sizeof(uint8_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + return sizeof(int8_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + return sizeof(uint16_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + return sizeof(int16_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + return sizeof(uint32_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + return sizeof(int32_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + return sizeof(uint64_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + return sizeof(int64_t); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + return sizeof(std::string); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + return sizeof(std::u16string); + case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + assert(0 && "Cannot get the size of a nested message"); + return 0; + default: + assert(0 && "Cannot get the size of an unknown message type"); + return 0; + } +} + +// Convert the binary data for an individual element of an array member to YAML +void member_to_yaml_array_item( + const MemberInfo_Cpp & member_info, + const uint8_t * member_data, + YAML::Node & array_node) +{ + DYNMSG_DEBUG( + std::cout << "DEBUG: member_to_yaml_array_item: " << (int)member_info.type_id_ << std::endl); + switch (member_info.type_id_) { + case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING + array_node.push_back(std::to_string(*reinterpret_cast(member_data))); +#else + array_node.push_back(*reinterpret_cast(member_data)); +#endif + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING + array_node.push_back(std::to_string(*reinterpret_cast(member_data))); +#else + array_node.push_back(*reinterpret_cast(member_data)); +#endif + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING + array_node.push_back(std::to_string(*reinterpret_cast(member_data))); +#else + array_node.push_back(*reinterpret_cast(member_data)); +#endif + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING + array_node.push_back(std::to_string(*reinterpret_cast(member_data))); +#else + array_node.push_back(*reinterpret_cast(member_data)); +#endif + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + array_node.push_back(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + // WStrings require going through some intermediate formats + array_node.push_back( + u16string_to_string(*reinterpret_cast(member_data))); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + // For nested types, don't copy the data out of the buffer directly. Recursively read the + // nested type into the YAML. + RosMessage_Cpp nested_member; + nested_member.type_info = reinterpret_cast(member_info.members_->data); + nested_member.data = const_cast(member_data); + array_node.push_back(message_to_yaml(nested_member)); + break; + default: + assert(0 && "Unknown value for unknown type"); + break; + } +} + +// Convert an individual member's value from binary to YAML +void basic_value_to_yaml( + const MemberInfo_Cpp & member_info, + const uint8_t * member_data, + YAML::Node & member) +{ + DYNMSG_DEBUG( + std::cout << "DEBUG: basic_value_to_yaml: " << + member_info.name_ << ":" << (int)member_info.type_id_ << std::endl); + // return; + switch (member_info.type_id_) { + case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING + member["value"] = std::to_string(*reinterpret_cast(member_data)); +#else + member["value"] = *reinterpret_cast(member_data); +#endif + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING + member["value"] = std::to_string(*reinterpret_cast(member_data)); +#else + member["value"] = *reinterpret_cast(member_data); +#endif + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING + member["value"] = std::to_string(*reinterpret_cast(member_data)); +#else + member["value"] = *reinterpret_cast(member_data); +#endif + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING + member["value"] = std::to_string(*reinterpret_cast(member_data)); +#else + member["value"] = *reinterpret_cast(member_data); +#endif + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + DYNMSG_DEBUG(std::cout << "DEBUG: basic_value_to_yaml() string: " << std::flush); + DYNMSG_DEBUG( + std::cout << (member_data == nullptr ? "NULL" : "not null") << std::flush << + ", size=" << reinterpret_cast(member_data)->size() << std::endl); + member["value"] = *reinterpret_cast(member_data); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + // WStrings require going through some intermediate formats + member["value"] = + u16string_to_string(*reinterpret_cast(member_data)); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + // For nested types, don't copy the data out of the buffer directly. Recursively read the + // nested type into the YAML. + RosMessage_Cpp nested_member; + nested_member.type_info = reinterpret_cast(member_info.members_->data); + nested_member.data = const_cast(member_data); + member["value"] = message_to_yaml(nested_member); + break; + default: + assert(0 && "unknown type"); + break; + } +} + +// Convert a dynamically-sized sequence to YAML - implementation function +template +void +dynamic_array_to_yaml_impl( + const MemberInfo_Cpp & member_info, + const std::vector * v, + YAML::Node & array_node) +{ + DYNMSG_DEBUG( + std::cout << "DEBUG: dynamic_array_to_yaml_impl: " << (int)member_info.type_id_ << std::endl); + std::vector * vn = const_cast *>(v); + for (size_t ii = 0; ii < vn->size(); ++ii) { + member_to_yaml_array_item( + member_info, + reinterpret_cast(&(vn->data()[ii])), + array_node); + } +} +// Custom version for std::vector because its implementation is different +// https://en.cppreference.com/w/cpp/container/vector_bool +void +dynamic_array_to_yaml_impl_bool( + const MemberInfo_Cpp & member_info, + const std::vector * v, + YAML::Node & array_node) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: dynamic_array_to_yaml_impl_bool" << std::endl); + static_cast(member_info); + for (size_t ii = 0; ii < v->size(); ++ii) { + DYNMSG_DEBUG(std::cout << (v->operator[](ii) ? "true" : "false") << ", "); + array_node.push_back(v->operator[](ii)); + } + DYNMSG_DEBUG(std::cout << std::endl); +} + +// Convert a dynamically-sized sequence to YAML +void +dynamic_array_to_yaml( + const MemberInfo_Cpp & member_info, + const uint8_t * member_data, + YAML::Node & array_node) +{ + DYNMSG_DEBUG( + std::cout << "DEBUG: dynamic_array_to_yaml: " << (int)member_info.type_id_ << std::endl); + switch (member_info.type_id_) { + case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: + dynamic_array_to_yaml_impl_bool( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + dynamic_array_to_yaml_impl( + member_info, + reinterpret_cast *>(member_data), + array_node); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + // We do not know the specific type of the sequence because the type is not available at + // compile-time, but we know it's a vector and we know the size of the contained type. + RosMessage_Cpp nested_member; + nested_member.type_info = reinterpret_cast(member_info.members_->data); + uint8_t * element_data; + memcpy(&element_data, member_data, sizeof(void *)); + size_t element_size; + element_size = nested_member.type_info->size_of_; + size_t element_count; + element_count = dynmsg::get_vector_size(member_data, element_size); + DYNMSG_DEBUG(std::cout << "\telement_size=" << element_size << std::endl); + DYNMSG_DEBUG(std::cout << "\telement_count=" << element_count << std::endl); + for (size_t ii = 0; ii < element_count; ++ii) { + nested_member.data = element_data + ii * element_size; + // Recursively read the nested type into the array element in the YAML representation + array_node.push_back(message_to_yaml(nested_member)); + } + break; + default: + assert(0 && "Unknown value for unknown type"); + break; + } +} + +// Convert a fixed-sized sequence (a C-style array) to YAML +void fixed_array_to_yaml( + const MemberInfo_Cpp & member_info, + uint8_t * member_data, + YAML::Node & array_node) +{ + DYNMSG_DEBUG( + std::cout << "DEBUG: fixed_array_to_yaml: " << (int)member_info.type_id_ << std::endl); + size_t element_size(0); + if (member_info.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) { + element_size = reinterpret_cast(member_info.members_->data)->size_of_; + } else { + element_size = size_of_member_type(member_info.type_id_); + } + DYNMSG_DEBUG(std::cout << element_size << ", " << member_info.array_size_ << std::endl); + for (size_t ii = 0; ii < member_info.array_size_; ++ii) { + member_to_yaml_array_item(member_info, &member_data[ii * element_size], array_node); + } +} + +// Read one member of a message into a YAML node and return that node +YAML::Node member_to_yaml( + const MemberInfo_Cpp & member_info, + uint8_t * member_data) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: member_to_yaml: " << (int)member_info.type_id_ << std::endl); + YAML::Node member; +#ifndef DYNMSG_VALUE_ONLY + member["type"] = member_type_to_string(member_info); + if (member_info.default_value_ != nullptr) { + member["default"] = "default value here"; + } +#endif + + if (member_info.is_array_) { + YAML::Node array; + if (member_info.is_upper_bound_ || member_info.array_size_ == 0) { + dynamic_array_to_yaml(member_info, member_data, array); + } else { + fixed_array_to_yaml(member_info, member_data, array); + } +#ifdef DYNMSG_VALUE_ONLY + return array; +#else + member["value"] = array; +#endif + } else { + if (member_info.type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE) { + RosMessage_Cpp nested_member; + nested_member.type_info = reinterpret_cast(member_info.members_->data); + nested_member.data = const_cast(member_data); +#ifdef DYNMSG_VALUE_ONLY + return message_to_yaml(nested_member); +#else + member["value"] = message_to_yaml(nested_member); +#endif + } else { + basic_value_to_yaml(member_info, member_data, member); +#ifdef DYNMSG_VALUE_ONLY + return member["value"]; +#endif + } + } + + return member; +} + +} // namespace impl + +YAML::Node +message_to_yaml(const RosMessage_Cpp & message) +{ + YAML::Node yaml_msg; + + DYNMSG_DEBUG(std::cout << "DEBUG: message_to_yaml" << std::endl); + DYNMSG_DEBUG( + std::cout << "DEBUG: message.type_info message_namespace_: " << + message.type_info->message_namespace_ << std::endl); + DYNMSG_DEBUG( + std::cout << "DEBUG: message.type_info message_name_: " << + message.type_info->message_name_ << std::endl); + + // Iterate over the members of the message, converting the binary data for each into a node in + // the YAML representation + for (uint32_t ii = 0; ii < message.type_info->member_count_; ++ii) { + // Get the introspection information for this particular member + const MemberInfo_Cpp & member_info = message.type_info->members_[ii]; + DYNMSG_DEBUG(std::cout << "DEBUG: member_info name: " << member_info.name_ << std::endl); + // Get a pointer to the member's data in the binary buffer + uint8_t * member_data = &message.data[member_info.offset_]; + // Recursively (because some members may be non-primitive types themeslves) convert the member + // to YAML + yaml_msg[member_info.name_] = impl::member_to_yaml(member_info, member_data); + } + return yaml_msg; +} + +} // namespace cpp +} // namespace dynmsg diff --git a/src/ros2_medkit_serialization/src/vendored/dynmsg/msg_parser_cpp.cpp b/src/ros2_medkit_serialization/src/vendored/dynmsg/msg_parser_cpp.cpp new file mode 100644 index 00000000..0aa4d0a3 --- /dev/null +++ b/src/ros2_medkit_serialization/src/vendored/dynmsg/msg_parser_cpp.cpp @@ -0,0 +1,628 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2021 Christophe Bedard +// +// 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 "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" +#include "rosidl_runtime_c/u16string.h" +#include "rosidl_runtime_c/u16string_functions.h" +#include "rosidl_runtime_c/primitives_sequence_functions.h" +#include "rosidl_typesupport_introspection_c/field_types.h" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" +#include "rcutils/logging_macros.h" +#include "rcutils/allocator.h" + +#include "ros2_medkit_serialization/vendored/dynmsg/config.hpp" +#include "ros2_medkit_serialization/vendored/dynmsg/msg_parser.hpp" +#include "ros2_medkit_serialization/vendored/dynmsg/string_utils.hpp" + +namespace dynmsg +{ +namespace cpp +{ + +namespace impl +{ + +void yaml_to_rosmsg_impl( + const YAML::Node & root, + const TypeInfo_Cpp * typeinfo, + uint8_t * buffer +); + +// Helper structures to make the code cleaner +template +struct TypeMappingCpp {}; + +template<> +struct TypeMappingCpp +{ + using CppType = float; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = double; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = long double; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = signed char; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = uint16_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = bool; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = uint8_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = uint8_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = int8_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = uint16_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = int16_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = uint32_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = int32_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = uint64_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = int64_t; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = std::string; + using SequenceType = std::vector; +}; + +template<> +struct TypeMappingCpp +{ + using CppType = std::u16string; + using SequenceType = std::vector; +}; + +// Write an individual member into the binary message - generic +template +void write_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_member_item(): " << std::endl); + using CppType = typename TypeMappingCpp::CppType; + *reinterpret_cast(buffer) = yaml.as(); +} + +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING +// Write an individual member into the binary message - [u]int8_t (char, octet, uint8, int8) +template<> +void write_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_member_item(): " << std::endl); + using CppType = + typename TypeMappingCpp::CppType; + std::string s = yaml.as(); + *reinterpret_cast(buffer) = (uint8_t)std::stoul(s); +} +template<> +void write_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_member_item(): " << std::endl); + using CppType = + typename TypeMappingCpp::CppType; + std::string s = yaml.as(); + *reinterpret_cast(buffer) = (uint8_t)std::stoul(s); +} +template<> +void write_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_member_item(): " << std::endl); + using CppType = + typename TypeMappingCpp::CppType; + std::string s = yaml.as(); + *reinterpret_cast(buffer) = (uint8_t)std::stoul(s); +} +template<> +void write_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_member_item(): " << std::endl); + using CppType = + typename TypeMappingCpp::CppType; + std::string s = yaml.as(); + *reinterpret_cast(buffer) = (int8_t)std::stoi(s); +} +#endif // DYNMSG_YAML_CPP_BAD_INT8_HANDLING + +// Write an individual member into the binary message - string +template<> +void write_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG( + std::cout << "DEBUG: write_member_item(): " << yaml.as() << std::endl); + using CppType = + typename TypeMappingCpp::CppType; + *reinterpret_cast(buffer) = yaml.as(); +} + +// Write an individual member into the binary message - wstring +template<> +void write_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG( + std::cout << "DEBUG: write_member_item(): " << yaml.as() << std::endl); + using CppType = + typename TypeMappingCpp::CppType; + *reinterpret_cast(buffer) = string_to_u16string(yaml.as()); +} + +// Write an individual sequence member into the binary message - generic +template +void write_sequence_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_sequence_member_item(): " << std::endl); + using CppType = typename TypeMappingCpp::CppType; + using SequenceType = typename TypeMappingCpp::SequenceType; + auto seq = reinterpret_cast(buffer); + seq->push_back(yaml.as()); +} + +#ifdef DYNMSG_YAML_CPP_BAD_INT8_HANDLING +// Write an individual sequence member into the binary message - [u]int8_t: char, octet, uint8/int8 +template<> +void write_sequence_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_sequence_member_item(): " << std::endl); + using SequenceType = + typename TypeMappingCpp::SequenceType; + std::string s = yaml.as(); + auto seq = reinterpret_cast(buffer); + seq->push_back((uint8_t)std::stoul(s)); +} +template<> +void write_sequence_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_sequence_member_item(): " << std::endl); + using SequenceType = + typename TypeMappingCpp::SequenceType; + std::string s = yaml.as(); + auto seq = reinterpret_cast(buffer); + seq->push_back((uint8_t)std::stoul(s)); +} +template<> +void write_sequence_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_sequence_member_item(): " << std::endl); + using SequenceType = + typename TypeMappingCpp::SequenceType; + std::string s = yaml.as(); + auto seq = reinterpret_cast(buffer); + seq->push_back((uint8_t)std::stoul(s)); +} +template<> +void write_sequence_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_sequence_member_item(): " << std::endl); + using SequenceType = + typename TypeMappingCpp::SequenceType; + std::string s = yaml.as(); + auto seq = reinterpret_cast(buffer); + seq->push_back((int8_t)std::stoi(s)); +} +#endif // DYNMSG_YAML_CPP_BAD_INT8_HANDLING + +// Write an individual sequence member into the binary message - string +template<> +void write_sequence_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG( + std::cout << "DEBUG: write_sequence_member_item(): " << + yaml.as() << std::endl); + using SequenceType = + typename TypeMappingCpp::SequenceType; + auto seq = reinterpret_cast(buffer); + seq->push_back(yaml.as()); +} + +// Write an individual sequence member into the binary message - wstring +template<> +void write_sequence_member_item( + const YAML::Node & yaml, + uint8_t * buffer) +{ + DYNMSG_DEBUG( + std::cout << "DEBUG: write_sequence_member_item(): " << + yaml.as() << std::endl); + using SequenceType = + typename TypeMappingCpp::SequenceType; + auto seq = reinterpret_cast(buffer); + seq->push_back(string_to_u16string(yaml.as())); +} + +// Write a sequence member into the binary message - generic +template +void write_member_sequence( + const YAML::Node & yaml, + uint8_t * buffer, + const MemberInfo_Cpp & member) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_member_sequence: " << std::flush); + DYNMSG_DEBUG(std::cout << yaml.size() << ":" << yaml << std::endl); + if (member.is_upper_bound_ && yaml.size() > member.array_size_) { + throw std::runtime_error("yaml sequence is more than capacity"); + } + for (size_t i = 0; i < yaml.size(); i++) { // NOLINT(modernize-loop-convert) + write_sequence_member_item( + yaml[i], + buffer + ); + } +} +// std::vector is different +// https://en.cppreference.com/w/cpp/container/vector_bool +template<> +void write_member_sequence( + const YAML::Node & yaml, + uint8_t * buffer, + const MemberInfo_Cpp & member) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_member_sequence: " << std::flush); + DYNMSG_DEBUG(std::cout << yaml.size() << ":" << yaml << std::endl); + if (member.is_upper_bound_ && yaml.size() > member.array_size_) { + throw std::runtime_error("yaml sequence is more than capacity"); + } + // Just cast the sequence to std::vector and copy YAML node elements into it + using SequenceType = + typename TypeMappingCpp::SequenceType; + auto seq = reinterpret_cast(buffer); + for (size_t i = 0; i < yaml.size(); i++) { // NOLINT(modernize-loop-convert) + seq->push_back(yaml[i].as()); + } +} + +// Check if a field is a sequence of some kind +bool is_sequence(const MemberInfo_Cpp & member) +{ + // There isn't a "is_sequence" flag in the introspection data. It has to be inferred from several + // flags: + // + // fixed-length array: + // is_array == true + // array_size > 0 + // is_upper_bound == false + // bounded_sequence (has a maximum size but may be smaller): + // is_array == true + // array_size > 0 + // is_upper_bound == true + // unbounded_sequence (no maximum size): + // is_array == true + // array_size == 0 + // is_upper_bound == false + // + // Unhandled is the case of an array with size 0 + if ((member.is_array_ && member.array_size_ == 0) || member.is_upper_bound_) { + return true; // NOLINT(readability-simplify-boolean-expr) + } + return false; +} + +// Convert a YAML node into a field in the binary ROS message - generic +template +void write_member(const YAML::Node & yaml, uint8_t * buffer, const MemberInfo_Cpp & member) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: write_member" << std::endl); + using CppType = typename TypeMappingCpp::CppType; + // Arrays and sequences have different struct representation. An array is represented by a + // classic C array (pointer with data size == sizeof(type) * array_size). + // + // Sequences on the other hand use a custom-defined struct with data, size and capacity members. + + // Handle sequences + if (is_sequence(member)) { + write_member_sequence(yaml[member.name_], buffer + member.offset_, member); + return; + } + + // Handle classic C arrays + if (member.is_array_) { + for (size_t i = 0; i < member.array_size_; i++) { + write_member_item( + yaml[member.name_][i], + buffer + member.offset_ + sizeof(CppType) * i + ); + } + } else { + // Handle single-item members + write_member_item(yaml[member.name_], buffer + member.offset_); + } +} + +// Convert a nested YAML sequence node +void write_member_sequence_nested( + const YAML::Node & yaml, + uint8_t * buffer, + const MemberInfo_Cpp & member) +{ + if (member.is_upper_bound_ && yaml.size() > member.array_size_) { + throw std::runtime_error("yaml sequence is more than capacity"); + } + const TypeInfo_Cpp * member_typeinfo = + reinterpret_cast(member.members_->data); + auto & seq = buffer; + member.resize_function(seq, yaml.size()); + for (size_t i = 0; i < yaml.size(); i++) { + yaml_to_rosmsg_impl( + yaml[i], + member_typeinfo, + reinterpret_cast(member.get_function(seq, i)) + ); + } +} + +// Convert a nested YAML node +void write_member_nested( + const YAML::Node & yaml, + uint8_t * buffer, + const MemberInfo_Cpp & member) +{ + if (is_sequence(member)) { + write_member_sequence_nested(yaml[member.name_], buffer + member.offset_, member); + return; + } + + const TypeInfo_Cpp * member_typeinfo = + reinterpret_cast(member.members_->data); + if (member.is_array_) { + for (size_t i = 0; i < yaml[member.name_].size(); i++) { + yaml_to_rosmsg_impl( + yaml[member.name_][i], + member_typeinfo, buffer + member.offset_ + member_typeinfo->size_of_ * i + ); + } + } else { + yaml_to_rosmsg_impl(yaml[member.name_], member_typeinfo, buffer + member.offset_); + } +} + +// Convert a YAML representation to a binary ROS message by looping over the expected fields of the +// ROS message and getting their values from the YAML +void yaml_to_rosmsg_impl( + const YAML::Node & root, + const TypeInfo_Cpp * typeinfo, + uint8_t * buffer) +{ + DYNMSG_DEBUG(std::cout << "DEBUG: yaml_to_rosmsg_impl" << std::endl); + DYNMSG_DEBUG( + std::cout << "DEBUG: type_info message_namespace_: " << + typeinfo->message_namespace_ << std::endl); + DYNMSG_DEBUG( + std::cout << "DEBUG: type_info message_name_: " << typeinfo->message_name_ << std::endl); + for (uint32_t i = 0; i < typeinfo->member_count_; i++) { + const auto & member = typeinfo->members_[i]; + + if (!root[member.name_]) { + continue; + } + + switch (member.type_id_) { + case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: + write_member( + root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + write_member(root, buffer, member); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + write_member_nested(root, buffer, member); + break; + default: + throw std::runtime_error("unknown type"); + } + } +} + +} // namespace impl + +void yaml_and_typeinfo_to_rosmsg( + const TypeInfo_Cpp * type_info, + const std::string & yaml_str, + void * ros_message) +{ + // Parse the YAML representation to an in-memory representation + YAML::Node root = YAML::Load(yaml_str); + // Convert the YAML representation to a binary representation + uint8_t * buffer = reinterpret_cast(ros_message); + impl::yaml_to_rosmsg_impl(root, type_info, buffer); +} + +RosMessage_Cpp yaml_and_typeinfo_to_rosmsg( + const TypeInfo_Cpp * type_info, + const std::string & yaml_str, + rcutils_allocator_t * allocator) +{ + rcutils_allocator_t default_allocator = rcutils_get_default_allocator(); + if (!allocator) { + allocator = &default_allocator; + } + RosMessage_Cpp ros_msg; + // Load the introspection information and allocate space for the ROS message's binary + // representation + if (DYNMSG_RET_OK != + dynmsg::cpp::ros_message_with_typeinfo_init(type_info, &ros_msg, allocator)) + { + return {nullptr, nullptr}; + } + yaml_and_typeinfo_to_rosmsg(type_info, yaml_str, reinterpret_cast(ros_msg.data)); + return ros_msg; +} + +RosMessage_Cpp yaml_to_rosmsg( + const InterfaceTypeName & interface_type, + const std::string & yaml_str) +{ + const auto * type_info = dynmsg::cpp::get_type_info(interface_type); + if (nullptr == type_info) { + return {nullptr, nullptr}; + } + rcutils_allocator_t * allocator = nullptr; + return yaml_and_typeinfo_to_rosmsg(type_info, yaml_str, allocator); +} + +} // namespace cpp +} // namespace dynmsg diff --git a/src/ros2_medkit_serialization/src/vendored/dynmsg/string_utils.cpp b/src/ros2_medkit_serialization/src/vendored/dynmsg/string_utils.cpp new file mode 100644 index 00000000..bec1cd1f --- /dev/null +++ b/src/ros2_medkit_serialization/src/vendored/dynmsg/string_utils.cpp @@ -0,0 +1,31 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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/vendored/dynmsg/string_utils.hpp" + +std::u16string string_to_u16string(const std::string & input) +{ + std::wstring_convert, char16_t> converter; + return converter.from_bytes(input); +} + +std::string u16string_to_string(const std::u16string & input) +{ + std::wstring_convert, char16_t> converter; + return converter.to_bytes(input); +} diff --git a/src/ros2_medkit_serialization/src/vendored/dynmsg/typesupport.cpp b/src/ros2_medkit_serialization/src/vendored/dynmsg/typesupport.cpp new file mode 100644 index 00000000..3eafb821 --- /dev/null +++ b/src/ros2_medkit_serialization/src/vendored/dynmsg/typesupport.cpp @@ -0,0 +1,250 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2021 Christophe Bedard +// +// 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 "rcutils/allocator.h" +#include "rcutils/logging_macros.h" + +#include "ros2_medkit_serialization/vendored/dynmsg/types.h" +#include "ros2_medkit_serialization/vendored/dynmsg/typesupport.hpp" + +namespace dynmsg +{ + +namespace c +{ + +const TypeInfo * get_type_info(const InterfaceTypeName & interface_type) +{ + // Delegate to the full version with "msg" as the default interface type + return get_type_info(FullInterfaceTypeName{interface_type.first, "msg", interface_type.second}); +} + +const TypeInfo * get_type_info(const FullInterfaceTypeName & interface_type) +{ + const std::string & pkg_name = std::get<0>(interface_type); + const std::string & iface_type = std::get<1>(interface_type); // msg, srv, or action + const std::string & type_name = std::get<2>(interface_type); + + // Load the introspection library for the package containing the requested type + std::stringstream ts_lib_name; + ts_lib_name << "lib" << pkg_name << "__rosidl_typesupport_introspection_c.so"; + RCUTILS_LOG_DEBUG_NAMED( + "dynmsg", + "Loading introspection type support library %s", + ts_lib_name.str().c_str()); + void * introspection_type_support_lib = dlopen(ts_lib_name.str().c_str(), RTLD_LAZY); + if (introspection_type_support_lib == nullptr) { + RCUTILS_LOG_ERROR_NAMED( + "dynmsg", "failed to load introspection type support library: %s", dlerror()); + return nullptr; + } + // Load the function that, when called, will give us the introspection information for the + // interface type we are interested in + std::stringstream ts_func_name; + ts_func_name << "rosidl_typesupport_introspection_c__get_message_type_support_handle__" << + pkg_name << "__" << iface_type << "__" << type_name; + RCUTILS_LOG_DEBUG_NAMED( + "dynmsg", "Loading type support function %s", ts_func_name.str().c_str()); + + get_message_ts_func introspection_type_support_handle_func = + reinterpret_cast(dlsym( + introspection_type_support_lib, + ts_func_name.str().c_str())); + if (introspection_type_support_handle_func == nullptr) { + RCUTILS_LOG_ERROR_NAMED( + "dynmsg", + "failed to load introspection type support function: %s", + dlerror()); + return nullptr; + } + + // Call the function to get the introspection information we want + const rosidl_message_type_support_t * introspection_ts = + introspection_type_support_handle_func(); + RCUTILS_LOG_DEBUG_NAMED( + "dynmsg", + "Loaded type support %s", + introspection_ts->typesupport_identifier); + const rosidl_typesupport_introspection_c__MessageMembers * type_info = + reinterpret_cast( + introspection_ts->data); + + return type_info; +} + +dynmsg_ret_t ros_message_with_typeinfo_init( + const TypeInfo * type_info, + RosMessage * ros_msg, + rcutils_allocator_t * allocator) +{ + rcutils_allocator_t default_allocator = rcutils_get_default_allocator(); + if (!allocator) { + allocator = &default_allocator; + } + RCUTILS_LOG_DEBUG_NAMED( + "dynmsg", + "Allocating message buffer of size %ld bytes", + type_info->size_of_); + // Allocate space to store the binary representation of the message + uint8_t * data = + static_cast(allocator->allocate(type_info->size_of_, allocator->state)); + if (nullptr == data) { + return DYNMSG_RET_ERROR; + } + // Initialise the message buffer according to the interface type + type_info->init_function(data, ROSIDL_RUNTIME_C_MSG_INIT_ALL); + *ros_msg = RosMessage{type_info, data}; + return DYNMSG_RET_OK; +} + +dynmsg_ret_t ros_message_init( + const InterfaceTypeName & interface_type, + RosMessage * ros_msg) +{ + const auto * type_info = get_type_info(interface_type); + if (nullptr == type_info) { + return DYNMSG_RET_ERROR; + } + return dynmsg::c::ros_message_with_typeinfo_init(type_info, ros_msg, nullptr); +} + +void ros_message_destroy_with_allocator(RosMessage * ros_msg, rcutils_allocator_t * allocator) +{ + ros_msg->type_info->fini_function(ros_msg->data); + allocator->deallocate(ros_msg->data, allocator->state); +} + +void ros_message_destroy(RosMessage * ros_msg) +{ + ros_msg->type_info->fini_function(ros_msg->data); + delete[] ros_msg->data; +} + +} // namespace c + +namespace cpp +{ + +const TypeInfo_Cpp * get_type_info(const InterfaceTypeName & interface_type) +{ + // Delegate to the full version with "msg" as the default interface type + return get_type_info(FullInterfaceTypeName{interface_type.first, "msg", interface_type.second}); +} + +const TypeInfo_Cpp * get_type_info(const FullInterfaceTypeName & interface_type) +{ + const std::string & pkg_name = std::get<0>(interface_type); + const std::string & iface_type = std::get<1>(interface_type); // msg, srv, or action + const std::string & type_name = std::get<2>(interface_type); + + // Load the introspection library for the package containing the requested type + std::string ts_lib_name = "lib" + pkg_name + "__rosidl_typesupport_introspection_cpp.so"; + RCUTILS_LOG_DEBUG_NAMED( + "dynmsg", + "Loading C++ introspection type support library %s", + ts_lib_name.c_str()); + void * introspection_type_support_lib = dlopen(ts_lib_name.c_str(), RTLD_LAZY); + if (nullptr == introspection_type_support_lib) { + RCUTILS_LOG_ERROR_NAMED( + "dynmsg", "failed to load C++ introspection type support library: %s", dlerror()); + return nullptr; + } + + // Try the unmangled C-style function name first (more reliable across compilers) + // Format: rosidl_typesupport_introspection_cpp__get_message_type_support_handle__ + // [pkg]__[iface]__[type] + std::string ts_func_name_c = + "rosidl_typesupport_introspection_cpp__get_message_type_support_handle__" + + pkg_name + "__" + iface_type + "__" + type_name; + RCUTILS_LOG_DEBUG_NAMED("dynmsg", "Trying C-style function name: %s", ts_func_name_c.c_str()); + + get_message_ts_func introspection_type_support_handle_func = + reinterpret_cast(dlsym( + introspection_type_support_lib, + ts_func_name_c.c_str())); + + if (nullptr == introspection_type_support_handle_func) { + // Fall back to mangled C++ name for messages only (backward compatibility) + // The mangled name format only works reliably for "msg" types + if (iface_type == "msg") { + std::string ts_func_name_cpp = + "_ZN36rosidl_typesupport_introspection_cpp31get_message_type_support_handleIN" + + std::to_string(pkg_name.length()) + pkg_name + "3msg" + + std::to_string(type_name.length() + 1) + type_name + + "_ISaIvEEEEEPK29rosidl_message_type_support_tv"; + RCUTILS_LOG_DEBUG_NAMED("dynmsg", "Trying mangled C++ function name: %s", + ts_func_name_cpp.c_str()); + + introspection_type_support_handle_func = + reinterpret_cast(dlsym( + introspection_type_support_lib, + ts_func_name_cpp.c_str())); + } + } + + if (nullptr == introspection_type_support_handle_func) { + RCUTILS_LOG_ERROR_NAMED( + "dynmsg", + "failed to load C++ introspection type support function: %s", + dlerror()); + return nullptr; + } + + // Call the function to get the introspection information we want + const rosidl_message_type_support_t * introspection_ts = introspection_type_support_handle_func(); + RCUTILS_LOG_DEBUG_NAMED( + "dynmsg", + "Loaded C++ type support %s", + introspection_ts->typesupport_identifier); + const TypeInfo_Cpp * type_info = reinterpret_cast(introspection_ts->data); + + return type_info; +} + +dynmsg_ret_t ros_message_with_typeinfo_init( + const TypeInfo_Cpp * type_info, + RosMessage_Cpp * ros_msg, + rcutils_allocator_t * allocator) +{ + RCUTILS_LOG_DEBUG_NAMED( + "dynmsg", + "Allocating message buffer of size %ld bytes", + type_info->size_of_); + // Allocate space to store the binary representation of the message + uint8_t * data = + static_cast(allocator->allocate(type_info->size_of_, allocator->state)); + if (nullptr == data) { + return DYNMSG_RET_ERROR; + } + // Initialise the message buffer according to the interface type + type_info->init_function(data, rosidl_runtime_cpp::MessageInitialization::ALL); + *ros_msg = RosMessage_Cpp{type_info, data}; + return DYNMSG_RET_OK; +} + +void ros_message_destroy_with_allocator(RosMessage_Cpp * ros_msg, rcutils_allocator_t * allocator) +{ + ros_msg->type_info->fini_function(ros_msg->data); + allocator->deallocate(ros_msg->data, allocator->state); +} + +} // namespace cpp + +} // namespace dynmsg diff --git a/src/ros2_medkit_serialization/src/vendored/dynmsg/vector_utils.cpp b/src/ros2_medkit_serialization/src/vendored/dynmsg/vector_utils.cpp new file mode 100644 index 00000000..4fb4e67c --- /dev/null +++ b/src/ros2_medkit_serialization/src/vendored/dynmsg/vector_utils.cpp @@ -0,0 +1,61 @@ +// Copyright 2021 Christophe Bedard +// +// 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/vendored/dynmsg/vector_utils.hpp" + +/// Memory layout of an std::vector. +/** + * https://en.cppreference.com/w/cpp/container/vector + * https://stackoverflow.com/a/52337100 + */ +struct fake_vector +{ + void * begin; + void * end; + void * end_capacity; +}; + +union vector_union { + // The here doesn't matter because the memory + // layout (see fake_vector) is always the same. + std::vector * std; + fake_vector * fake; +}; + +namespace dynmsg +{ + +/** + * This uses the fact that std::vector is a contiguous container, therefore + * vector_size = (end - begin) / element_size + * + * This doesn't work for std::vector because that specialization + * is space-optimized and doesn't store its elements in a contiguous array, + * so that above gives a wildly invalid result. + */ +size_t get_vector_size(const uint8_t * vector, size_t element_size) +{ + // Should not get an element size of 0, but we want to avoid errors + if (0ul == element_size) { + return 0ul; + } + vector_union v = {reinterpret_cast *>(const_cast(vector))}; + return ( + reinterpret_cast(v.fake->end) - reinterpret_cast(v.fake->begin) + ) / element_size; +} + +} // namespace dynmsg diff --git a/src/ros2_medkit_serialization/src/vendored/dynmsg/yaml_utils.cpp b/src/ros2_medkit_serialization/src/vendored/dynmsg/yaml_utils.cpp new file mode 100644 index 00000000..06eb97f3 --- /dev/null +++ b/src/ros2_medkit_serialization/src/vendored/dynmsg/yaml_utils.cpp @@ -0,0 +1,40 @@ +// Copyright 2021 Christophe Bedard +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "ros2_medkit_serialization/vendored/dynmsg/yaml_utils.hpp" + +namespace dynmsg +{ + +std::string yaml_to_string( + const YAML::Node & yaml, + const bool double_quoted, + const bool flow_style) +{ + YAML::Emitter emitter; + if (double_quoted) { + emitter << YAML::DoubleQuoted; + } + if (flow_style) { + emitter << YAML::Flow; + } + emitter << yaml; + return emitter.c_str(); +} + +} // namespace dynmsg