Type hash distribution in discovery (rep2011) (#671)

* Adding type hash to user data

Signed-off-by: Emerson Knapp <[email protected]>

rmw_fastrtps_cpp/src/publisher.cpp

@@ -256,7 +256,7 @@ rmw_fastrtps_cpp::create_publisher(
   }
 
   // Get QoS from RMW
-  if (!get_datawriter_qos(*qos_policies, writer_qos)) {
+  if (!get_datawriter_qos(*qos_policies, *type_supports->type_hash, writer_qos)) {
     RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS");
     return nullptr;
   }

rmw_fastrtps_cpp/src/rmw_client.cpp

@@ -314,7 +314,9 @@ rmw_create_client(
     reader_qos.data_sharing().off();
   }
 
-  if (!get_datareader_qos(adapted_qos_policies, reader_qos)) {
+  if (!get_datareader_qos(
+      adapted_qos_policies, *type_supports->response_typesupport->type_hash, reader_qos))
+  {
     RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS");
     return nullptr;
   }
@@ -368,7 +370,9 @@ rmw_create_client(
     writer_qos.data_sharing().off();
   }
 
-  if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) {
+  if (!get_datawriter_qos(
+      adapted_qos_policies, *type_supports->request_typesupport->type_hash, writer_qos))
+  {
     RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS");
     return nullptr;
   }

rmw_fastrtps_cpp/src/rmw_service.cpp

@@ -313,7 +313,9 @@ rmw_create_service(
     reader_qos.data_sharing().off();
   }
 
-  if (!get_datareader_qos(adapted_qos_policies, reader_qos)) {
+  if (!get_datareader_qos(
+      adapted_qos_policies, *type_supports->request_typesupport->type_hash, reader_qos))
+  {
     RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS");
     return nullptr;
   }
@@ -371,7 +373,9 @@ rmw_create_service(
     writer_qos.data_sharing().off();
   }
 
-  if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) {
+  if (!get_datawriter_qos(
+      adapted_qos_policies, *type_supports->response_typesupport->type_hash, writer_qos))
+  {
     RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS");
     return nullptr;
   }

rmw_fastrtps_cpp/src/subscription.cpp

@@ -280,7 +280,7 @@ create_subscription(
     reader_qos.data_sharing().off();
   }
 
-  if (!get_datareader_qos(*qos_policies, reader_qos)) {
+  if (!get_datareader_qos(*qos_policies, *type_supports->type_hash, reader_qos)) {
     RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS");
     return nullptr;
   }

rmw_fastrtps_dynamic_cpp/src/publisher.cpp

@@ -262,7 +262,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher(
   }
 
   // Get QoS from RMW
-  if (!get_datawriter_qos(*qos_policies, writer_qos)) {
+  if (!get_datawriter_qos(*qos_policies, *type_supports->type_hash, writer_qos)) {
     RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS");
     return nullptr;
   }

rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp

@@ -345,7 +345,9 @@ rmw_create_client(
     reader_qos.data_sharing().off();
   }
 
-  if (!get_datareader_qos(adapted_qos_policies, reader_qos)) {
+  if (!get_datareader_qos(
+      adapted_qos_policies, *type_supports->response_typesupport->type_hash, reader_qos))
+  {
     RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS");
     return nullptr;
   }
@@ -399,7 +401,9 @@ rmw_create_client(
     writer_qos.data_sharing().off();
   }
 
-  if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) {
+  if (!get_datawriter_qos(
+      adapted_qos_policies, *type_supports->request_typesupport->type_hash, writer_qos))
+  {
     RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS");
     return nullptr;
   }

rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp

@@ -344,7 +344,9 @@ rmw_create_service(
     reader_qos.data_sharing().off();
   }
 
-  if (!get_datareader_qos(adapted_qos_policies, reader_qos)) {
+  if (!get_datareader_qos(
+      adapted_qos_policies, *type_supports->request_typesupport->type_hash, reader_qos))
+  {
     RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS");
     return nullptr;
   }
@@ -402,7 +404,9 @@ rmw_create_service(
     writer_qos.data_sharing().off();
   }
 
-  if (!get_datawriter_qos(adapted_qos_policies, writer_qos)) {
+  if (!get_datawriter_qos(
+      adapted_qos_policies, *type_supports->response_typesupport->type_hash, writer_qos))
+  {
     RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS");
     return nullptr;
   }

rmw_fastrtps_dynamic_cpp/src/subscription.cpp

@@ -32,6 +32,8 @@
 #include "rmw/rmw.h"
 #include "rmw/validate_full_topic_name.h"
 
+#include "rosidl_runtime_c/type_hash.h"
+
 #include "rcpputils/scope_exit.hpp"
 
 #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
@@ -264,7 +266,7 @@ create_subscription(
     reader_qos.data_sharing().off();
   }
 
-  if (!get_datareader_qos(*qos_policies, reader_qos)) {
+  if (!get_datareader_qos(*qos_policies, *type_supports->type_hash, reader_qos)) {
     RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS");
     return nullptr;
   }

rmw_fastrtps_shared_cpp/CMakeLists.txt

@@ -39,6 +39,7 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(rcpputils REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(rmw_dds_common REQUIRED)
+find_package(rosidl_runtime_c REQUIRED)
 find_package(rosidl_typesupport_introspection_c REQUIRED)
 find_package(rosidl_typesupport_introspection_cpp REQUIRED)
 find_package(tracetools REQUIRED)

rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp

@@ -40,6 +40,7 @@
 #include "rmw/rmw.h"
 
 #include "rmw_dds_common/context.hpp"
+#include "rmw_dds_common/qos.hpp"
 
 #include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp"
 #include "rmw_fastrtps_shared_cpp/custom_event_info.hpp"
@@ -205,12 +206,28 @@ class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantList
         rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown;
         rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile);
 
+        const auto & userDataValue = proxyData.m_qos.m_userData.getValue();
+        rosidl_type_hash_t type_hash;
+        if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data(
+            userDataValue.data(), userDataValue.size(), type_hash))
+        {
+          RCUTILS_LOG_WARN_NAMED(
+            "rmw_fastrtps_shared_cpp",
+            "Failed to parse type hash for topic '%s' with type '%s' from USER_DATA '%*s'.",
+            proxyData.topicName().c_str(),
+            proxyData.typeName().c_str(),
+            static_cast<int>(userDataValue.size()),
+            reinterpret_cast<const char *>(userDataValue.data()));
+          type_hash = rosidl_get_zero_initialized_type_hash();
+        }
+
         context->graph_cache.add_entity(
           rmw_fastrtps_shared_cpp::create_rmw_gid(
             identifier_,
             proxyData.guid()),
           proxyData.topicName().to_string(),
           proxyData.typeName().to_string(),
+          type_hash,
           rmw_fastrtps_shared_cpp::create_rmw_gid(
             identifier_,
             iHandle2GUID(proxyData.RTPSParticipantKey())),

rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp

@@ -27,6 +27,8 @@
 
 #include "rmw_fastrtps_shared_cpp/visibility_control.h"
 
+#include "rosidl_runtime_c/type_hash.h"
+
 RMW_FASTRTPS_SHARED_CPP_PUBLIC
 bool
 is_valid_qos(const rmw_qos_profile_t & qos_policies);
@@ -35,12 +37,14 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC
 bool
 get_datareader_qos(
   const rmw_qos_profile_t & qos_policies,
+  const rosidl_type_hash_t & type_hash,
   eprosima::fastdds::dds::DataReaderQos & reader_qos);
 
 RMW_FASTRTPS_SHARED_CPP_PUBLIC
 bool
 get_datawriter_qos(
   const rmw_qos_profile_t & qos_policies,
+  const rosidl_type_hash_t & type_hash,
   eprosima::fastdds::dds::DataWriterQos & writer_qos);
 
 RMW_FASTRTPS_SHARED_CPP_PUBLIC

rmw_fastrtps_shared_cpp/package.xml

@@ -27,6 +27,7 @@
   <build_depend>rcutils</build_depend>
   <build_depend>rmw</build_depend>
   <build_depend>rmw_dds_common</build_depend>
+  <build_depend>rosidl_runtime_c</build_depend>
   <build_depend>rosidl_typesupport_introspection_c</build_depend>
   <build_depend>rosidl_typesupport_introspection_cpp</build_depend>
   <build_depend>tracetools</build_depend>

rmw_fastrtps_shared_cpp/src/qos.cpp

@@ -13,6 +13,9 @@
 // limitations under the License.
 
 #include <limits>
+#include <vector>
+
+#include "rcutils/logging_macros.h"
 
 #include "rmw_fastrtps_shared_cpp/qos.hpp"
 
@@ -22,6 +25,10 @@
 
 #include "rmw/error_handling.h"
 
+#include "rmw_dds_common/qos.hpp"
+
+#include "rosidl_runtime_c/type_hash.h"
+
 #include "time_utils.hpp"
 
 static
@@ -142,20 +149,45 @@ bool fill_entity_qos_from_profile(
   return true;
 }
 
+template<typename DDSEntityQos>
+bool
+fill_data_entity_qos_from_profile(
+  const rmw_qos_profile_t & qos_policies,
+  const rosidl_type_hash_t & type_hash,
+  DDSEntityQos & entity_qos)
+{
+  if (!fill_entity_qos_from_profile(qos_policies, entity_qos)) {
+    return false;
+  }
+  std::string user_data_str;
+  if (RMW_RET_OK != rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, user_data_str)) {
+    RCUTILS_LOG_WARN_NAMED(
+      "rmw_fastrtps_shared_cpp",
+      "Failed to encode type hash for topic, will not distribute it in USER_DATA.");
+    user_data_str.clear();
+  }
+  std::vector<uint8_t> user_data(user_data_str.begin(), user_data_str.end());
+  entity_qos.user_data().resize(user_data.size());
+  entity_qos.user_data().setValue(user_data);
+  return true;
+}
+
 bool
 get_datareader_qos(
   const rmw_qos_profile_t & qos_policies,
+  const rosidl_type_hash_t & type_hash,
   eprosima::fastdds::dds::DataReaderQos & datareader_qos)
 {
-  return fill_entity_qos_from_profile(qos_policies, datareader_qos);
+  return fill_data_entity_qos_from_profile(qos_policies, type_hash, datareader_qos);
 }
 
 bool
 get_datawriter_qos(
   const rmw_qos_profile_t & qos_policies,
+  const rosidl_type_hash_t & type_hash,
   eprosima::fastdds::dds::DataWriterQos & datawriter_qos)
 {
-  return fill_entity_qos_from_profile(qos_policies, datawriter_qos);
+  return fill_data_entity_qos_from_profile(qos_policies, type_hash, datawriter_qos);
 }
 
 bool