Port message_collection_impl.hpp to ROS2

include/warehouse_ros/impl/message_collection_impl.hpp

@@ -37,6 +37,10 @@
  * \author Bhaskara Marthi
  */
 
+#include <rcutils/allocator.h>
+
+#include <rmw/serialized_message.h>
+
 namespace warehouse_ros
 {
 template <class M>
@@ -74,16 +78,33 @@ void MessageCollection<M>::insert(const M& msg, Metadata::Ptr metadata)
   if (!md5sum_matches_)
     throw Md5SumException("Cannot insert additional elements.");
 
-  metadata->append("creation_time", ros::Time::now().toSec());
+  metadata->append("creation_time", rclcpp::Clock().now().seconds());
 
   /// Serialize the message into a buffer
-  size_t serial_size = ros::serialization::serializationLength(msg);
-  boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
-  ros::serialization::OStream stream(buffer.get(), serial_size);
-  ros::serialization::serialize(stream, msg);
-  char* data = (char*)buffer.get();
-
-  collection_->insert(data, serial_size, metadata);
+  // size_t serial_size = ros::serialization::serializationLength(msg);
+  // boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
+  // ros::serialization::OStream stream(buffer.get(), serial_size);
+  // ros::serialization::serialize(stream, msg);
+  // char* data = (char*)buffer.get();
+  rcl_serialized_message_t serialized_msg_ = rmw_get_zero_initialized_serialized_message();
+  auto allocator = rcutils_get_default_allocator();
+  auto initial_capacity = 0u;
+  auto ret = rmw_serialized_message_init(
+    &serialized_msg_,
+    initial_capacity,
+    &allocator);
+  if (ret != RCL_RET_OK) {
+    throw std::runtime_error("failed to initialize serialized message");
+  }
+
+  auto string_ts = rosidl_typesupport_cpp::get_message_type_support_handle<M>();
+  ret = rmw_serialize(&msg, string_ts, &serialized_msg_);
+  if (ret != RMW_RET_OK) {
+    fprintf(stderr, "failed to serialize serialized message\n");
+    return;
+  }
+  char* data = (char*)serialized_msg_.buffer;
+  collection_->insert(data, serialized_msg_.buffer_length, metadata);
 }
 
 template <class M>