From c2fa9ed11d1be6cb36441ad317d0797deb4714d9 Mon Sep 17 00:00:00 2001 From: Derek Date: Sun, 15 Jan 2023 16:55:20 -0800 Subject: [PATCH] Provide direct serialization of ROS2 messsage to ROS1 streams (#381) * Generated functions for (de)serializing ROS2 messages to ROS1 streams. Signed-off-by: Derek King * Partial set of fixes from review. Signed-off-by: Derek King * Rename functions based on discussion from review Signed-off-by: Derek King * Whitespace fix. Signed-off-by: Derek King * Name change for conversion helper fucntion. Signed-off-by: Derek King * Use explicit function overloading instead of template for stream types. Signed-off-by: Derek King * Fix factory.hpp crustify errors. Signed-off-by: Derek King Signed-off-by: Derek King --- CMakeLists.txt | 15 +- .../builtin_interfaces_factories.hpp | 58 +++ .../convert_builtin_interfaces.hpp | 25 + include/ros1_bridge/convert_decl.hpp | 6 + include/ros1_bridge/factory.hpp | 32 ++ package.xml | 2 + resource/interface_factories.cpp.em | 206 ++++++++ ros1_bridge/__init__.py | 6 +- src/builtin_interfaces_factories.cpp | 247 ++++++++++ src/convert_builtin_interfaces.cpp | 61 +++ test/test_ros2_to_ros1_serialization.cpp | 460 ++++++++++++++++++ 11 files changed, 1116 insertions(+), 2 deletions(-) create mode 100644 test/test_ros2_to_ros1_serialization.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9036fa3b..5f878af4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,16 +91,29 @@ if(BUILD_TESTING) if(ros1_geometry_msgs_FOUND AND ros1_sensor_msgs_FOUND AND ros1_std_msgs_FOUND) ament_add_gtest(test_convert_generic test/test_convert_generic.cpp) ament_target_dependencies(test_convert_generic + "geometry_msgs" + "rclcpp" "ros1_roscpp" "ros1_sensor_msgs" "ros1_std_msgs" "ros1_geometry_msgs" "sensor_msgs" "std_msgs" + ) + target_link_libraries(test_convert_generic ${PROJECT_NAME}) + + ament_add_gtest(test_ros2_to_ros1_serialization test/test_ros2_to_ros1_serialization.cpp) + ament_target_dependencies(test_ros2_to_ros1_serialization "geometry_msgs" "rclcpp" + "ros1_roscpp" + "ros1_sensor_msgs" + "ros1_std_msgs" + "ros1_geometry_msgs" + "sensor_msgs" + "std_msgs" ) - target_link_libraries(test_convert_generic ${PROJECT_NAME}) + target_link_libraries(test_ros2_to_ros1_serialization ${PROJECT_NAME}) endif() endif() diff --git a/include/ros1_bridge/builtin_interfaces_factories.hpp b/include/ros1_bridge/builtin_interfaces_factories.hpp index 993d8a6c..b974a533 100644 --- a/include/ros1_bridge/builtin_interfaces_factories.hpp +++ b/include/ros1_bridge/builtin_interfaces_factories.hpp @@ -15,8 +15,11 @@ #ifndef ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_ #define ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_ +#include + // include ROS 1 messages #include +#include #include #include @@ -25,6 +28,7 @@ // include ROS 2 messages #include #include +#include #include "ros1_bridge/factory.hpp" @@ -56,6 +60,33 @@ Factory< const builtin_interfaces::msg::Duration & ros2_msg, std_msgs::Duration & ros1_msg); +template<> +void +Factory< + std_msgs::Duration, + builtin_interfaces::msg::Duration +>::internal_stream_translate_helper( + ros::serialization::OStream & stream, + const builtin_interfaces::msg::Duration & msg); + +template<> +void +Factory< + std_msgs::Duration, + builtin_interfaces::msg::Duration +>::internal_stream_translate_helper( + ros::serialization::IStream & stream, + builtin_interfaces::msg::Duration & msg); + +template<> +void +Factory< + std_msgs::Duration, + builtin_interfaces::msg::Duration +>::internal_stream_translate_helper( + ros::serialization::LStream & stream, + const builtin_interfaces::msg::Duration & msg); + template<> void Factory< @@ -74,6 +105,33 @@ Factory< const builtin_interfaces::msg::Time & ros2_msg, std_msgs::Time & ros1_msg); +template<> +void +Factory< + std_msgs::Time, + builtin_interfaces::msg::Time +>::internal_stream_translate_helper( + ros::serialization::OStream & stream, + const builtin_interfaces::msg::Time & msg); + +template<> +void +Factory< + std_msgs::Time, + builtin_interfaces::msg::Time +>::internal_stream_translate_helper( + ros::serialization::IStream & stream, + builtin_interfaces::msg::Time & msg); + +template<> +void +Factory< + std_msgs::Time, + builtin_interfaces::msg::Time +>::internal_stream_translate_helper( + ros::serialization::LStream & stream, + const builtin_interfaces::msg::Time & msg); + } // namespace ros1_bridge #endif // ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_ diff --git a/include/ros1_bridge/convert_builtin_interfaces.hpp b/include/ros1_bridge/convert_builtin_interfaces.hpp index 9c6ea381..b5b612cc 100644 --- a/include/ros1_bridge/convert_builtin_interfaces.hpp +++ b/include/ros1_bridge/convert_builtin_interfaces.hpp @@ -40,6 +40,19 @@ convert_2_to_1( const builtin_interfaces::msg::Duration & ros2_msg, ros::Duration & ros1_type); +template +void +internal_stream_translate_helper( + STREAM_T & stream, + const builtin_interfaces::msg::Duration & msg); + +template +void +internal_stream_translate_helper( + STREAM_T & stream, + builtin_interfaces::msg::Duration & msg); + + template<> void convert_1_to_2( @@ -52,6 +65,18 @@ convert_2_to_1( const builtin_interfaces::msg::Time & ros2_msg, ros::Time & ros1_type); +template +void +internal_stream_translate_helper( + STREAM_T & stream, + const builtin_interfaces::msg::Time & msg); + +template +void +internal_stream_translate_helper( + STREAM_T & stream, + builtin_interfaces::msg::Time & msg); + } // namespace ros1_bridge #endif // ROS1_BRIDGE__CONVERT_BUILTIN_INTERFACES_HPP_ diff --git a/include/ros1_bridge/convert_decl.hpp b/include/ros1_bridge/convert_decl.hpp index 47c573d4..6cefe6b3 100644 --- a/include/ros1_bridge/convert_decl.hpp +++ b/include/ros1_bridge/convert_decl.hpp @@ -30,6 +30,12 @@ convert_2_to_1( const ROS2_T & ros2_msg, ROS1_T & ros1_msg); +template +void +internal_stream_translate_helper( + STREAM_T & out_stream, + const ROS2_T & msg); + } // namespace ros1_bridge #endif // ROS1_BRIDGE__CONVERT_DECL_HPP_ diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index 0399e483..f70f57f6 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -352,6 +352,37 @@ class Factory : public FactoryInterface return true; } + /** + * @brief Writes (serializes) a ROS2 class directly to a ROS1 stream + */ + static void convert_2_to_1(const ROS2_T & msg, ros::serialization::OStream & out_stream); + + /** + * @brief Reads (deserializes) a ROS2 class directly from a ROS1 stream + */ + static void convert_1_to_2(ros::serialization::IStream & in_stream, ROS2_T & msg); + + /** + * @brief Determines the length of a ROS2 class if it was serialized to a ROS1 stream + */ + static uint32_t length_2_as_1_stream(const ROS2_T & msg); + + /** + * @brief Internal helper functions conversion for ROS2 message types to/from ROS1 streams + * + * This function is not meant to be used externally. However, since this the internal helper + * functions call each other for sub messages they must be public. + */ + static void internal_stream_translate_helper( + ros::serialization::OStream & stream, + const ROS2_T & msg); + static void internal_stream_translate_helper( + ros::serialization::IStream & stream, + ROS2_T & msg); + static void internal_stream_translate_helper( + ros::serialization::LStream & stream, + const ROS2_T & msg); + std::string ros1_type_name_; std::string ros2_type_name_; @@ -359,6 +390,7 @@ class Factory : public FactoryInterface const rosidl_message_type_support_t * type_support_ = nullptr; }; + template class ServiceFactory : public ServiceFactoryInterface { diff --git a/package.xml b/package.xml index cf82aeb3..77773553 100644 --- a/package.xml +++ b/package.xml @@ -47,11 +47,13 @@ ament_lint_common demo_nodes_cpp diagnostic_msgs + geometry_msgs launch launch_testing launch_testing_ament_cmake launch_testing_ros ros2run + sensor_msgs rosidl_interface_packages diff --git a/resource/interface_factories.cpp.em b/resource/interface_factories.cpp.em index e6a1cea6..f3e536e4 100644 --- a/resource/interface_factories.cpp.em +++ b/resource/interface_factories.cpp.em @@ -24,10 +24,12 @@ from rosidl_parser.definition import AbstractNestedType from rosidl_parser.definition import AbstractSequence from rosidl_parser.definition import BoundedSequence from rosidl_parser.definition import NamespacedType +from rosidl_parser.definition import UnboundedString }@ #include "@(ros2_package_name)_factories.hpp" #include +#include #include "rclcpp/rclcpp.hpp" @@ -332,3 +334,207 @@ void ServiceFactory< @[ end for]@ @[end for]@ } // namespace ros1_bridge + + +// ROS1 serialization functions +namespace ros1_bridge +{ + +// This version is for write or length +template +static void streamVectorSize(STREAM_T& stream, const VEC_T& vec) +{ + // Output size of vector to stream + uint32_t data_len = vec.size(); + stream.next(data_len); +} + +// This version is for read +template +static void streamVectorSize(STREAM_T& stream, VEC_T& vec) +{ + // Resize vector to match size in stream + uint32_t data_len = 0; + stream.next(data_len); + vec.resize(data_len); +} + +// This version is for write +template +static void streamPrimitiveVector(ros::serialization::OStream & stream, const VEC_PRIMITIVE_T& vec) +{ + const uint32_t data_len = vec.size() * sizeof(typename VEC_PRIMITIVE_T::value_type); + // copy data from std::vector/std::array into stream + memcpy(stream.advance(data_len), &vec.front(), data_len); +} + +// This version is for length +template +static void streamPrimitiveVector(ros::serialization::LStream & stream, const VEC_PRIMITIVE_T& vec) +{ + const uint32_t data_len = vec.size() * sizeof(typename VEC_PRIMITIVE_T::value_type); + stream.advance(data_len); +} + +// This version is for read +template +static void streamPrimitiveVector(ros::serialization::IStream & stream, VEC_PRIMITIVE_T& vec) +{ + const uint32_t data_len = vec.size() * sizeof(typename VEC_PRIMITIVE_T::value_type); + // copy data from stream into std::vector/std::array + memcpy(&vec.front(), stream.advance(data_len), data_len); +} + +@[for m in mapped_msgs]@ + +@[ if m.ros2_msg.package_name=="std_msgs" and m.ros2_msg.message_name=="Header"] +// std_msgs/Header does not have a 1-to-1 field mapping because it ROS2 dropped the "seq" field. +// Typically, the auto-generated internal_stream_translate_helper() function will throw an exception for this. +// However, since Header is so fundimental, a hand-written template specialization is provided in +// builtin_interfaces_factories. +@[ else] + +@[ for stream_type, msg_const in (("OStream", "const"), ("IStream", ""), ("LStream", "const")) ]@ +template<> +void +Factory< + @(m.ros1_msg.package_name)::@(m.ros1_msg.message_name), + @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name) +>::internal_stream_translate_helper( + ros::serialization::@(stream_type) & stream, + @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name) @(msg_const) & ros2_msg) +{ +@[ if m.ros1_field_missing_in_ros2]@ + // Only messages that have exactly matching fields are supported -- for now + throw std::runtime_error("direct stream conversion is unsupported for messages types where fields do not match exactly"); +@[ else]@ +@[ for ros2_fields, ros1_fields in m.fields_2_to_1.items()]@ +@{ +ros2_field_selection = '.'.join((str(field.name) for field in ros2_fields)) +ros1_field_selection = '.'.join((str(field.name) for field in ros1_fields)) + +if isinstance(ros2_fields[-1].type, NamespacedType): + namespaces = ros2_fields[-1].type.namespaces + assert len(namespaces) == 2 and namespaces[1] == 'msg', \ + "messages not using the ', msg, ' triplet are not supported" +} +@[ if not isinstance(ros2_fields[-1].type, AbstractNestedType)]@ + // write non-array field +@[ if not isinstance(ros2_fields[-1].type, NamespacedType)]@ + // write primitive field + stream.next(ros2_msg.@(ros2_field_selection)); +@[ elif ros2_fields[-1].type.namespaces[0] == 'builtin_interfaces']@ + // write builtin field + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg.@(ros2_field_selection)); +@[ else]@ + // write sub message field + Factory< + @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name), + @(ros2_fields[-1].type.namespaces[0])::msg::@(ros2_fields[-1].type.name) + >::internal_stream_translate_helper(stream, ros2_msg.@(ros2_field_selection)); +@[ end if]@ +@[ else]@ + // write array or sequence field +@[ if isinstance(ros2_fields[-1].type, AbstractSequence)]@ + // dynamically sized sequence + streamVectorSize(stream, ros2_msg.@(ros2_field_selection)); +@[ else]@ + // statically sized array + static_assert(std::tuple_size::value == + decltype(@(m.ros1_msg.package_name)::@(m.ros1_msg.message_name)::@(ros1_field_selection))::static_size, + "size mismatch of static arrays"); +@[ end if]@ +@[ if not isinstance(ros2_fields[-1].type.value_type, NamespacedType)]@ + // write primitive array elements +@[ if isinstance(ros2_fields[-1].type.value_type, UnboundedString)]@ + // write UnboundedString + for ( + auto ros2_it = ros2_msg.@(ros2_field_selection).begin(); + ros2_it != ros2_msg.@(ros2_field_selection).end(); + ++ros2_it + ) + { + stream.next(*ros2_it); + } +@[ elif ros2_fields[-1].type.value_type.typename == 'builtin_interfaces']@ + // write builtin + for ( + auto ros2_it = ros2_msg.@(ros2_field_selection).begin(); + ros2_it != ros2_msg.@(ros2_field_selection).end(); + ++ros2_it + ) + { + ros1_bridge::internal_stream_translate_helper(stream, *ros2_it); + } +@[ else]@ + // write primitive type + streamPrimitiveVector(stream, ros2_msg.@(ros2_field_selection)); +@[ end if]@ +@[ else]@ + // write element wise since the type is different + { + for ( + auto ros2_it = ros2_msg.@(ros2_field_selection).begin(); + ros2_it != ros2_msg.@(ros2_field_selection).end(); + ++ros2_it + ) + { + // write sub message element +@[ if ros2_fields[-1].type.value_type.namespaces[0] == 'builtin_interfaces']@ + ros1_bridge::internal_stream_translate_helper(stream, *ros2_it); +@[ else]@ + Factory< + @(ros1_fields[-1].pkg_name)::@(ros1_fields[-1].msg_name), + @(ros2_fields[-1].type.value_type.namespaces[0])::msg::@(ros2_fields[-1].type.value_type.name) + >::internal_stream_translate_helper(stream, *ros2_it); +@[ end if]@ + } + } +@[ end if]@ +@[ end if]@ +@[ end for]@ +@[ end if]@ +} + + +@[ end for]@ + +template<> +void +Factory< + @(m.ros1_msg.package_name)::@(m.ros1_msg.message_name), + @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name) +>::convert_2_to_1(const @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name)& ros2_msg, + ros::serialization::OStream& out_stream) +{ + internal_stream_translate_helper(out_stream, ros2_msg); +} + + +template<> +void +Factory< + @(m.ros1_msg.package_name)::@(m.ros1_msg.message_name), + @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name) +>::convert_1_to_2(ros::serialization::IStream& in_stream, + @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name)& ros2_msg) +{ + internal_stream_translate_helper(in_stream, ros2_msg); +} + +template<> +uint32_t +Factory< + @(m.ros1_msg.package_name)::@(m.ros1_msg.message_name), + @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name) +>::length_2_as_1_stream(const @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name)& ros2_msg) +{ + ros::serialization::LStream len_stream; + internal_stream_translate_helper(len_stream, ros2_msg); + return len_stream.getLength(); +} + +@[ end if] + +@[end for]@ +} // namespace ros1_bridge diff --git a/ros1_bridge/__init__.py b/ros1_bridge/__init__.py index 756ef3f8..7cc78d14 100644 --- a/ros1_bridge/__init__.py +++ b/ros1_bridge/__init__.py @@ -807,6 +807,8 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx): ros1_field_missing_in_ros2 = any(ros1_fields_not_mapped) + mapping.ros1_field_missing_in_ros2 = ros1_field_missing_in_ros2 + if ros1_field_missing_in_ros2: # if some fields exist in ROS 1 but not in ROS 2 # check that no fields exist in ROS 2 but not in ROS 1 @@ -912,7 +914,8 @@ class Mapping: 'ros2_msg', 'fields_1_to_2', 'fields_2_to_1', - 'depends_on_ros2_messages' + 'depends_on_ros2_messages', + 'ros1_field_missing_in_ros2', ] def __init__(self, ros1_msg, ros2_msg): @@ -921,6 +924,7 @@ def __init__(self, ros1_msg, ros2_msg): self.fields_1_to_2 = OrderedDict() self.fields_2_to_1 = OrderedDict() self.depends_on_ros2_messages = set() + self.ros1_field_missing_in_ros2 = False def add_field_pair(self, ros1_fields, ros2_members): """ diff --git a/src/builtin_interfaces_factories.cpp b/src/builtin_interfaces_factories.cpp index 9b8d34e8..1d6b0227 100644 --- a/src/builtin_interfaces_factories.cpp +++ b/src/builtin_interfaces_factories.cpp @@ -80,6 +80,78 @@ Factory< ros1_bridge::convert_2_to_1(ros2_msg, ros1_msg.data); } +template<> +void +Factory< + std_msgs::Duration, + builtin_interfaces::msg::Duration +>::internal_stream_translate_helper( + ros::serialization::OStream & stream, + const builtin_interfaces::msg::Duration & ros2_msg) +{ + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); +} + +template<> +void +Factory< + std_msgs::Duration, + builtin_interfaces::msg::Duration +>::internal_stream_translate_helper( + ros::serialization::IStream & stream, + builtin_interfaces::msg::Duration & ros2_msg) +{ + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); +} + +template<> +void +Factory< + std_msgs::Duration, + builtin_interfaces::msg::Duration +>::internal_stream_translate_helper( + ros::serialization::LStream & stream, + const builtin_interfaces::msg::Duration & ros2_msg) +{ + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); +} + +template<> +void +Factory< + std_msgs::Duration, + builtin_interfaces::msg::Duration +>::convert_2_to_1( + const builtin_interfaces::msg::Duration & ros2_msg, + ros::serialization::OStream & out_stream) +{ + internal_stream_translate_helper(out_stream, ros2_msg); +} + +template<> +void +Factory< + std_msgs::Duration, + builtin_interfaces::msg::Duration +>::convert_1_to_2( + ros::serialization::IStream & in_stream, + builtin_interfaces::msg::Duration & ros2_msg) +{ + internal_stream_translate_helper(in_stream, ros2_msg); +} + +template<> +uint32_t +Factory< + std_msgs::Duration, + builtin_interfaces::msg::Duration +>::length_2_as_1_stream(const builtin_interfaces::msg::Duration & ros2_msg) +{ + ros::serialization::LStream len_stream; + internal_stream_translate_helper(len_stream, ros2_msg); + return len_stream.getLength(); +} + template<> void Factory< @@ -104,4 +176,179 @@ Factory< ros1_bridge::convert_2_to_1(ros2_msg, ros1_msg.data); } +template<> +void +Factory< + std_msgs::Time, + builtin_interfaces::msg::Time +>::internal_stream_translate_helper( + ros::serialization::OStream & stream, + const builtin_interfaces::msg::Time & ros2_msg) +{ + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); +} + +template<> +void +Factory< + std_msgs::Time, + builtin_interfaces::msg::Time +>::internal_stream_translate_helper( + ros::serialization::IStream & stream, + builtin_interfaces::msg::Time & ros2_msg) +{ + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); +} + +template<> +void +Factory< + std_msgs::Time, + builtin_interfaces::msg::Time +>::internal_stream_translate_helper( + ros::serialization::LStream & stream, + const builtin_interfaces::msg::Time & ros2_msg) +{ + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg); +} + +template<> +void +Factory< + std_msgs::Time, + builtin_interfaces::msg::Time +>::convert_2_to_1( + const builtin_interfaces::msg::Time & ros2_msg, + ros::serialization::OStream & out_stream) +{ + internal_stream_translate_helper(out_stream, ros2_msg); +} + +template<> +void +Factory< + std_msgs::Time, + builtin_interfaces::msg::Time +>::convert_1_to_2( + ros::serialization::IStream & in_stream, + builtin_interfaces::msg::Time & ros2_msg) +{ + internal_stream_translate_helper(in_stream, ros2_msg); +} + +template<> +uint32_t +Factory< + std_msgs::Time, + builtin_interfaces::msg::Time +>::length_2_as_1_stream(const builtin_interfaces::msg::Time & ros2_msg) +{ + ros::serialization::LStream len_stream; + internal_stream_translate_helper(len_stream, ros2_msg); + return len_stream.getLength(); +} + +template<> +void +Factory< + std_msgs::Header, + std_msgs::msg::Header +>::internal_stream_translate_helper( + ros::serialization::OStream & stream, + const std_msgs::msg::Header & ros2_msg) +{ + // ROS2 Header is missing seq, provide a fake one so stream matches + uint32_t seq = 0; + stream.next(seq); + + // write non-array field + // write builtin field + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg.stamp); + + // write non-array field + // write primitive field + stream.next(ros2_msg.frame_id); +} + +template<> +void +Factory< + std_msgs::Header, + std_msgs::msg::Header +>::internal_stream_translate_helper( + ros::serialization::IStream & stream, + std_msgs::msg::Header & ros2_msg) +{ + // ROS2 Header is missing seq, provide a fake one so stream matches + uint32_t seq = 0; + stream.next(seq); + + // write non-array field + // write builtin field + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg.stamp); + + // write non-array field + // write primitive field + stream.next(ros2_msg.frame_id); +} + +template<> +void +Factory< + std_msgs::Header, + std_msgs::msg::Header +>::internal_stream_translate_helper( + ros::serialization::LStream & stream, + const std_msgs::msg::Header & ros2_msg) +{ + // ROS2 Header is missing seq, provide a fake one so stream matches + uint32_t seq = 0; + stream.next(seq); + + // write non-array field + // write builtin field + ros1_bridge::internal_stream_translate_helper(stream, ros2_msg.stamp); + + // write non-array field + // write primitive field + stream.next(ros2_msg.frame_id); +} + +template<> +void +Factory< + std_msgs::Header, + std_msgs::msg::Header +>::convert_2_to_1( + const std_msgs::msg::Header & ros2_msg, + ros::serialization::OStream & out_stream) +{ + internal_stream_translate_helper(out_stream, ros2_msg); +} + + +template<> +void +Factory< + std_msgs::Header, + std_msgs::msg::Header +>::convert_1_to_2( + ros::serialization::IStream & in_stream, + std_msgs::msg::Header & ros2_msg) +{ + internal_stream_translate_helper(in_stream, ros2_msg); +} + +template<> +uint32_t +Factory< + std_msgs::Header, + std_msgs::msg::Header +>::length_2_as_1_stream(const std_msgs::msg::Header & ros2_msg) +{ + ros::serialization::LStream len_stream; + internal_stream_translate_helper(len_stream, ros2_msg); + return len_stream.getLength(); +} + } // namespace ros1_bridge diff --git a/src/convert_builtin_interfaces.cpp b/src/convert_builtin_interfaces.cpp index f0412f5b..d8eeed6a 100644 --- a/src/convert_builtin_interfaces.cpp +++ b/src/convert_builtin_interfaces.cpp @@ -14,6 +14,8 @@ #include "ros1_bridge/convert_builtin_interfaces.hpp" +#include "ros/serialization.h" + namespace ros1_bridge { @@ -37,6 +39,35 @@ convert_2_to_1( ros1_type.nsec = ros2_msg.nanosec; } +template<> +void +internal_stream_translate_helper( + ros::serialization::OStream & stream, + const builtin_interfaces::msg::Duration & ros2_msg) +{ + stream.next(ros2_msg.sec); + stream.next(ros2_msg.nanosec); +} + +template<> +void +internal_stream_translate_helper( + ros::serialization::IStream & stream, + builtin_interfaces::msg::Duration & ros2_msg) +{ + stream.next(ros2_msg.sec); + stream.next(ros2_msg.nanosec); +} + +template<> +void +internal_stream_translate_helper( + ros::serialization::LStream & stream, + const builtin_interfaces::msg::Duration & ros2_msg) +{ + stream.next(ros2_msg.sec); + stream.next(ros2_msg.nanosec); +} template<> void @@ -58,4 +89,34 @@ convert_2_to_1( ros1_type.nsec = ros2_msg.nanosec; } +template<> +void +internal_stream_translate_helper( + ros::serialization::OStream & stream, + const builtin_interfaces::msg::Time & ros2_msg) +{ + stream.next(ros2_msg.sec); + stream.next(ros2_msg.nanosec); +} + +template<> +void +internal_stream_translate_helper( + ros::serialization::IStream & stream, + builtin_interfaces::msg::Time & ros2_msg) +{ + stream.next(ros2_msg.sec); + stream.next(ros2_msg.nanosec); +} + +template<> +void +internal_stream_translate_helper( + ros::serialization::LStream & stream, + const builtin_interfaces::msg::Time & ros2_msg) +{ + stream.next(ros2_msg.sec); + stream.next(ros2_msg.nanosec); +} + } // namespace ros1_bridge diff --git a/test/test_ros2_to_ros1_serialization.cpp b/test/test_ros2_to_ros1_serialization.cpp new file mode 100644 index 00000000..977c1e98 --- /dev/null +++ b/test/test_ros2_to_ros1_serialization.cpp @@ -0,0 +1,460 @@ +// Copyright 2022 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. + +// GTest +#include + +// ROS1 Messsages +#include +#include +#include +#include +#include +#include +#include +#include + +// C++ Standard Library +#include +#include +#include + +// ros1_bridge +#include "ros1_bridge/bridge.hpp" +#include "ros1_bridge/factory.hpp" + +// RCLCPP +#include "rclcpp/serialized_message.hpp" + +// ROS2 Messsages +#include +#include +#include +#include +#include +#include + + +template +struct GenericTestBase +{ + using ROS1_T = ROS1_T_; + using ROS2_T = ROS2_T_; + using FACTORY_T = ros1_bridge::Factory; + + FACTORY_T factory; + ROS1_T ros1_msg; + ROS2_T ros2_msg; + GenericTestBase(const std::string & ros1_type_name, const std::string & ros2_type_name) + : factory(ros1_type_name, ros2_type_name) {} + + // Generate serialized buffer from ROS1 message + static std::vector generateRos1SerializedMessage(const ROS1_T & ros1_msg) + { + // Serialize ROS1 message + const uint32_t length = ros::serialization::serializationLength(ros1_msg); + std::vector buffer(length); + ros::serialization::OStream out_stream(buffer.data(), length); + ros::serialization::serialize(out_stream, ros1_msg); + return buffer; + } + + // Generate SerializedMessage from a ROS2 message + rclcpp::SerializedMessage generateRos2SerializedMessage(const ROS2_T & ros2_msg) + { + // Directly serialize ROS2 message + rclcpp::SerializedMessage serialized_msg; + auto ret = rmw_serialize( + &ros2_msg, factory.type_support_, + &serialized_msg.get_rcl_serialized_message()); + EXPECT_EQ(RMW_RET_OK, ret); + return serialized_msg; + } + + static bool compare(const ROS2_T &, const ROS2_T &) + { + return false; + } +}; + +template +bool compareXYZ(const T & a, const T & b) +{ + return (a.x == b.x) && (a.y == b.y) && (a.z == b.z); +} + +template +bool compareXYZW(const T & a, const T & b) +{ + return (a.x == b.x) && (a.y == b.y) && (a.z == b.z) && (a.w == b.w); +} + +bool compareTime( + const builtin_interfaces::msg::Time & a, + const builtin_interfaces::msg::Time & b) +{ + return (a.sec == b.sec) && (a.nanosec == b.nanosec); +} + +bool compareDuration( + const builtin_interfaces::msg::Duration & a, + const builtin_interfaces::msg::Duration & b) +{ + return (a.sec == b.sec) && (a.nanosec == b.nanosec); +} + +bool compareHeader( + const std_msgs::msg::Header & a, + const std_msgs::msg::Header & b) +{ + return compareTime(a.stamp, b.stamp) && (a.frame_id == b.frame_id); +} + +bool comparePose( + const geometry_msgs::msg::Pose & a, + const geometry_msgs::msg::Pose & b) +{ + return compareXYZ(a.position, b.position) && compareXYZW(a.orientation, b.orientation); +} + +bool comparePoseArray( + const geometry_msgs::msg::PoseArray & a, + const geometry_msgs::msg::PoseArray & b) +{ + if (!compareHeader(a.header, b.header)) { + return false; + } + if (a.poses.size() != b.poses.size()) { + std::cerr << "a.poses.size() != b.poses.size()" << a.poses.size() << " != " << b.poses.size() << + std::endl; + return false; + } + for (size_t idx = 0; idx < a.poses.size(); ++idx) { + if (!comparePose(a.poses.at(idx), b.poses.at(idx))) { + return false; + } + } + return true; +} + +struct Vector3Test : public GenericTestBase +{ + Vector3Test() + : GenericTestBase("geometry_msgs/Vector3", "geometry_msgs/msg/Vector3") + { + ros1_msg.x = ros2_msg.x = 1.1; + ros1_msg.y = ros2_msg.y = 2.2; + ros1_msg.z = ros2_msg.z = 3.3; + } + + static bool compare( + const geometry_msgs::msg::Vector3 & a, + const geometry_msgs::msg::Vector3 & b) + { + return compareXYZ(a, b); + } +}; + + +struct StringTestEmpty : public GenericTestBase +{ + StringTestEmpty() + : GenericTestBase("std_msgs/String", "std_msgs/msg/String") {} + + static bool compare( + const std_msgs::msg::String & a, + const std_msgs::msg::String & b) + { + return a.data == b.data; + } +}; + + +struct StringTestHello : public StringTestEmpty +{ + StringTestHello() + { + ros1_msg.data = ros2_msg.data = "hello"; + } +}; + +struct TimeTest : public GenericTestBase< + std_msgs::Time, + builtin_interfaces::msg::Time> +{ + TimeTest() + : GenericTestBase("std_msgs/Time", "builtin_interfaces/msg/Time") + { + ros1_msg.data.sec = ros2_msg.sec = 1000 * 2000; + ros1_msg.data.nsec = ros2_msg.nanosec = 1000 * 1000 * 1000; + } + + static bool compare( + const builtin_interfaces::msg::Time & a, + const builtin_interfaces::msg::Time & b) + { + return compareTime(a, b); + } +}; + +struct DurationTest : public GenericTestBase< + std_msgs::Duration, + builtin_interfaces::msg::Duration> +{ + DurationTest() + : GenericTestBase("std_msgs/Duration", "builtin_interfaces/msg/Duration") + { + ros1_msg.data.sec = ros2_msg.sec = 1001 * 2001; + ros1_msg.data.nsec = ros2_msg.nanosec = 1002 * 1003 * 1004; + } + + static bool compare( + const builtin_interfaces::msg::Duration & a, + const builtin_interfaces::msg::Duration & b) + { + return compareDuration(a, b); + } +}; + +struct HeaderTestEmpty : public GenericTestBase< + std_msgs::Header, + std_msgs::msg::Header> +{ + HeaderTestEmpty() + : GenericTestBase("std_msgs/Header", "std_msgs/msg/Header") + { + ros1_msg.stamp.sec = ros2_msg.stamp.sec = 100 * 100; + ros1_msg.stamp.nsec = ros2_msg.stamp.nanosec = 100 * 200 * 300; + ros1_msg.seq = 0; + // Leave header.seq as zero, ros2_msg header does not have seq number so generic + // serialization function will always write a zero to output stream + } + + static bool compare( + const std_msgs::msg::Header & a, + const std_msgs::msg::Header & b) + { + return compareHeader(a, b); + } +}; + +struct HeaderTestBaseLink : public HeaderTestEmpty +{ + HeaderTestBaseLink() + { + ros1_msg.frame_id = ros2_msg.frame_id = "base_link"; + } + + static bool compare( + const std_msgs::msg::Header & a, + const std_msgs::msg::Header & b) + { + return compareHeader(a, b); + } +}; + +struct PoseTest : public GenericTestBase< + geometry_msgs::Pose, + geometry_msgs::msg::Pose> +{ + PoseTest() + : GenericTestBase("geometry_msgs/Pose", "geometry_msgs/msg/Pose") + { + ros1_msg.position.x = ros2_msg.position.x = 1.0; + ros1_msg.position.y = ros2_msg.position.y = 2.0; + ros1_msg.position.z = ros2_msg.position.z = 3.0; + ros1_msg.orientation.x = ros2_msg.orientation.x = 4.0; + ros1_msg.orientation.y = ros2_msg.orientation.y = 5.0; + ros1_msg.orientation.z = ros2_msg.orientation.z = 6.0; + ros1_msg.orientation.w = ros2_msg.orientation.w = 7.0; + } + + static bool compare( + const geometry_msgs::msg::Pose & a, + const geometry_msgs::msg::Pose & b) + { + return comparePose(a, b); + } +}; + +struct PoseArrayTestEmpty : public GenericTestBase< + geometry_msgs::PoseArray, + geometry_msgs::msg::PoseArray> +{ + PoseArrayTestEmpty() + : GenericTestBase("geometry_msgs/PoseArray", "geometry_msgs/msg/PoseArray") {} + + static bool compare( + const geometry_msgs::msg::PoseArray & a, + const geometry_msgs::msg::PoseArray & b) + { + return comparePoseArray(a, b); + } +}; + +struct PoseArrayTest : public PoseArrayTestEmpty +{ + PoseArrayTest() + { + ros1_msg.header.frame_id = ros2_msg.header.frame_id = "base_link"; + ros1_msg.header.stamp.sec = ros2_msg.header.stamp.sec = 0x12345678; + ros1_msg.header.stamp.nsec = ros2_msg.header.stamp.nanosec = 0x76543210; + const size_t size = 10; + ros1_msg.poses.resize(size); + ros2_msg.poses.resize(size); + for (size_t ii = 0; ii < size; ++ii) { + auto & pose1 = ros1_msg.poses.at(ii); + auto & pose2 = ros2_msg.poses.at(ii); + pose1.orientation.x = pose2.orientation.x = 0.1 * ii; + // By default ROS2 message orientation is 1.0, so need to set this + pose1.orientation.w = pose2.orientation.w = 0.9 * ii; + } + } +}; + + +template +class Ros2ToRos1SerializationTest : public testing::Test +{ +public: + using TEST_T = TEST_T_; + using ROS1_T = typename TEST_T::ROS1_T; + using ROS2_T = typename TEST_T::ROS2_T; + using FACTORY_T = typename TEST_T::FACTORY_T; + + TEST_T test; + + void TestBody() {} +}; + + +using Ros2ToRos1SerializationTypes = ::testing::Types< + Vector3Test, // 0 + StringTestEmpty, // 1 + StringTestHello, // 2 + TimeTest, // 3 + DurationTest, // 4 + HeaderTestEmpty, // 5 + HeaderTestBaseLink, // 6 + PoseTest, // 7 + PoseArrayTestEmpty, // 8 + PoseArrayTest // 9 +>; +TYPED_TEST_SUITE(Ros2ToRos1SerializationTest, Ros2ToRos1SerializationTypes); + + +// cppcheck-suppress syntaxError +TYPED_TEST(Ros2ToRos1SerializationTest, test_length) +{ + using FACTORY_T = typename TestFixture::FACTORY_T; + using ROS1_T = typename TestFixture::ROS1_T; + using ROS2_T = typename TestFixture::ROS2_T; + const ROS1_T & ros1_msg = this->test.ros1_msg; + const ROS2_T & ros2_msg = this->test.ros2_msg; + uint32_t ros1_len = ros::serialization::serializationLength(ros1_msg); + uint32_t ros2_len = FACTORY_T::length_2_as_1_stream(ros2_msg); + EXPECT_EQ(ros1_len, ros2_len); +} + + +// cppcheck-suppress syntaxError +TYPED_TEST(Ros2ToRos1SerializationTest, test_get_factory) +{ + // Make sure get_factory still works with Header and other types + const std::string & ros1_type_name = this->test.factory.ros1_type_name_; + const std::string & ros2_type_name = this->test.factory.ros2_type_name_; + std::shared_ptr factory; + EXPECT_NO_THROW(factory = ros1_bridge::get_factory(ros1_type_name, ros2_type_name)); + ASSERT_TRUE(static_cast(factory)); +} + + +// cppcheck-suppress syntaxError +TYPED_TEST(Ros2ToRos1SerializationTest, test_deserialization) +{ + using FACTORY_T = typename TestFixture::FACTORY_T; + using ROS1_T = typename TestFixture::ROS1_T; + using ROS2_T = typename TestFixture::ROS2_T; + const ROS1_T & ros1_msg = this->test.ros1_msg; + const ROS2_T & ros2_msg = this->test.ros2_msg; + const uint32_t ros1_len = ros::serialization::serializationLength(ros1_msg); + std::vector buffer(ros1_len); + ros::serialization::OStream out_stream(buffer.data(), ros1_len); + FACTORY_T::convert_2_to_1(ros2_msg, out_stream); + ros::serialization::IStream in_stream(buffer.data(), ros1_len); + ROS2_T ros2_msg2; + FACTORY_T::convert_1_to_2(in_stream, ros2_msg2); + EXPECT_TRUE(TestFixture::TEST_T::compare(ros2_msg, ros2_msg2)); +} + + +// cppcheck-suppress syntaxError +TYPED_TEST(Ros2ToRos1SerializationTest, test_serialization) +{ + using FACTORY_T = typename TestFixture::FACTORY_T; + using ROS1_T = typename TestFixture::ROS1_T; + using ROS2_T = typename TestFixture::ROS2_T; + const ROS1_T & ros1_msg = this->test.ros1_msg; + const ROS2_T & ros2_msg = this->test.ros2_msg; + const uint32_t ros1_len = ros::serialization::serializationLength(ros1_msg); + const uint32_t ros2_len = FACTORY_T::length_2_as_1_stream(ros2_msg); + ASSERT_EQ(ros1_len, ros2_len); + const uint32_t len = ros1_len; + + // Serialize both streams and check byte-by-byte the value match + const uint8_t guard_value = 0x42; + + std::vector buffer1(len + 1); + { + buffer1.at(len) = guard_value; + ros::serialization::OStream stream(buffer1.data(), len); + EXPECT_EQ(stream.getLength(), len); + ros::serialization::serialize(stream, ros1_msg); + // after serializating all the "capacity" should be used up + EXPECT_EQ(stream.getLength(), 0ul); + // Hacky check to make sure serialization didn't run over the end of the buffer + EXPECT_EQ(buffer1.at(len), guard_value); + } + + std::vector buffer2(len + 1); + { + buffer2.at(len) = guard_value; + ros::serialization::OStream stream(buffer2.data(), len); + EXPECT_EQ(stream.getLength(), len); + FACTORY_T::convert_2_to_1(ros2_msg, stream); + EXPECT_EQ(stream.getLength(), 0ul); + EXPECT_EQ(buffer2.at(len), guard_value); + } + + unsigned mismatch_count = 0; + const unsigned max_mismatch_count = 0; + for (size_t idx = 0; idx < len; ++idx) { + unsigned val1 = buffer1.at(idx); + unsigned val2 = buffer2.at(idx); + EXPECT_EQ(val1, val2) << " idx=" << idx << " of " << len; + if (val1 != val2) { + ++mismatch_count; + ASSERT_LE(mismatch_count, max_mismatch_count); + } + } + ASSERT_EQ(mismatch_count, 0u); +} + + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}