Support topic instances (#178)

* Initial implementation of keyed instances support for DDS topics

* Return correct max key size and serialize using correct endianness

* Apply uncrustify

* Fix Windows compilation and warning

Signed-off-by: Francisco Gallego Salido <[email protected]>

* Applied feedback

Signed-off-by: Francisco Gallego Salido <[email protected]>

---------

Signed-off-by: Francisco Gallego Salido <[email protected]>

Author: fgallegosalido

rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp

@@ -44,7 +44,8 @@
 rcutils_ret_t
 rcutils_uint8_array_copy(
   rcutils_uint8_array_t * const dst,
-  const rcutils_uint8_array_t * const src);
+  const rcutils_uint8_array_t * const src,
+  const bool realloc_if_needed = true);
 
 rmw_qos_policy_kind_t
 dds_qos_policy_to_rmw_qos_policy(const DDS_QosPolicyId_t last_policy_id);

rmw_connextdds_common/include/rmw_connextdds/type_support.hpp

@@ -15,6 +15,7 @@
 #ifndef RMW_CONNEXTDDS__TYPE_SUPPORT_HPP_
 #define RMW_CONNEXTDDS__TYPE_SUPPORT_HPP_
 
+#include <cstdint>
 #include <string>
 #include <stdexcept>
 
@@ -56,10 +57,16 @@ struct RMW_Connext_RequestReplyMessage
 
 class RMW_Connext_MessageTypeSupport
 {
+  const rosidl_message_type_support_t * _message_type_support;
   const rosidl_message_type_support_t * _type_support_fastrtps;
+  message_type_support_key_callbacks_t _key_callbacks;
   bool _unbounded;
   bool _empty;
+  bool _keyed;
+  bool _unbounded_key;
+  bool _is_cpp;
   uint32_t _serialized_size_max;
+  uint32_t _key_serialized_size_max;
   std::string _type_name;
   RMW_Connext_MessageType _message_type;
   rmw_context_impl_t * const _ctx;
@@ -79,6 +86,11 @@ class RMW_Connext_MessageTypeSupport
       this->_type_support_fastrtps->data);
   }
 
+  const rosidl_message_type_support_t * message_type_support() const
+  {
+    return this->_message_type_support;
+  }
+
   rmw_context_impl_t * ctx() const
   {
     return this->_ctx;
@@ -99,6 +111,14 @@ class RMW_Connext_MessageTypeSupport
     return this->_serialized_size_max;
   }
 
+  uint32_t type_key_serialized_size_max() const
+  {
+    if (!this->_keyed) {
+      return 0;
+    }
+    return this->_key_serialized_size_max;
+  }
+
   bool unbounded() const
   {
     return this->_unbounded;
@@ -109,6 +129,21 @@ class RMW_Connext_MessageTypeSupport
     return this->_empty;
   }
 
+  bool keyed() const
+  {
+    return this->_keyed;
+  }
+
+  bool unbounded_key() const
+  {
+    return this->_unbounded_key;
+  }
+
+  bool is_cpp() const
+  {
+    return this->_is_cpp;
+  }
+
   RMW_Connext_MessageType message_type() const
   {
     return this->_message_type;
@@ -139,14 +174,32 @@ class RMW_Connext_MessageTypeSupport
 
   rmw_ret_t serialize(
     const void * const ros_msg,
-    rcutils_uint8_array_t * const to_buffer);
+    rcutils_uint8_array_t * const to_buffer,
+    const bool include_encapsulation = true);
 
   rmw_ret_t deserialize(
     void * const ros_msg,
     const rcutils_uint8_array_t * const from_buffer,
-    size_t & size_out,
     const bool header_only = false);
 
+  rmw_ret_t serialize_key(
+    const void * const ros_msg,
+    rcutils_uint8_array_t * const to_buffer,
+    RTIEncapsulationId encapsulation_id,
+    const bool include_encapsulation = true);
+
+  // Not available in message_type_support_key_callbacks_t yet
+  //
+  // Because deserialize_key is not supported yet there will be errors receiving dispose samples
+  // that have the serialized key as payload and the sample will be reported as lost.
+  //
+  // By default, dispose samples do not contain the serialized key, so this should only be a
+  // problem if the DataWriterQos.protocol.serialize_key_with_dispose is set to true.
+  //
+  // rmw_ret_t deserialize_key(
+  //   void * const ros_msg,
+  //   const rcutils_uint8_array_t * const from_buffer);
+
   static
   RMW_Connext_MessageTypeSupport *
   register_type_support(
@@ -174,7 +227,11 @@ class RMW_Connext_MessageTypeSupport
     const rosidl_message_type_support_t * const type_support,
     uint32_t & serialized_size_max,
     bool & unbounded,
-    bool & empty);
+    bool & empty,
+    bool & keyed,
+    bool & unbounded_key,
+    message_type_support_key_callbacks_t & key_callbacks,
+    uint32_t & key_serialized_size_max);
 };
 
 struct RMW_Connext_Message

rmw_connextdds_common/src/common/rmw_impl.cpp

@@ -69,10 +69,15 @@ rmw_connextdds_create_topic_name(
 rcutils_ret_t
 rcutils_uint8_array_copy(
   rcutils_uint8_array_t * const dst,
-  const rcutils_uint8_array_t * const src)
+  const rcutils_uint8_array_t * const src,
+  const bool realloc_if_needed)
 {
   if (src->buffer_length > 0) {
     if (src->buffer_length > dst->buffer_capacity) {
+      if (!realloc_if_needed) {
+        return RCUTILS_RET_ERROR;
+      }
+
       rcutils_ret_t rc =
         rcutils_uint8_array_resize(dst, src->buffer_length);
 
@@ -1844,12 +1849,9 @@ RMW_Connext_Subscriber::take_next(
         // request header.
         if (this->type_support->type_requestreply()) {
           if (this->ctx->request_reply_mapping == RMW_Connext_RequestReplyMapping::Basic) {
-            size_t deserialized_size = 0;
-            UNUSED_ARG(deserialized_size);
-
             if (RMW_RET_OK !=
               this->type_support->deserialize(
-                ros_message, &msg->data_buffer, deserialized_size, true /* header_only */))
+                ros_message, &msg->data_buffer, true /* header_only */))
             {
               RMW_CONNEXT_LOG_ERROR_SET("failed to deserialize taken sample")
               rc_exit = RMW_RET_ERROR;
@@ -1895,11 +1897,8 @@ RMW_Connext_Subscriber::take_next(
             continue;
           }
         } else {
-          size_t deserialized_size = 0;
-
           if (RMW_RET_OK !=
-            this->type_support->deserialize(
-              ros_message, &msg->data_buffer, deserialized_size))
+            this->type_support->deserialize(ros_message, &msg->data_buffer))
           {
             RMW_CONNEXT_LOG_ERROR_SET(
               "failed to deserialize taken sample")