Merge branch 'master' into sriram_beta3

Signed-off-by: Sriram Raghunathan <sriram.max@gmail.com>

rmw_fastrtps_cpp/CMakeLists.txt

@@ -36,14 +36,12 @@ find_package(FastRTPS REQUIRED MODULE)
 
 find_package(rmw REQUIRED)
 find_package(rosidl_generator_c REQUIRED)
-find_package(rosidl_generator_cpp REQUIRED)
 find_package(rosidl_typesupport_introspection_c REQUIRED)
 find_package(rosidl_typesupport_introspection_cpp REQUIRED)
 
 ament_export_dependencies(rcutils)
 ament_export_dependencies(rmw)
 ament_export_dependencies(rosidl_generator_c)
-ament_export_dependencies(rosidl_generator_cpp)
 ament_export_dependencies(rosidl_typesupport_introspection_c)
 ament_export_dependencies(rosidl_typesupport_introspection_cpp)
 
@@ -60,7 +58,6 @@ add_library(rmw_fastrtps_cpp
   src/identifier.cpp
   src/namespace_prefix.cpp
   src/qos.cpp
-  src/rmw_init.cpp
   src/rmw_logging.cpp
   src/rmw_client.cpp
   src/rmw_compare_gids_equal.cpp
@@ -96,7 +93,7 @@ ament_target_dependencies(rmw_fastrtps_cpp
   "rosidl_typesupport_introspection_cpp"
   "rmw"
   "rosidl_generator_c"
-  "rosidl_generator_cpp")
+)
 
 configure_rmw_library(rmw_fastrtps_cpp)
 

rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport.hpp

@@ -133,11 +133,12 @@ class TypeSupport : public eprosima::fastrtps::TopicDataType
   const MembersType * members_;
 
 private:
-  bool serializeROSmessage(eprosima::fastcdr::Cdr & ser, const MembersType * members,
-    const void * ros_message);
+  bool serializeROSmessage(
+    eprosima::fastcdr::Cdr & ser, const MembersType * members, const void * ros_message);
 
-  bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, const MembersType * members,
-    void * ros_message, bool call_new);
+  bool deserializeROSmessage(
+    eprosima::fastcdr::Cdr & deser, const MembersType * members, void * ros_message,
+    bool call_new);
 };
 
 }  // namespace rmw_fastrtps_cpp

rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/TypeSupport_impl.hpp

@@ -118,17 +118,11 @@ void TypeSupport<MembersType>::deleteData(void * data)
 }
 
 static inline void *
-align_(size_t __align, size_t __size, void * & __ptr, size_t & __space) noexcept
+align_(size_t __align, void * & __ptr) noexcept
 {
   const auto __intptr = reinterpret_cast<uintptr_t>(__ptr);
   const auto __aligned = (__intptr - 1u + __align) & ~(__align - 1);
-  const auto __diff = __aligned - __intptr;
-  if ((__size + __diff) > __space) {
-    return nullptr;
-  } else {
-    __space -= __diff;
-    return __ptr = reinterpret_cast<void *>(__aligned);
-  }
+  return __ptr = reinterpret_cast<void *>(__aligned);
 }
 
 template<typename MembersType>
@@ -273,12 +267,11 @@ size_t get_array_size_and_assign_field(
   void * field,
   void * & subros_message,
   size_t sub_members_size,
-  size_t max_align,
-  size_t space)
+  size_t max_align)
 {
   std::vector<unsigned char> * vector = reinterpret_cast<std::vector<unsigned char> *>(field);
   void * ptr = reinterpret_cast<void *>(sub_members_size);
-  size_t vsize = vector->size() / reinterpret_cast<size_t>(align_(max_align, 0, ptr, space));
+  size_t vsize = vector->size() / reinterpret_cast<size_t>(align_(max_align, ptr));
   if (member->is_upper_bound_ && vsize > member->array_size_) {
     throw std::runtime_error("vector overcomes the maximum length");
   }
@@ -291,7 +284,7 @@ size_t get_array_size_and_assign_field(
   const rosidl_typesupport_introspection_c__MessageMember * member,
   void * field,
   void * & subros_message,
-  size_t, size_t, size_t)
+  size_t, size_t)
 {
   rosidl_generator_c__void__Array * tmparray =
     static_cast<rosidl_generator_c__void__Array *>(field);
@@ -424,14 +417,13 @@ bool TypeSupport<MembersType>::serializeROSmessage(
             size_t array_size = 0;
             size_t sub_members_size = sub_members->size_of_;
             size_t max_align = calculateMaxAlign(sub_members);
-            size_t space = 100;
 
             if (member->array_size_ && !member->is_upper_bound_) {
               subros_message = field;
               array_size = member->array_size_;
             } else {
               array_size = get_array_size_and_assign_field(
-                member, field, subros_message, sub_members_size, max_align, space);
+                member, field, subros_message, sub_members_size, max_align);
 
               // Serialize length
               ser << (uint32_t)array_size;
@@ -440,8 +432,7 @@ bool TypeSupport<MembersType>::serializeROSmessage(
             for (size_t index = 0; index < array_size; ++index) {
               serializeROSmessage(ser, sub_members, subros_message);
               subros_message = static_cast<char *>(subros_message) + sub_members_size;
-              // TODO(richiprosima) Change 100 values.
-              subros_message = align_(max_align, 0, subros_message, space);
+              subros_message = align_(max_align, subros_message);
             }
           }
           break;
@@ -536,8 +527,7 @@ inline size_t get_submessage_array_deserialize(
   void * & subros_message,
   bool call_new,
   size_t sub_members_size,
-  size_t max_align,
-  size_t space)
+  size_t max_align)
 {
   (void)member;
   uint32_t vsize = 0;
@@ -548,7 +538,7 @@ inline size_t get_submessage_array_deserialize(
     new(vector) std::vector<unsigned char>;
   }
   void * ptr = reinterpret_cast<void *>(sub_members_size);
-  vector->resize(vsize * (size_t)align_(max_align, 0, ptr, space));
+  vector->resize(vsize * (size_t)align_(max_align, ptr));
   subros_message = reinterpret_cast<void *>(vector->data());
   return vsize;
 }
@@ -560,7 +550,7 @@ inline size_t get_submessage_array_deserialize(
   void * & subros_message,
   bool,
   size_t sub_members_size,
-  size_t, size_t)
+  size_t)
 {
   (void)member;
   // Deserialize length
@@ -682,7 +672,6 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
             size_t array_size = 0;
             size_t sub_members_size = sub_members->size_of_;
             size_t max_align = calculateMaxAlign(sub_members);
-            size_t space = 100;
             bool recall_new = call_new;
 
             if (member->array_size_ && !member->is_upper_bound_) {
@@ -691,15 +680,14 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
             } else {
               array_size = get_submessage_array_deserialize(
                 member, deser, field, subros_message,
-                call_new, sub_members_size, max_align, space);
+                call_new, sub_members_size, max_align);
               recall_new = true;
             }
 
             for (size_t index = 0; index < array_size; ++index) {
               deserializeROSmessage(deser, sub_members, subros_message, recall_new);
               subros_message = static_cast<char *>(subros_message) + sub_members_size;
-              // TODO(richiprosima) Change 100 values.
-              subros_message = align_(max_align, 0, subros_message, space);
+              subros_message = align_(max_align, subros_message);
             }
           }
           break;
@@ -913,7 +901,7 @@ std::function<uint32_t()> TypeSupport<MembersType>::getSerializedSizeProvider(vo
   assert(data);
 
   eprosima::fastcdr::Cdr * ser = static_cast<eprosima::fastcdr::Cdr *>(data);
-  return [ser]()->uint32_t {return static_cast<uint32_t>(ser->getSerializedDataLength()); };
+  return [ser]() -> uint32_t {return static_cast<uint32_t>(ser->getSerializedDataLength());};
 }
 
 }  // namespace rmw_fastrtps_cpp

rmw_fastrtps_cpp/package.xml

@@ -18,7 +18,6 @@
   <build_depend>rcutils</build_depend>
   <build_depend>rmw</build_depend>
   <build_depend>rosidl_generator_c</build_depend>
-  <build_depend>rosidl_generator_cpp</build_depend>
   <build_depend>rosidl_typesupport_introspection_c</build_depend>
   <build_depend>rosidl_typesupport_introspection_cpp</build_depend>
 
@@ -28,7 +27,6 @@
   <build_export_depend>rcutils</build_export_depend>
   <build_export_depend>rmw</build_export_depend>
   <build_export_depend>rosidl_generator_c</build_export_depend>
-  <build_export_depend>rosidl_generator_cpp</build_export_depend>
   <build_export_depend>rosidl_typesupport_introspection_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_introspection_cpp</build_export_depend>
 

rmw_fastrtps_cpp/src/demangle.cpp

@@ -64,9 +64,12 @@ _demangle_service_from_topic(const std::string & topic_name)
     ros_service_response_prefix,
     ros_service_requester_prefix,
   };
-  if (std::none_of(prefixes.cbegin(), prefixes.cend(), [&prefix](auto x) {
-    return prefix == x;
-  }))
+  if (
+    std::none_of(
+      prefixes.cbegin(), prefixes.cend(),
+      [&prefix](auto x) {
+        return prefix == x;
+      }))
   {
     // not a ROS service topic
     return "";

rmw_fastrtps_cpp/src/qos.cpp

@@ -78,8 +78,7 @@ get_datareader_qos(
   assert(sattr.topic.historyQos.depth >= 0);
   if (
     sattr.topic.historyQos.kind == eprosima::fastrtps::KEEP_LAST_HISTORY_QOS &&
-    static_cast<size_t>(sattr.topic.historyQos.depth) < qos_policies.depth
-  )
+    static_cast<size_t>(sattr.topic.historyQos.depth) < qos_policies.depth)
   {
     if (qos_policies.depth > (std::numeric_limits<int32_t>::max)()) {
       RMW_SET_ERROR_MSG(
@@ -146,8 +145,7 @@ get_datawriter_qos(
   assert(pattr.topic.historyQos.depth >= 0);
   if (
     pattr.topic.historyQos.kind == eprosima::fastrtps::KEEP_LAST_HISTORY_QOS &&
-    static_cast<size_t>(pattr.topic.historyQos.depth) < qos_policies.depth
-  )
+    static_cast<size_t>(pattr.topic.historyQos.depth) < qos_policies.depth)
   {
     if (qos_policies.depth > (std::numeric_limits<int32_t>::max)()) {
       RMW_SET_ERROR_MSG(

rmw_fastrtps_cpp/src/rmw_client.cpp

@@ -35,7 +35,8 @@
 extern "C"
 {
 rmw_client_t *
-rmw_create_client(const rmw_node_t * node,
+rmw_create_client(
+  const rmw_node_t * node,
   const rosidl_service_type_support_t * type_supports,
   const char * service_name, const rmw_qos_profile_t * qos_policies)
 {
@@ -50,7 +51,8 @@ rmw_create_client(const rmw_node_t * node,
   }
 
   if (!service_name || strlen(service_name) == 0) {
-    RMW_SET_ERROR_MSG("publisher topic is null or empty string");
+    RMW_SET_ERROR_MSG("client topic is null or empty string");
+    return NULL;
   }
 
   if (!qos_policies) {

rmw_fastrtps_cpp/src/rmw_node.cpp

@@ -194,6 +194,10 @@ rmw_create_node(
   }
 
   ParticipantAttributes participantAttrs;
+
+  // Load default XML profile.
+  Domain::getDefaultParticipantAttributes(participantAttrs);
+
   participantAttrs.rtps.builtin.domainId = static_cast<uint32_t>(domain_id);
   participantAttrs.rtps.setName(name);
 

rmw_fastrtps_cpp/src/rmw_publisher.cpp

@@ -29,7 +29,8 @@
 extern "C"
 {
 rmw_publisher_t *
-rmw_create_publisher(const rmw_node_t * node,
+rmw_create_publisher(
+  const rmw_node_t * node,
   const rosidl_message_type_support_t * type_supports,
   const char * topic_name, const rmw_qos_profile_t * qos_policies)
 {
@@ -45,6 +46,7 @@ rmw_create_publisher(const rmw_node_t * node,
 
   if (!topic_name || strlen(topic_name) == 0) {
     RMW_SET_ERROR_MSG("publisher topic is null or empty string");
+    return NULL;
   }
 
   if (!qos_policies) {
@@ -80,6 +82,9 @@ rmw_create_publisher(const rmw_node_t * node,
   PublisherAttributes publisherParam;
   const GUID_t * guid = nullptr;
 
+  // Load default XML profile.
+  Domain::getDefaultPublisherAttributes(publisherParam);
+
   // TODO(karsten1987) Verify consequences for std::unique_ptr?
   info = new CustomPublisherInfo();
   info->typesupport_identifier_ = type_support->typesupport_identifier;

rmw_fastrtps_cpp/src/rmw_request.cpp

@@ -31,7 +31,8 @@
 extern "C"
 {
 rmw_ret_t
-rmw_send_request(const rmw_client_t * client,
+rmw_send_request(
+  const rmw_client_t * client,
   const void * ros_request,
   int64_t * sequence_id)
 {
@@ -73,7 +74,8 @@ rmw_send_request(const rmw_client_t * client,
 }
 
 rmw_ret_t
-rmw_take_request(const rmw_service_t * service,
+rmw_take_request(
+  const rmw_service_t * service,
   rmw_request_id_t * request_header,
   void * ros_request,
   bool * taken)

rmw_fastrtps_cpp/src/rmw_response.cpp

@@ -29,7 +29,8 @@
 extern "C"
 {
 rmw_ret_t
-rmw_take_response(const rmw_client_t * client,
+rmw_take_response(
+  const rmw_client_t * client,
   rmw_request_id_t * request_header,
   void * ros_response,
   bool * taken)
@@ -67,7 +68,8 @@ rmw_take_response(const rmw_client_t * client,
 }
 
 rmw_ret_t
-rmw_send_response(const rmw_service_t * service,
+rmw_send_response(
+  const rmw_service_t * service,
   rmw_request_id_t * request_header,
   void * ros_response)
 {

rmw_fastrtps_cpp/src/rmw_service.cpp

@@ -45,7 +45,8 @@
 extern "C"
 {
 rmw_service_t *
-rmw_create_service(const rmw_node_t * node,
+rmw_create_service(
+  const rmw_node_t * node,
   const rosidl_service_type_support_t * type_supports,
   const char * service_name, const rmw_qos_profile_t * qos_policies)
 {
@@ -60,7 +61,8 @@ rmw_create_service(const rmw_node_t * node,
   }
 
   if (!service_name || strlen(service_name) == 0) {
-    RMW_SET_ERROR_MSG("publisher topic is null or empty string");
+    RMW_SET_ERROR_MSG("service topic is null or empty string");
+    return NULL;
   }
 
   if (!qos_policies) {

rmw_fastrtps_cpp/src/rmw_subscription.cpp

@@ -33,7 +33,8 @@
 extern "C"
 {
 rmw_subscription_t *
-rmw_create_subscription(const rmw_node_t * node,
+rmw_create_subscription(
+  const rmw_node_t * node,
   const rosidl_message_type_support_t * type_supports,
   const char * topic_name, const rmw_qos_profile_t * qos_policies, bool ignore_local_publications)
 {
@@ -48,7 +49,8 @@ rmw_create_subscription(const rmw_node_t * node,
   }
 
   if (!topic_name || strlen(topic_name) == 0) {
-    RMW_SET_ERROR_MSG("publisher topic is null or empty string");
+    RMW_SET_ERROR_MSG("subscription topic is null or empty string");
+    return NULL;
   }
 
   if (!qos_policies) {
@@ -84,6 +86,9 @@ rmw_create_subscription(const rmw_node_t * node,
   rmw_subscription_t * rmw_subscription = nullptr;
   SubscriberAttributes subscriberParam;
 
+  // Load default XML profile.
+  Domain::getDefaultSubscriberAttributes(subscriberParam);
+
   info = new CustomSubscriberInfo();
   info->typesupport_identifier_ = type_support->typesupport_identifier;