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 <[email protected]> * Partial set of fixes from review. Signed-off-by: Derek King <[email protected]> * Rename functions based on discussion from review Signed-off-by: Derek King <[email protected]> * Whitespace fix. Signed-off-by: Derek King <[email protected]> * Name change for conversion helper fucntion. Signed-off-by: Derek King <[email protected]> * Use explicit function overloading instead of template for stream types. Signed-off-by: Derek King <[email protected]> * Fix factory.hpp crustify errors. Signed-off-by: Derek King <[email protected]> Signed-off-by: Derek King <[email protected]>
ros2 · Jan 16, 2023 · c2fa9ed · c2fa9ed
1 parent bed9d5d
commit c2fa9ed
Show file tree

Hide file tree

Showing 11 changed files with 1,116 additions and 2 deletions.
diff --git 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
@@ -15,8 +15,11 @@
 #ifndef ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_
 #define ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_
 
+#include <ros/serialization.h>
+
 // include ROS 1 messages
 #include <std_msgs/Duration.h>
+#include <std_msgs/Header.h>
 #include <std_msgs/Time.h>
 
 #include <memory>
@@ -25,6 +28,7 @@
 // include ROS 2 messages
 #include <builtin_interfaces/msg/duration.hpp>
 #include <builtin_interfaces/msg/time.hpp>
+#include <std_msgs/msg/header.hpp>
 
 #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
@@ -40,6 +40,19 @@ convert_2_to_1(
   const builtin_interfaces::msg::Duration & ros2_msg,
   ros::Duration & ros1_type);
 
+template<typename STREAM_T>
+void
+internal_stream_translate_helper(
+  STREAM_T & stream,
+  const builtin_interfaces::msg::Duration & msg);
+
+template<typename STREAM_T>
+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<typename STREAM_T>
+void
+internal_stream_translate_helper(
+  STREAM_T & stream,
+  const builtin_interfaces::msg::Time & msg);
+
+template<typename STREAM_T>
+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
@@ -30,6 +30,12 @@ convert_2_to_1(
   const ROS2_T & ros2_msg,
   ROS1_T & ros1_msg);
 
+template<typename ROS2_T, typename STREAM_T>
+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
@@ -352,13 +352,45 @@ 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_;
 
   std::shared_ptr<rcpputils::SharedLibrary> ts_lib_;
   const rosidl_message_type_support_t * type_support_ = nullptr;
 };
 
+
 template<class ROS1_T, class ROS2_T>
 class ServiceFactory : public ServiceFactoryInterface
 {

diff --git a/package.xml b/package.xml
@@ -47,11 +47,13 @@
   <test_depend>ament_lint_common</test_depend>
   <test_depend>demo_nodes_cpp</test_depend>
   <test_depend>diagnostic_msgs</test_depend>
+  <test_depend>geometry_msgs</test_depend>
   <test_depend>launch</test_depend>
   <test_depend>launch_testing</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>launch_testing_ros</test_depend>
   <test_depend>ros2run</test_depend>
+  <test_depend>sensor_msgs</test_depend>
 
   <group_depend>rosidl_interface_packages</group_depend>