Support DDS XTypes creation

rmw_cyclonedds_cpp/src/rmw_node.cpp

@@ -1235,7 +1235,6 @@ static bool check_create_domain(dds_domainid_t did, rmw_discovery_options_t * di
         }
         config += "</Peers>";
       }
-
       /* NOTE: Empty configuration fragments are ignored, so it is safe to
         unconditionally append a comma. */
       config += "</Discovery></Domain></CycloneDDS>,";
@@ -2316,6 +2315,7 @@ static rmw_qos_policy_kind_t dds_qos_policy_to_rmw_qos_policy(dds_qos_policy_id_
     case DDS_LIFESPAN_QOS_POLICY_ID:
       return RMW_QOS_POLICY_LIFESPAN;
     default:
+      RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", "%d", policy_id);
       return RMW_QOS_POLICY_INVALID;
   }
 }
@@ -2478,10 +2478,16 @@ static CddsPublisher * create_cdds_publisher(
   std::string fqtopic_name = make_fqtopic(ROS_TOPIC_PREFIX, topic_name, "", qos_policies);
   bool is_fixed_type = is_type_self_contained(type_support);
   uint32_t sample_size = static_cast<uint32_t>(rmw_cyclonedds_cpp::get_message_size(type_support));
+
+  auto dstruct = create_msg_dds_dynamic_type(type_support->typesupport_identifier, type_support->data, dds_ppant);
   auto sertype = create_sertype(
     type_support->typesupport_identifier,
-    create_message_type_support(type_support->data, type_support->typesupport_identifier), false,
-    rmw_cyclonedds_cpp::make_message_value_type(type_supports), sample_size, is_fixed_type);
+    create_message_type_support(type_support->data, type_support->typesupport_identifier), 
+    false,
+    rmw_cyclonedds_cpp::make_message_value_type(type_supports), 
+    sample_size, is_fixed_type,
+    dstruct, 
+    dds_ppant);
   struct ddsi_sertype * stact = nullptr;
   topic = create_topic(dds_ppant, fqtopic_name.c_str(), sertype, &stact);
 
@@ -2506,6 +2512,7 @@ static CddsPublisher * create_cdds_publisher(
     RMW_SET_ERROR_MSG("failed to get instance handle for writer");
     goto fail_instance_handle;
   }
+
   get_entity_gid(pub->enth, pub->gid);
   pub->sertype = stact;
   dds_delete_listener(listener);
@@ -2514,6 +2521,7 @@ static CddsPublisher * create_cdds_publisher(
   pub->sample_size = sample_size;
   dds_delete_qos(qos);
   dds_delete(topic);
+
   return pub;
 
 fail_instance_handle:
@@ -3011,10 +3019,16 @@ static CddsSubscription * create_cdds_subscription(
   std::string fqtopic_name = make_fqtopic(ROS_TOPIC_PREFIX, topic_name, "", qos_policies);
   bool is_fixed_type = is_type_self_contained(type_support);
   uint32_t sample_size = static_cast<uint32_t>(rmw_cyclonedds_cpp::get_message_size(type_support));
+
+  auto dstruct = create_msg_dds_dynamic_type(type_support->typesupport_identifier, type_support->data, dds_ppant);
   auto sertype = create_sertype(
     type_support->typesupport_identifier,
-    create_message_type_support(type_support->data, type_support->typesupport_identifier), false,
-    rmw_cyclonedds_cpp::make_message_value_type(type_supports), sample_size, is_fixed_type);
+    create_message_type_support(type_support->data, type_support->typesupport_identifier), 
+    false,
+    rmw_cyclonedds_cpp::make_message_value_type(type_supports), 
+    sample_size, is_fixed_type,
+    dstruct, 
+    dds_ppant);
   topic = create_topic(dds_ppant, fqtopic_name.c_str(), sertype);
 
   dds_listener_t * listener = dds_create_listener(&sub->user_callback_data);
@@ -3047,6 +3061,7 @@ static CddsSubscription * create_cdds_subscription(
   sub->is_loaning_available = is_fixed_type && dds_is_loan_available(sub->enth);
   dds_delete_qos(qos);
   dds_delete(topic);
+
   return sub;
 fail_readcond:
   if (dds_delete(sub->enth) < 0) {
@@ -4959,6 +4974,11 @@ static rmw_ret_t rmw_init_cs(
   dds_listener_t * listener = dds_create_listener(cb_data);
   dds_lset_data_available_arg(listener, dds_listener_callback, cb_data, false);
 
+  dds_dynamic_type_t res = create_res_dds_dynamic_type(type_support->typesupport_identifier, type_support->data, node->context->impl->ppant);
+  dds_dynamic_type_t req = create_req_dds_dynamic_type(type_support->typesupport_identifier, type_support->data, node->context->impl->ppant);
+
+  struct sertype_rmw * pub_st, * sub_st;
+
   if (is_service) {
     std::tie(sub_msg_ts, pub_msg_ts) =
       rmw_cyclonedds_cpp::make_request_response_value_types(type_supports);
@@ -4974,6 +4994,18 @@ static rmw_ret_t rmw_init_cs(
     subtopic_name =
       make_fqtopic(ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", qos_policies);
     pubtopic_name = make_fqtopic(ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", qos_policies);
+
+    pub_st = create_sertype(
+      type_support->typesupport_identifier, pub_type_support, true,
+      std::move(pub_msg_ts), 0U, false,
+      res, node->context->impl->ppant
+    );
+    sub_st = create_sertype(
+      type_support->typesupport_identifier, sub_type_support, true,
+      std::move(sub_msg_ts), 0U, false,
+      req, node->context->impl->ppant
+    );
+
   } else {
     std::tie(pub_msg_ts, sub_msg_ts) =
       rmw_cyclonedds_cpp::make_request_response_value_types(type_supports);
@@ -4989,6 +5021,18 @@ static rmw_ret_t rmw_init_cs(
     pubtopic_name =
       make_fqtopic(ROS_SERVICE_REQUESTER_PREFIX, service_name, "Request", qos_policies);
     subtopic_name = make_fqtopic(ROS_SERVICE_RESPONSE_PREFIX, service_name, "Reply", qos_policies);
+
+    pub_st = create_sertype(
+      type_support->typesupport_identifier, pub_type_support, true,
+      std::move(pub_msg_ts), 0U, false,
+      req, node->context->impl->ppant
+    );
+    sub_st = create_sertype(
+      type_support->typesupport_identifier, sub_type_support, true,
+      std::move(sub_msg_ts), 0U, false,
+      res, node->context->impl->ppant
+    );
+
   }
 
   RCUTILS_LOG_DEBUG_NAMED(
@@ -4999,21 +5043,14 @@ static rmw_ret_t rmw_init_cs(
   RCUTILS_LOG_DEBUG_NAMED("rmw_cyclonedds_cpp", "***********");
 
   dds_entity_t pubtopic, subtopic;
-  struct sertype_rmw * pub_st, * sub_st;
-
-  pub_st = create_sertype(
-    type_support->typesupport_identifier, pub_type_support, true,
-    std::move(pub_msg_ts));
+
   struct ddsi_sertype * pub_stact;
   pubtopic = create_topic(node->context->impl->ppant, pubtopic_name.c_str(), pub_st, &pub_stact);
   if (pubtopic < 0) {
     set_error_message_from_create_topic(pubtopic, pubtopic_name);
     goto fail_pubtopic;
   }
-
-  sub_st = create_sertype(
-    type_support->typesupport_identifier, sub_type_support, true,
-    std::move(sub_msg_ts));
+
   subtopic = create_topic(node->context->impl->ppant, subtopic_name.c_str(), sub_st);
   if (subtopic < 0) {
     set_error_message_from_create_topic(subtopic, subtopic_name);
@@ -5060,8 +5097,8 @@ static rmw_ret_t rmw_init_cs(
   dds_delete_listener(listener);
   dds_delete_qos(pub_qos);
   dds_delete_qos(sub_qos);
-  dds_delete(subtopic);
   dds_delete(pubtopic);
+  dds_delete(subtopic);
 
   cs->pub = std::move(pub);
   cs->sub = std::move(sub);