New function for converting between ROS1 and ROS2 generic message for…

…mats. (#377)

Signed-off-by: Derek King <[email protected]>

Signed-off-by: Derek King <[email protected]>

CMakeLists.txt

@@ -80,6 +80,29 @@ if(BUILD_TESTING)
  endif()
  set(TEST_ROS1_BRIDGE TRUE)
  endif()
+
+ find_package(ament_cmake_gtest REQUIRED)
+ find_package(geometry_msgs REQUIRED)
+ find_package(std_msgs REQUIRED)
+ find_package(sensor_msgs REQUIRED)
+ find_ros1_package(geometry_msgs)
+ find_ros1_package(std_msgs)
+ find_ros1_package(sensor_msgs)
+ 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
+ "ros1_roscpp"
+ "ros1_sensor_msgs"
+ "ros1_std_msgs"
+ "ros1_geometry_msgs"
+ "sensor_msgs"
+ "std_msgs"
+ "geometry_msgs"
+ "rclcpp"
+ )
+ target_link_libraries(test_convert_generic ${PROJECT_NAME})
+ endif()
+
 endif()
 
 ament_export_include_directories(include)

include/ros1_bridge/factory.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 #include <string>
 #include <utility>
+#include <vector>
 
 #include "rmw/rmw.h"
 #include "rclcpp/rclcpp.hpp"
@@ -41,7 +42,14 @@ class Factory : public FactoryInterface
  const std::string & ros1_type_name, const std::string & ros2_type_name)
  : ros1_type_name_(ros1_type_name),
  ros2_type_name_(ros2_type_name)
- {}
+ {
+ ts_lib_ = rclcpp::get_typesupport_library(ros2_type_name, "rosidl_typesupport_cpp");
+ if (static_cast<bool>(ts_lib_)) {
+ type_support_ = rclcpp::get_typesupport_handle(
+ ros2_type_name, "rosidl_typesupport_cpp",
+ *ts_lib_);
+ }
+ }
 
  ros::Publisher
  create_ros1_publisher(
@@ -149,14 +157,14 @@ class Factory : public FactoryInterface
  topic_name, qos, callback, options);
  }
 
- void convert_1_to_2(const void * ros1_msg, void * ros2_msg) override
+ void convert_1_to_2(const void * ros1_msg, void * ros2_msg) const override
  {
  auto typed_ros1_msg = static_cast<const ROS1_T *>(ros1_msg);
  auto typed_ros2_msg = static_cast<ROS2_T *>(ros2_msg);
  convert_1_to_2(*typed_ros1_msg, *typed_ros2_msg);
  }
 
- void convert_2_to_1(const void * ros2_msg, void * ros1_msg) override
+ void convert_2_to_1(const void * ros2_msg, void * ros1_msg) const override
  {
  auto typed_ros2_msg = static_cast<const ROS2_T *>(ros2_msg);
  auto typed_ros1_msg = static_cast<ROS1_T *>(ros1_msg);
@@ -266,8 +274,89 @@ class Factory : public FactoryInterface
  const ROS2_T & ros2_msg,
  ROS1_T & ros1_msg);
 
+
+ const char * get_ros1_md5sum() const override
+ {
+ return ros::message_traits::MD5Sum<ROS1_T>::value();
+ }
+
+ const char * get_ros1_data_type() const override
+ {
+ return ros::message_traits::DataType<ROS1_T>::value();
+ }
+
+ const char * get_ros1_message_definition() const override
+ {
+ return ros::message_traits::Definition<ROS1_T>::value();
+ }
+
+ bool convert_2_to_1_generic(
+ const rclcpp::SerializedMessage & ros2_msg,
+ std::vector<uint8_t> & ros1_msg) const override
+ {
+ if (type_support_ == nullptr) {
+ return false;
+ }
+
+ // Deserialize to a ROS2 message
+ ROS2_T ros2_typed_msg;
+ if (rmw_deserialize(
+ &ros2_msg.get_rcl_serialized_message(), type_support_,
+ &ros2_typed_msg) != RMW_RET_OK)
+ {
+ return false;
+ }
+
+ // Call convert_2_to_1
+ ROS1_T ros1_typed_msg;
+ convert_2_to_1(&ros2_typed_msg, &ros1_typed_msg);
+
+ // Serialize the ROS1 message into a buffer
+ uint32_t length = ros::serialization::serializationLength(ros1_typed_msg);
+ ros1_msg.resize(length);
+ ros::serialization::OStream out_stream(ros1_msg.data(), length);
+ ros::serialization::serialize(out_stream, ros1_typed_msg);
+
+ return true;
+ }
+
+ bool convert_1_to_2_generic(
+ const std::vector<uint8_t> & ros1_msg,
+ rclcpp::SerializedMessage & ros2_msg) const override
+ {
+ if (type_support_ == nullptr) {
+ return false;
+ }
+
+ // Deserialize to a ROS1 message
+ ROS1_T ros1_typed_msg;
+ // Both IStream and OStream inherits their functionality from Stream
+ // So IStream needs a non-const data reference to data
+ // However deserialization function probably shouldn't modify data they are serializing from
+ uint8_t * ros1_msg_data = const_cast<uint8_t *>(ros1_msg.data());
+ ros::serialization::IStream in_stream(ros1_msg_data, ros1_msg.size());
+ ros::serialization::deserialize(in_stream, ros1_typed_msg);
+
+ // Call convert_1_to_2
+ ROS2_T ros2_typed_msg;
+ convert_1_to_2(&ros1_typed_msg, &ros2_typed_msg);
+
+ // Serialize ROS2 message
+ if (rmw_serialize(
+ &ros2_typed_msg, type_support_,
+ &ros2_msg.get_rcl_serialized_message()) != RMW_RET_OK)
+ {
+ return false;
+ }
+
+ return true;
+ }
+
  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>

include/ros1_bridge/factory_interface.hpp

@@ -16,6 +16,7 @@
 #define ROS1_BRIDGE__FACTORY_INTERFACE_HPP_
 
 #include <string>
+#include <vector>
 
 // include ROS 1
 #include "ros/node_handle.h"
@@ -112,11 +113,25 @@ class FactoryInterface
 
  virtual
  void
- convert_1_to_2(const void * ros1_msg, void * ros2_msg) = 0;
+ convert_1_to_2(const void * ros1_msg, void * ros2_msg) const = 0;
 
  virtual
  void
- convert_2_to_1(const void * ros2_msg, void * ros1_msg) = 0;
+ convert_2_to_1(const void * ros2_msg, void * ros1_msg) const = 0;
+
+ virtual
+ bool convert_2_to_1_generic(
+ const rclcpp::SerializedMessage & ros2_msg,
+ std::vector<uint8_t> & ros1_msg) const = 0;
+
+ virtual
+ bool convert_1_to_2_generic(
+ const std::vector<uint8_t> & ros1_msg,
+ rclcpp::SerializedMessage & ros2_msg) const = 0;
+
+ virtual const char * get_ros1_md5sum() const = 0;
+ virtual const char * get_ros1_data_type() const = 0;
+ virtual const char * get_ros1_message_definition() const = 0;
 };
 
 class ServiceFactoryInterface

package.xml

@@ -40,6 +40,7 @@
  <exec_depend>rcutils</exec_depend>
  <exec_depend>std_msgs</exec_depend>
 
+ <test_depend>ament_cmake_gtest</test_depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
  <test_depend>demo_nodes_cpp</test_depend>