From 67ad27ad100699c9b413d8a1cdc00c948affb6d1 Mon Sep 17 00:00:00 2001
From: RoboticsYY <yu.yan@intel.com>
Date: Wed, 5 Aug 2020 19:26:56 +0800
Subject: [PATCH] Port message_collection_impl.hpp to ROS2

---
 .../impl/message_collection_impl.hpp          | 37 +++++++++++++++----
 1 file changed, 29 insertions(+), 8 deletions(-)

diff --git a/include/warehouse_ros/impl/message_collection_impl.hpp b/include/warehouse_ros/impl/message_collection_impl.hpp
index c1842e4..42a6800 100644
--- a/include/warehouse_ros/impl/message_collection_impl.hpp
+++ b/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>