From edc34653dc55631943439a69aec895f704419a44 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 14 Feb 2023 23:58:41 -0800 Subject: [PATCH 01/15] WIP type_version_hash just fix compile for API change Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index 29e7c53a0..8da7d9187 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -942,6 +942,7 @@ static void handle_builtintopic_endpoint( gid, std::string(s->topic_name), std::string(s->type_name), + nullptr, ppgid, qos_profile, is_reader); From 7895b0c4cf7176908367e001ff20020a92eb9e15 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 20 Feb 2023 11:48:29 -0800 Subject: [PATCH 02/15] Starting to transfer type hash values, but need to interleave keyval with service ids in user_data Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 78 +++++++++++++++++++++++++---- 1 file changed, 67 insertions(+), 11 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index 8da7d9187..87fa21258 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -925,10 +925,13 @@ static void handle_builtintopic_endpoint( dds_entity_t reader, rmw_context_impl_t * impl, bool is_reader) { + uint8_t type_hash[RCUTILS_SHA256_BLOCK_SIZE]; dds_sample_info_t si; void * raw = NULL; while (dds_take(reader, &raw, &si, 1, 1) == 1) { auto s = static_cast(raw); + RCUTILS_LOG_ERROR("Discovery: %s --- %p", + s->topic_name, type_hash); rmw_gid_t gid; convert_guid_to_gid(s->key, gid); if (si.instance_state != DDS_ALIVE_INSTANCE_STATE) { @@ -938,11 +941,18 @@ static void handle_builtintopic_endpoint( rmw_gid_t ppgid; dds_qos_to_rmw_qos(s->qos, &qos_profile); convert_guid_to_gid(s->participant_key, ppgid); + + if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data_qos( + s->qos->user_data.value, s->qos->user_data.length, type_hash)) + { + throw std::runtime_error("TODO(emersonknapp) handle null typehash"); + } + impl->common.graph_cache.add_entity( gid, std::string(s->topic_name), std::string(s->type_name), - nullptr, + type_hash, ppgid, qos_profile, is_reader); @@ -2054,8 +2064,31 @@ static rmw_time_t dds_duration_to_rmw(dds_duration_t duration) } } +static const uint8_t * get_type_hash( + const rosidl_message_type_support_t * type_support +) +{ + RCUTILS_LOG_WARN("Get type hash"); + if (type_support->typesupport_identifier == rosidl_typesupport_introspection_c__identifier) { + RCUTILS_LOG_WARN("It's the C"); + auto members = static_cast< + const rosidl_typesupport_introspection_c__MessageMembers *>(type_support->data); + return members->type_hash_; + } else if (type_support->typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier) { + RCUTILS_LOG_WARN("It's the C++"); + auto members = + static_cast(type_support->data); + return members->type_hash_; + } else { + RCUTILS_LOG_WARN("Unknown!!"); + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; + } +} + static dds_qos_t * create_readwrite_qos( const rmw_qos_profile_t * qos_policies, + const uint8_t * type_hash, bool ignore_local_publications) { dds_duration_t ldur; @@ -2137,7 +2170,7 @@ static dds_qos_t * create_readwrite_qos( case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: dds_qset_liveliness(qos, DDS_LIVELINESS_AUTOMATIC, ldur); break; - case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: + case LINESS_MANUAL_BY_TOPIC: dds_qset_liveliness(qos, DDS_LIVELINESS_MANUAL_BY_TOPIC, ldur); break; case RMW_QOS_POLICY_LIVELINESS_UNKNOWN: @@ -2148,6 +2181,11 @@ static dds_qos_t * create_readwrite_qos( if (ignore_local_publications) { dds_qset_ignorelocal(qos, DDS_IGNORELOCAL_PARTICIPANT); } + + std::vector user_data; + rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, user_data); + dds_qset_userdata(qos, user_data.data(), user_data.size()); + return qos; } @@ -2325,6 +2363,7 @@ static CddsPublisher * create_cdds_publisher( CddsPublisher * pub = new CddsPublisher(); dds_entity_t topic; dds_qos_t * qos; + const uint8_t * type_hash; std::string fqtopic_name = make_fqtopic(ROS_TOPIC_PREFIX, topic_name, "", qos_policies); bool is_fixed_type = is_type_self_contained(type_support); @@ -2344,7 +2383,10 @@ static CddsPublisher * create_cdds_publisher( set_error_message_from_create_topic(topic, fqtopic_name); goto fail_topic; } - if ((qos = create_readwrite_qos(qos_policies, false)) == nullptr) { + + type_hash = get_type_hash(type_support); + RCUTILS_LOG_WARN("Type hash addr: %p", type_hash); + if ((qos = create_readwrite_qos(qos_policies, type_hash, false)) == nullptr) { goto fail_qos; } if ((pub->enth = dds_create_writer(dds_pub, topic, qos, listener)) < 0) { @@ -2860,6 +2902,8 @@ 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(rmw_cyclonedds_cpp::get_message_size(type_support)); + const uint8_t * type_hash; + auto sertype = create_sertype( type_support->typesupport_identifier, create_message_type_support(type_support->data, type_support->typesupport_identifier), false, @@ -2876,7 +2920,11 @@ static CddsSubscription * create_cdds_subscription( set_error_message_from_create_topic(topic, fqtopic_name); goto fail_topic; } - if ((qos = create_readwrite_qos(qos_policies, ignore_local_publications)) == nullptr) { + type_hash = get_type_hash(type_support); + RCUTILS_LOG_WARN("Type hash addr: %p", type_hash); + if ((qos = create_readwrite_qos( + qos_policies, type_hash, ignore_local_publications)) == nullptr) + { goto fail_qos; } if ((sub->enth = dds_create_reader(dds_sub, topic, qos, listener)) < 0) { @@ -4800,9 +4848,15 @@ static rmw_ret_t rmw_init_cs( goto fail_subtopic; } // before proceeding to outright ignore given QoS policies, sanity check them - dds_qos_t * qos; - if ((qos = create_readwrite_qos(qos_policies, false)) == nullptr) { - goto fail_qos; + dds_qos_t * pub_qos; + dds_qos_t * sub_qos; + const uint8_t * pub_type_hash = get_type_hash(pub_type_support); + const uint8_t * sub_type_hash = get_type_hash(sub_type_support); + if ((pub_qos = create_readwrite_qos(qos_policies, pub_type_hash, false)) == nullptr) { + goto fail_pub_qos; + } + if ((sub_qos = create_readwrite_qos(qos_policies, sub_type_hash, false)) == nullptr) { + goto fail_sub_qos; } // store a unique identifier for this client/service in the user @@ -4814,13 +4868,13 @@ static rmw_ret_t rmw_init_cs( cs->id) + std::string(";"); dds_qset_userdata(qos, user_data.c_str(), user_data.size()); } - if ((pub->enth = dds_create_writer(node->context->impl->dds_pub, pubtopic, qos, nullptr)) < 0) { + if ((pub->enth = dds_create_writer(node->context->impl->dds_pub, pubtopic, pub_qos, nullptr)) < 0) { RMW_SET_ERROR_MSG("failed to create writer"); goto fail_writer; } get_entity_gid(pub->enth, pub->gid); pub->sertype = pub_stact; - if ((sub->enth = dds_create_reader(node->context->impl->dds_sub, subtopic, qos, listener)) < 0) { + if ((sub->enth = dds_create_reader(node->context->impl->dds_sub, subtopic, sub_qos, listener)) < 0) { RMW_SET_ERROR_MSG("failed to create reader"); goto fail_reader; } @@ -4849,8 +4903,10 @@ static rmw_ret_t rmw_init_cs( fail_reader: dds_delete(pub->enth); fail_writer: - dds_delete_qos(qos); -fail_qos: + dds_delete_qos(sub_qos); +fail_sub_qos: + dds_delete_qos(pub_qos); +fail_pub_qos: dds_delete(subtopic); fail_subtopic: dds_delete(pubtopic); From 70cffca31811006e8ca736ef9fdc28b8c80447e8 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 20 Feb 2023 16:30:00 -0800 Subject: [PATCH 03/15] WIP user-data type hash passing Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 75 ++++++++++++++++++++--------- 1 file changed, 51 insertions(+), 24 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index 87fa21258..bf44e1e41 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -77,6 +77,12 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + +#include "rosidl_typesupport_introspection_c/service_introspection.h" +#include "rosidl_typesupport_introspection_c/message_introspection.h" + #include "tracetools/tracetools.h" #include "namespace_prefix.hpp" @@ -926,12 +932,14 @@ static void handle_builtintopic_endpoint( bool is_reader) { uint8_t type_hash[RCUTILS_SHA256_BLOCK_SIZE]; + memset(type_hash, 0x2c, RCUTILS_SHA256_BLOCK_SIZE); + dds_sample_info_t si; void * raw = NULL; while (dds_take(reader, &raw, &si, 1, 1) == 1) { auto s = static_cast(raw); - RCUTILS_LOG_ERROR("Discovery: %s --- %p", - s->topic_name, type_hash); + std::string discovered_user_data(s->qos->user_data.value, s->qos->user_data.value + s->qos->user_data.length); + RCUTILS_LOG_ERROR("Discovery: %s", s->topic_name); rmw_gid_t gid; convert_guid_to_gid(s->key, gid); if (si.instance_state != DDS_ALIVE_INSTANCE_STATE) { @@ -2086,10 +2094,18 @@ static const uint8_t * get_type_hash( } } +static const uint8_t * get_service_type_hash( + const rosidl_service_type_support_t * type_support +) +{ + return nullptr; +} + static dds_qos_t * create_readwrite_qos( const rmw_qos_profile_t * qos_policies, const uint8_t * type_hash, - bool ignore_local_publications) + bool ignore_local_publications, + const std::string & extra_user_data) { dds_duration_t ldur; dds_qos_t * qos = dds_create_qos(); @@ -2170,7 +2186,7 @@ static dds_qos_t * create_readwrite_qos( case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: dds_qset_liveliness(qos, DDS_LIVELINESS_AUTOMATIC, ldur); break; - case LINESS_MANUAL_BY_TOPIC: + case DDS_LIVELINESS_MANUAL_BY_TOPIC: dds_qset_liveliness(qos, DDS_LIVELINESS_MANUAL_BY_TOPIC, ldur); break; case RMW_QOS_POLICY_LIVELINESS_UNKNOWN: @@ -2182,8 +2198,9 @@ static dds_qos_t * create_readwrite_qos( dds_qset_ignorelocal(qos, DDS_IGNORELOCAL_PARTICIPANT); } - std::vector user_data; - rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, user_data); + std::string user_data = + extra_user_data + + rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash); dds_qset_userdata(qos, user_data.data(), user_data.size()); return qos; @@ -2386,7 +2403,7 @@ static CddsPublisher * create_cdds_publisher( type_hash = get_type_hash(type_support); RCUTILS_LOG_WARN("Type hash addr: %p", type_hash); - if ((qos = create_readwrite_qos(qos_policies, type_hash, false)) == nullptr) { + if ((qos = create_readwrite_qos(qos_policies, type_hash, false, "")) == nullptr) { goto fail_qos; } if ((pub->enth = dds_create_writer(dds_pub, topic, qos, listener)) < 0) { @@ -2923,7 +2940,7 @@ static CddsSubscription * create_cdds_subscription( type_hash = get_type_hash(type_support); RCUTILS_LOG_WARN("Type hash addr: %p", type_hash); if ((qos = create_readwrite_qos( - qos_policies, type_hash, ignore_local_publications)) == nullptr) + qos_policies, type_hash, ignore_local_publications, "")) == nullptr) { goto fail_qos; } @@ -4789,12 +4806,23 @@ static rmw_ret_t rmw_init_cs( auto sub = std::make_unique(); std::string subtopic_name, pubtopic_name; void * pub_type_support, * sub_type_support; + dds_qos_t * pub_qos, * sub_qos; + uint8_t pub_type_hash[32]; + uint8_t sub_type_hash[32]; + std::string user_data; std::unique_ptr pub_msg_ts, sub_msg_ts; dds_listener_t * listener = dds_create_listener(cb_data); dds_lset_data_available_arg(listener, dds_listener_callback, cb_data, false); + // Find + // auto service_members = static_cast(type_support->data); + // auto request_members = static_cast( + // service_members->request_members_->data); + // auto response_members = static_cast( + // service_members->response_members_->data); + if (is_service) { std::tie(sub_msg_ts, pub_msg_ts) = rmw_cyclonedds_cpp::make_request_response_value_types(type_supports); @@ -4847,27 +4875,25 @@ static rmw_ret_t rmw_init_cs( set_error_message_from_create_topic(subtopic, subtopic_name); goto fail_subtopic; } - // before proceeding to outright ignore given QoS policies, sanity check them - dds_qos_t * pub_qos; - dds_qos_t * sub_qos; - const uint8_t * pub_type_hash = get_type_hash(pub_type_support); - const uint8_t * sub_type_hash = get_type_hash(sub_type_support); - if ((pub_qos = create_readwrite_qos(qos_policies, pub_type_hash, false)) == nullptr) { - goto fail_pub_qos; - } - if ((sub_qos = create_readwrite_qos(qos_policies, sub_type_hash, false)) == nullptr) { - goto fail_sub_qos; - } // store a unique identifier for this client/service in the user // data of the reader and writer so that we can always determine // which pairs belong together get_unique_csid(node, cs->id); - { - std::string user_data = std::string(is_service ? "serviceid=" : "clientid=") + csid_to_string( - cs->id) + std::string(";"); - dds_qset_userdata(qos, user_data.c_str(), user_data.size()); + user_data = std::string(is_service ? "serviceid=" : "clientid=") + csid_to_string( + cs->id) + std::string(";"); + + // before proceeding to outright ignore given QoS policies, sanity check them + memset(pub_type_hash, 0x1a, 32); + memset(sub_type_hash, 0x3c, 32); + + if ((pub_qos = create_readwrite_qos(qos_policies, pub_type_hash, false, user_data)) == nullptr) { + goto fail_pub_qos; } + if ((sub_qos = create_readwrite_qos(qos_policies, sub_type_hash, false, user_data)) == nullptr) { + goto fail_sub_qos; + } + if ((pub->enth = dds_create_writer(node->context->impl->dds_pub, pubtopic, pub_qos, nullptr)) < 0) { RMW_SET_ERROR_MSG("failed to create writer"); goto fail_writer; @@ -4888,7 +4914,8 @@ static rmw_ret_t rmw_init_cs( goto fail_instance_handle; } dds_delete_listener(listener); - dds_delete_qos(qos); + dds_delete_qos(pub_qos); + dds_delete_qos(sub_qos); dds_delete(subtopic); dds_delete(pubtopic); From ce2a99cbff6ea5c7e963c450caebd74e8f05e626 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 20 Feb 2023 19:14:23 -0800 Subject: [PATCH 04/15] Fetching request and response type hashes Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 54 ++++++++++++++++++----------- 1 file changed, 33 insertions(+), 21 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index bf44e1e41..ab0833eff 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -932,7 +932,6 @@ static void handle_builtintopic_endpoint( bool is_reader) { uint8_t type_hash[RCUTILS_SHA256_BLOCK_SIZE]; - memset(type_hash, 0x2c, RCUTILS_SHA256_BLOCK_SIZE); dds_sample_info_t si; void * raw = NULL; @@ -2076,28 +2075,51 @@ static const uint8_t * get_type_hash( const rosidl_message_type_support_t * type_support ) { - RCUTILS_LOG_WARN("Get type hash"); if (type_support->typesupport_identifier == rosidl_typesupport_introspection_c__identifier) { - RCUTILS_LOG_WARN("It's the C"); auto members = static_cast< const rosidl_typesupport_introspection_c__MessageMembers *>(type_support->data); return members->type_hash_; } else if (type_support->typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier) { - RCUTILS_LOG_WARN("It's the C++"); auto members = static_cast(type_support->data); return members->type_hash_; } else { - RCUTILS_LOG_WARN("Unknown!!"); RMW_SET_ERROR_MSG("Unknown typesupport identifier"); return nullptr; } } -static const uint8_t * get_service_type_hash( +static const uint8_t * get_service_request_type_hash( const rosidl_service_type_support_t * type_support ) { + if (type_support->typesupport_identifier == rosidl_typesupport_introspection_c__identifier) { + auto members = static_cast< + const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); + return members->request_members_->type_hash_; + } else if (type_support->typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier) { + auto members = static_cast< + const rosidl_typesupport_introspection_cpp::ServiceMembers *>(type_support->data); + return members->request_members_->type_hash_; + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; +} + +static const uint8_t * get_service_response_type_hash( + const rosidl_service_type_support_t * type_support +) +{ + if (type_support->typesupport_identifier == rosidl_typesupport_introspection_c__identifier) { + auto members = + static_cast(type_support->data); + return members->response_members_->type_hash_; + } else if (type_support->typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier) { + auto members = + static_cast(type_support->data); + return members->response_members_->type_hash_; + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); return nullptr; } @@ -2402,7 +2424,6 @@ static CddsPublisher * create_cdds_publisher( } type_hash = get_type_hash(type_support); - RCUTILS_LOG_WARN("Type hash addr: %p", type_hash); if ((qos = create_readwrite_qos(qos_policies, type_hash, false, "")) == nullptr) { goto fail_qos; } @@ -2938,7 +2959,6 @@ static CddsSubscription * create_cdds_subscription( goto fail_topic; } type_hash = get_type_hash(type_support); - RCUTILS_LOG_WARN("Type hash addr: %p", type_hash); if ((qos = create_readwrite_qos( qos_policies, type_hash, ignore_local_publications, "")) == nullptr) { @@ -4807,8 +4827,7 @@ static rmw_ret_t rmw_init_cs( std::string subtopic_name, pubtopic_name; void * pub_type_support, * sub_type_support; dds_qos_t * pub_qos, * sub_qos; - uint8_t pub_type_hash[32]; - uint8_t sub_type_hash[32]; + const uint8_t * pub_type_hash, * sub_type_hash; std::string user_data; std::unique_ptr pub_msg_ts, sub_msg_ts; @@ -4816,21 +4835,16 @@ 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); - // Find - // auto service_members = static_cast(type_support->data); - // auto request_members = static_cast( - // service_members->request_members_->data); - // auto response_members = static_cast( - // service_members->response_members_->data); - if (is_service) { std::tie(sub_msg_ts, pub_msg_ts) = rmw_cyclonedds_cpp::make_request_response_value_types(type_supports); sub_type_support = create_request_type_support( type_support->data, type_support->typesupport_identifier); + sub_type_hash = get_service_request_type_hash(type_support); pub_type_support = create_response_type_support( type_support->data, type_support->typesupport_identifier); + pub_type_hash = get_service_response_type_hash(type_support); 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); @@ -4840,8 +4854,10 @@ static rmw_ret_t rmw_init_cs( pub_type_support = create_request_type_support( type_support->data, type_support->typesupport_identifier); + pub_type_hash = get_service_request_type_hash(type_support); sub_type_support = create_response_type_support( type_support->data, type_support->typesupport_identifier); + sub_type_hash = get_service_response_type_hash(type_support); 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); @@ -4883,10 +4899,6 @@ static rmw_ret_t rmw_init_cs( user_data = std::string(is_service ? "serviceid=" : "clientid=") + csid_to_string( cs->id) + std::string(";"); - // before proceeding to outright ignore given QoS policies, sanity check them - memset(pub_type_hash, 0x1a, 32); - memset(sub_type_hash, 0x3c, 32); - if ((pub_qos = create_readwrite_qos(qos_policies, pub_type_hash, false, user_data)) == nullptr) { goto fail_pub_qos; } From 216fcd95be8796790e1be209df20d7bbd8dd1a0a Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 21 Feb 2023 21:37:22 -0800 Subject: [PATCH 05/15] First pass type hash struct Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 42 +++++++++++++---------------- 1 file changed, 18 insertions(+), 24 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index ab0833eff..ee8f6126e 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -931,8 +931,6 @@ static void handle_builtintopic_endpoint( dds_entity_t reader, rmw_context_impl_t * impl, bool is_reader) { - uint8_t type_hash[RCUTILS_SHA256_BLOCK_SIZE]; - dds_sample_info_t si; void * raw = NULL; while (dds_take(reader, &raw, &si, 1, 1) == 1) { @@ -948,13 +946,8 @@ static void handle_builtintopic_endpoint( rmw_gid_t ppgid; dds_qos_to_rmw_qos(s->qos, &qos_profile); convert_guid_to_gid(s->participant_key, ppgid); - - if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data_qos( - s->qos->user_data.value, s->qos->user_data.length, type_hash)) - { - throw std::runtime_error("TODO(emersonknapp) handle null typehash"); - } - + auto type_hash = rmw_dds_common::parse_type_hash_from_user_data_qos( + s->qos->user_data.value, s->qos->user_data.length); impl->common.graph_cache.add_entity( gid, std::string(s->topic_name), @@ -2071,9 +2064,7 @@ static rmw_time_t dds_duration_to_rmw(dds_duration_t duration) } } -static const uint8_t * get_type_hash( - const rosidl_message_type_support_t * type_support -) +static rosidl_type_hash_t get_type_hash(const rosidl_message_type_support_t * type_support) { if (type_support->typesupport_identifier == rosidl_typesupport_introspection_c__identifier) { auto members = static_cast< @@ -2085,11 +2076,11 @@ static const uint8_t * get_type_hash( return members->type_hash_; } else { RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; + return rosidl_get_zero_initialized_type_hash(); } } -static const uint8_t * get_service_request_type_hash( +static rosidl_type_hash_t get_service_request_type_hash( const rosidl_service_type_support_t * type_support ) { @@ -2103,10 +2094,10 @@ static const uint8_t * get_service_request_type_hash( return members->request_members_->type_hash_; } RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; + return rosidl_get_zero_initialized_type_hash(); } -static const uint8_t * get_service_response_type_hash( +static rosidl_type_hash_t get_service_response_type_hash( const rosidl_service_type_support_t * type_support ) { @@ -2120,12 +2111,12 @@ static const uint8_t * get_service_response_type_hash( return members->response_members_->type_hash_; } RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return nullptr; + return rosidl_get_zero_initialized_type_hash(); } static dds_qos_t * create_readwrite_qos( const rmw_qos_profile_t * qos_policies, - const uint8_t * type_hash, + const rosidl_type_hash_t & type_hash, bool ignore_local_publications, const std::string & extra_user_data) { @@ -2220,9 +2211,12 @@ static dds_qos_t * create_readwrite_qos( dds_qset_ignorelocal(qos, DDS_IGNORELOCAL_PARTICIPANT); } - std::string user_data = - extra_user_data + - rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash); + auto allocator = rcutils_get_default_allocator(); + char * type_hash_c_str = nullptr; + if (RCUTILS_RET_OK != rosidl_stringify_type_hash(&type_hash, allocator, &type_hash_c_str)) { + return nullptr; + } + std::string user_data = extra_user_data + "typehash=" + std::string(type_hash_c_str) + ";"; dds_qset_userdata(qos, user_data.data(), user_data.size()); return qos; @@ -2402,7 +2396,7 @@ static CddsPublisher * create_cdds_publisher( CddsPublisher * pub = new CddsPublisher(); dds_entity_t topic; dds_qos_t * qos; - const uint8_t * type_hash; + rosidl_type_hash_t type_hash; std::string fqtopic_name = make_fqtopic(ROS_TOPIC_PREFIX, topic_name, "", qos_policies); bool is_fixed_type = is_type_self_contained(type_support); @@ -2936,11 +2930,11 @@ static CddsSubscription * create_cdds_subscription( CddsSubscription * sub = new CddsSubscription(); dds_entity_t topic; dds_qos_t * qos; + rosidl_type_hash_t type_hash; 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(rmw_cyclonedds_cpp::get_message_size(type_support)); - const uint8_t * type_hash; auto sertype = create_sertype( type_support->typesupport_identifier, @@ -4827,7 +4821,7 @@ static rmw_ret_t rmw_init_cs( std::string subtopic_name, pubtopic_name; void * pub_type_support, * sub_type_support; dds_qos_t * pub_qos, * sub_qos; - const uint8_t * pub_type_hash, * sub_type_hash; + rosidl_type_hash_t pub_type_hash, sub_type_hash; std::string user_data; std::unique_ptr pub_msg_ts, sub_msg_ts; From ec2f14a7ddafe4f85f8694900b705f485a373c62 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 21 Feb 2023 23:28:48 -0800 Subject: [PATCH 06/15] use common encoder fn Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index ee8f6126e..a498e88f5 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -2211,12 +2211,8 @@ static dds_qos_t * create_readwrite_qos( dds_qset_ignorelocal(qos, DDS_IGNORELOCAL_PARTICIPANT); } - auto allocator = rcutils_get_default_allocator(); - char * type_hash_c_str = nullptr; - if (RCUTILS_RET_OK != rosidl_stringify_type_hash(&type_hash, allocator, &type_hash_c_str)) { - return nullptr; - } - std::string user_data = extra_user_data + "typehash=" + std::string(type_hash_c_str) + ";"; + std::string user_data = extra_user_data + + rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash); dds_qset_userdata(qos, user_data.data(), user_data.size()); return qos; From 1c645d602b52c8bc81b94c430401c0ff5ebe1029 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 21 Feb 2023 23:47:27 -0800 Subject: [PATCH 07/15] Log type hash in discovery Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index a498e88f5..a1fae3efc 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -936,7 +936,7 @@ static void handle_builtintopic_endpoint( while (dds_take(reader, &raw, &si, 1, 1) == 1) { auto s = static_cast(raw); std::string discovered_user_data(s->qos->user_data.value, s->qos->user_data.value + s->qos->user_data.length); - RCUTILS_LOG_ERROR("Discovery: %s", s->topic_name); + RCUTILS_LOG_WARN("Discovery: %s\n -- %s", s->type_name, discovered_user_data.c_str()); rmw_gid_t gid; convert_guid_to_gid(s->key, gid); if (si.instance_state != DDS_ALIVE_INSTANCE_STATE) { From 46c8e219c2772b017519acdc50d0ef0b97decb86 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 23 Feb 2023 13:47:27 -0800 Subject: [PATCH 08/15] Fix incorrect qos usage Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index a1fae3efc..4e9a19602 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -935,8 +935,6 @@ static void handle_builtintopic_endpoint( void * raw = NULL; while (dds_take(reader, &raw, &si, 1, 1) == 1) { auto s = static_cast(raw); - std::string discovered_user_data(s->qos->user_data.value, s->qos->user_data.value + s->qos->user_data.length); - RCUTILS_LOG_WARN("Discovery: %s\n -- %s", s->type_name, discovered_user_data.c_str()); rmw_gid_t gid; convert_guid_to_gid(s->key, gid); if (si.instance_state != DDS_ALIVE_INSTANCE_STATE) { @@ -946,8 +944,12 @@ static void handle_builtintopic_endpoint( rmw_gid_t ppgid; dds_qos_to_rmw_qos(s->qos, &qos_profile); convert_guid_to_gid(s->participant_key, ppgid); - auto type_hash = rmw_dds_common::parse_type_hash_from_user_data_qos( - s->qos->user_data.value, s->qos->user_data.length); + std::string type_hash_str; + rosidl_type_hash_t type_hash = rosidl_get_zero_initialized_type_hash(); + if (get_user_data_key(s->qos, "typehash", type_hash_str)) { + rosidl_parse_type_hash_string( + type_hash_str.c_str(), &type_hash); + } impl->common.graph_cache.add_entity( gid, std::string(s->topic_name), @@ -2199,7 +2201,7 @@ static dds_qos_t * create_readwrite_qos( case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: dds_qset_liveliness(qos, DDS_LIVELINESS_AUTOMATIC, ldur); break; - case DDS_LIVELINESS_MANUAL_BY_TOPIC: + case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: dds_qset_liveliness(qos, DDS_LIVELINESS_MANUAL_BY_TOPIC, ldur); break; case RMW_QOS_POLICY_LIVELINESS_UNKNOWN: @@ -2931,7 +2933,6 @@ 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(rmw_cyclonedds_cpp::get_message_size(type_support)); - auto sertype = create_sertype( type_support->typesupport_identifier, create_message_type_support(type_support->data, type_support->typesupport_identifier), false, From 773d192040e9a3f7b967dfb0744d30a15aa1b2a9 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Mon, 13 Mar 2023 00:05:49 -0700 Subject: [PATCH 09/15] Fix linter Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index 4e9a19602..a13b735e7 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -2068,11 +2068,12 @@ static rmw_time_t dds_duration_to_rmw(dds_duration_t duration) static rosidl_type_hash_t get_type_hash(const rosidl_message_type_support_t * type_support) { - if (type_support->typesupport_identifier == rosidl_typesupport_introspection_c__identifier) { + const auto * id = type_support->typesupport_identifier; + if (id == rosidl_typesupport_introspection_c__identifier) { auto members = static_cast< const rosidl_typesupport_introspection_c__MessageMembers *>(type_support->data); return members->type_hash_; - } else if (type_support->typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier) { + } else if (id == rosidl_typesupport_introspection_cpp::typesupport_identifier) { auto members = static_cast(type_support->data); return members->type_hash_; @@ -2086,11 +2087,12 @@ static rosidl_type_hash_t get_service_request_type_hash( const rosidl_service_type_support_t * type_support ) { - if (type_support->typesupport_identifier == rosidl_typesupport_introspection_c__identifier) { + const auto * id = type_support->typesupport_identifier; + if (id == rosidl_typesupport_introspection_c__identifier) { auto members = static_cast< const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); return members->request_members_->type_hash_; - } else if (type_support->typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier) { + } else if (id == rosidl_typesupport_introspection_cpp::typesupport_identifier) { auto members = static_cast< const rosidl_typesupport_introspection_cpp::ServiceMembers *>(type_support->data); return members->request_members_->type_hash_; @@ -2103,11 +2105,12 @@ static rosidl_type_hash_t get_service_response_type_hash( const rosidl_service_type_support_t * type_support ) { - if (type_support->typesupport_identifier == rosidl_typesupport_introspection_c__identifier) { + const auto * id = type_support->typesupport_identifier; + if (id == rosidl_typesupport_introspection_c__identifier) { auto members = static_cast(type_support->data); return members->response_members_->type_hash_; - } else if (type_support->typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier) { + } else if (id == rosidl_typesupport_introspection_cpp::typesupport_identifier) { auto members = static_cast(type_support->data); return members->response_members_->type_hash_; @@ -2951,7 +2954,7 @@ static CddsSubscription * create_cdds_subscription( } type_hash = get_type_hash(type_support); if ((qos = create_readwrite_qos( - qos_policies, type_hash, ignore_local_publications, "")) == nullptr) + qos_policies, type_hash, ignore_local_publications, "")) == nullptr) { goto fail_qos; } @@ -4897,13 +4900,17 @@ static rmw_ret_t rmw_init_cs( goto fail_sub_qos; } - if ((pub->enth = dds_create_writer(node->context->impl->dds_pub, pubtopic, pub_qos, nullptr)) < 0) { + if ((pub->enth = + dds_create_writer(node->context->impl->dds_pub, pubtopic, pub_qos, nullptr)) < 0) + { RMW_SET_ERROR_MSG("failed to create writer"); goto fail_writer; } get_entity_gid(pub->enth, pub->gid); pub->sertype = pub_stact; - if ((sub->enth = dds_create_reader(node->context->impl->dds_sub, subtopic, sub_qos, listener)) < 0) { + if ((sub->enth = + dds_create_reader(node->context->impl->dds_sub, subtopic, sub_qos, listener)) < 0) + { RMW_SET_ERROR_MSG("failed to create reader"); goto fail_reader; } From 90ff03eaddb0d373c72fef4839ab8d186acd1651 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 15 Mar 2023 12:07:25 -0700 Subject: [PATCH 10/15] Use new fn signature Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index a13b735e7..eb53c5569 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -2216,8 +2216,12 @@ static dds_qos_t * create_readwrite_qos( dds_qset_ignorelocal(qos, DDS_IGNORELOCAL_PARTICIPANT); } - std::string user_data = extra_user_data + - rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash); + std::string typehash_user_data; + rmw_ret_t ret = rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, typehash_user_data); + if (ret != RMW_RET_OK) { + typehash_user_data = ""; + } + std::string user_data = extra_user_data + typehash_user_data; dds_qset_userdata(qos, user_data.data(), user_data.size()); return qos; From d9581c0f81211048718ded12c1e1157615a2f21c Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 15 Mar 2023 18:52:35 -0700 Subject: [PATCH 11/15] Address review comments Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index eb53c5569..7b94373f6 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -944,12 +944,18 @@ static void handle_builtintopic_endpoint( rmw_gid_t ppgid; dds_qos_to_rmw_qos(s->qos, &qos_profile); convert_guid_to_gid(s->participant_key, ppgid); - std::string type_hash_str; + rosidl_type_hash_t type_hash = rosidl_get_zero_initialized_type_hash(); - if (get_user_data_key(s->qos, "typehash", type_hash_str)) { - rosidl_parse_type_hash_string( - type_hash_str.c_str(), &type_hash); + void * userdata; + size_t userdata_size; + if (dds_qget_userdata(s->qos, &userdata, &userdata_size)) { + if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data( + reinterpret_cast(userdata), userdata_size, type_hash)) + { + type_hash = rosidl_get_zero_initialized_type_hash(); + } } + impl->common.graph_cache.add_entity( gid, std::string(s->topic_name), @@ -2068,7 +2074,7 @@ static rmw_time_t dds_duration_to_rmw(dds_duration_t duration) static rosidl_type_hash_t get_type_hash(const rosidl_message_type_support_t * type_support) { - const auto * id = type_support->typesupport_identifier; + const char * id = type_support->typesupport_identifier; if (id == rosidl_typesupport_introspection_c__identifier) { auto members = static_cast< const rosidl_typesupport_introspection_c__MessageMembers *>(type_support->data); @@ -2087,7 +2093,7 @@ static rosidl_type_hash_t get_service_request_type_hash( const rosidl_service_type_support_t * type_support ) { - const auto * id = type_support->typesupport_identifier; + const char * id = type_support->typesupport_identifier; if (id == rosidl_typesupport_introspection_c__identifier) { auto members = static_cast< const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); @@ -2105,7 +2111,7 @@ static rosidl_type_hash_t get_service_response_type_hash( const rosidl_service_type_support_t * type_support ) { - const auto * id = type_support->typesupport_identifier; + const char * id = type_support->typesupport_identifier; if (id == rosidl_typesupport_introspection_c__identifier) { auto members = static_cast(type_support->data); @@ -2401,7 +2407,7 @@ static CddsPublisher * create_cdds_publisher( CddsPublisher * pub = new CddsPublisher(); dds_entity_t topic; dds_qos_t * qos; - rosidl_type_hash_t type_hash; + rosidl_type_hash_t type_hash = get_type_hash(type_support); std::string fqtopic_name = make_fqtopic(ROS_TOPIC_PREFIX, topic_name, "", qos_policies); bool is_fixed_type = is_type_self_contained(type_support); @@ -2421,8 +2427,6 @@ static CddsPublisher * create_cdds_publisher( set_error_message_from_create_topic(topic, fqtopic_name); goto fail_topic; } - - type_hash = get_type_hash(type_support); if ((qos = create_readwrite_qos(qos_policies, type_hash, false, "")) == nullptr) { goto fail_qos; } @@ -2935,7 +2939,7 @@ static CddsSubscription * create_cdds_subscription( CddsSubscription * sub = new CddsSubscription(); dds_entity_t topic; dds_qos_t * qos; - rosidl_type_hash_t type_hash; + rosidl_type_hash_t type_hash = get_type_hash(type_support); std::string fqtopic_name = make_fqtopic(ROS_TOPIC_PREFIX, topic_name, "", qos_policies); bool is_fixed_type = is_type_self_contained(type_support); @@ -2946,6 +2950,7 @@ static CddsSubscription * create_cdds_subscription( rmw_cyclonedds_cpp::make_message_value_type(type_supports), sample_size, is_fixed_type); topic = create_topic(dds_ppant, fqtopic_name.c_str(), sertype); + dds_listener_t * listener = dds_create_listener(&sub->user_callback_data); // Set the callback to listen for new messages dds_lset_data_available_arg(listener, dds_listener_callback, &sub->user_callback_data, false); @@ -2956,7 +2961,6 @@ static CddsSubscription * create_cdds_subscription( set_error_message_from_create_topic(topic, fqtopic_name); goto fail_topic; } - type_hash = get_type_hash(type_support); if ((qos = create_readwrite_qos( qos_policies, type_hash, ignore_local_publications, "")) == nullptr) { From b14c4ddf19c89999f08d7231ae0972af4400d5c2 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Wed, 15 Mar 2023 19:02:04 -0700 Subject: [PATCH 12/15] Minor reformatting Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 29 ++++++++++++++--------------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index 7b94373f6..b826e7676 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -75,6 +75,8 @@ #include "rmw_dds_common/qos.hpp" #include "rmw_dds_common/security.hpp" +#include "rosidl_runtime_c/type_hash.h" + #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" @@ -2080,8 +2082,8 @@ static rosidl_type_hash_t get_type_hash(const rosidl_message_type_support_t * ty const rosidl_typesupport_introspection_c__MessageMembers *>(type_support->data); return members->type_hash_; } else if (id == rosidl_typesupport_introspection_cpp::typesupport_identifier) { - auto members = - static_cast(type_support->data); + auto members = static_cast< + const rosidl_typesupport_introspection_cpp::MessageMembers *>(type_support->data); return members->type_hash_; } else { RMW_SET_ERROR_MSG("Unknown typesupport identifier"); @@ -2090,8 +2092,7 @@ static rosidl_type_hash_t get_type_hash(const rosidl_message_type_support_t * ty } static rosidl_type_hash_t get_service_request_type_hash( - const rosidl_service_type_support_t * type_support -) + const rosidl_service_type_support_t * type_support) { const char * id = type_support->typesupport_identifier; if (id == rosidl_typesupport_introspection_c__identifier) { @@ -2108,17 +2109,16 @@ static rosidl_type_hash_t get_service_request_type_hash( } static rosidl_type_hash_t get_service_response_type_hash( - const rosidl_service_type_support_t * type_support -) + const rosidl_service_type_support_t * type_support) { const char * id = type_support->typesupport_identifier; if (id == rosidl_typesupport_introspection_c__identifier) { - auto members = - static_cast(type_support->data); + auto members = static_cast< + const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); return members->response_members_->type_hash_; } else if (id == rosidl_typesupport_introspection_cpp::typesupport_identifier) { - auto members = - static_cast(type_support->data); + auto members = static_cast< + const rosidl_typesupport_introspection_cpp::ServiceMembers *>(type_support->data); return members->response_members_->type_hash_; } RMW_SET_ERROR_MSG("Unknown typesupport identifier"); @@ -2222,12 +2222,11 @@ static dds_qos_t * create_readwrite_qos( dds_qset_ignorelocal(qos, DDS_IGNORELOCAL_PARTICIPANT); } - std::string typehash_user_data; - rmw_ret_t ret = rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, typehash_user_data); - if (ret != RMW_RET_OK) { - typehash_user_data = ""; + std::string typehash_str; + if (RMW_RET_OK != rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, typehash_str)) { + typehash_str.clear(); } - std::string user_data = extra_user_data + typehash_user_data; + std::string user_data = extra_user_data + typehash_str; dds_qset_userdata(qos, user_data.data(), user_data.size()); return qos; From f799d8920ed32937ec688a1ac87d4b04aefb6dfd Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Thu, 16 Mar 2023 18:34:09 -0700 Subject: [PATCH 13/15] Add warning on type hash codec failure Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index b826e7676..15da6e120 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -954,6 +954,11 @@ static void handle_builtintopic_endpoint( if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data( reinterpret_cast(userdata), userdata_size, type_hash)) { + RCUTILS_LOG_WARN_NAMED( + "rmw_cyclonedds_cpp", + "Failed to parse type hash for topic '%s' with type '%s' from USER_DATA '%*s'.", + s->topic_name, s->type_name, + static_cast(userdata_size), reinterpret_cast(userdata)); type_hash = rosidl_get_zero_initialized_type_hash(); } } @@ -2224,6 +2229,9 @@ static dds_qos_t * create_readwrite_qos( std::string typehash_str; if (RMW_RET_OK != rmw_dds_common::encode_type_hash_for_user_data_qos(type_hash, typehash_str)) { + RCUTILS_LOG_WARN_NAMED( + "rmw_cyclonedds_cpp", + "Failed to encode type hash for topic, will not distribute it in USER_DATA."); typehash_str.clear(); } std::string user_data = extra_user_data + typehash_str; From 0d823ec3ab601332783bd3fc3c073153306dac72 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 21 Mar 2023 23:17:31 -0700 Subject: [PATCH 14/15] Use new typesupport hashes Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 79 ++++------------------------- 1 file changed, 10 insertions(+), 69 deletions(-) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index 15da6e120..05f5d9df4 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -79,12 +79,6 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" -#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" - -#include "rosidl_typesupport_introspection_c/service_introspection.h" -#include "rosidl_typesupport_introspection_c/message_introspection.h" - #include "tracetools/tracetools.h" #include "namespace_prefix.hpp" @@ -2079,57 +2073,6 @@ static rmw_time_t dds_duration_to_rmw(dds_duration_t duration) } } -static rosidl_type_hash_t get_type_hash(const rosidl_message_type_support_t * type_support) -{ - const char * id = type_support->typesupport_identifier; - if (id == rosidl_typesupport_introspection_c__identifier) { - auto members = static_cast< - const rosidl_typesupport_introspection_c__MessageMembers *>(type_support->data); - return members->type_hash_; - } else if (id == rosidl_typesupport_introspection_cpp::typesupport_identifier) { - auto members = static_cast< - const rosidl_typesupport_introspection_cpp::MessageMembers *>(type_support->data); - return members->type_hash_; - } else { - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return rosidl_get_zero_initialized_type_hash(); - } -} - -static rosidl_type_hash_t get_service_request_type_hash( - const rosidl_service_type_support_t * type_support) -{ - const char * id = type_support->typesupport_identifier; - if (id == rosidl_typesupport_introspection_c__identifier) { - auto members = static_cast< - const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); - return members->request_members_->type_hash_; - } else if (id == rosidl_typesupport_introspection_cpp::typesupport_identifier) { - auto members = static_cast< - const rosidl_typesupport_introspection_cpp::ServiceMembers *>(type_support->data); - return members->request_members_->type_hash_; - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return rosidl_get_zero_initialized_type_hash(); -} - -static rosidl_type_hash_t get_service_response_type_hash( - const rosidl_service_type_support_t * type_support) -{ - const char * id = type_support->typesupport_identifier; - if (id == rosidl_typesupport_introspection_c__identifier) { - auto members = static_cast< - const rosidl_typesupport_introspection_c__ServiceMembers *>(type_support->data); - return members->response_members_->type_hash_; - } else if (id == rosidl_typesupport_introspection_cpp::typesupport_identifier) { - auto members = static_cast< - const rosidl_typesupport_introspection_cpp::ServiceMembers *>(type_support->data); - return members->response_members_->type_hash_; - } - RMW_SET_ERROR_MSG("Unknown typesupport identifier"); - return rosidl_get_zero_initialized_type_hash(); -} - static dds_qos_t * create_readwrite_qos( const rmw_qos_profile_t * qos_policies, const rosidl_type_hash_t & type_hash, @@ -2414,7 +2357,6 @@ static CddsPublisher * create_cdds_publisher( CddsPublisher * pub = new CddsPublisher(); dds_entity_t topic; dds_qos_t * qos; - rosidl_type_hash_t type_hash = get_type_hash(type_support); std::string fqtopic_name = make_fqtopic(ROS_TOPIC_PREFIX, topic_name, "", qos_policies); bool is_fixed_type = is_type_self_contained(type_support); @@ -2434,7 +2376,7 @@ static CddsPublisher * create_cdds_publisher( set_error_message_from_create_topic(topic, fqtopic_name); goto fail_topic; } - if ((qos = create_readwrite_qos(qos_policies, type_hash, false, "")) == nullptr) { + if ((qos = create_readwrite_qos(qos_policies, *type_support->type_hash, false, "")) == nullptr) { goto fail_qos; } if ((pub->enth = dds_create_writer(dds_pub, topic, qos, listener)) < 0) { @@ -2946,7 +2888,6 @@ static CddsSubscription * create_cdds_subscription( CddsSubscription * sub = new CddsSubscription(); dds_entity_t topic; dds_qos_t * qos; - rosidl_type_hash_t type_hash = get_type_hash(type_support); std::string fqtopic_name = make_fqtopic(ROS_TOPIC_PREFIX, topic_name, "", qos_policies); bool is_fixed_type = is_type_self_contained(type_support); @@ -2957,7 +2898,6 @@ static CddsSubscription * create_cdds_subscription( rmw_cyclonedds_cpp::make_message_value_type(type_supports), sample_size, is_fixed_type); topic = create_topic(dds_ppant, fqtopic_name.c_str(), sertype); - dds_listener_t * listener = dds_create_listener(&sub->user_callback_data); // Set the callback to listen for new messages dds_lset_data_available_arg(listener, dds_listener_callback, &sub->user_callback_data, false); @@ -2969,7 +2909,7 @@ static CddsSubscription * create_cdds_subscription( goto fail_topic; } if ((qos = create_readwrite_qos( - qos_policies, type_hash, ignore_local_publications, "")) == nullptr) + qos_policies, *type_support->type_hash, ignore_local_publications, "")) == nullptr) { goto fail_qos; } @@ -4836,7 +4776,8 @@ static rmw_ret_t rmw_init_cs( std::string subtopic_name, pubtopic_name; void * pub_type_support, * sub_type_support; dds_qos_t * pub_qos, * sub_qos; - rosidl_type_hash_t pub_type_hash, sub_type_hash; + const rosidl_type_hash_t * pub_type_hash; + const rosidl_type_hash_t * sub_type_hash; std::string user_data; std::unique_ptr pub_msg_ts, sub_msg_ts; @@ -4850,10 +4791,10 @@ static rmw_ret_t rmw_init_cs( sub_type_support = create_request_type_support( type_support->data, type_support->typesupport_identifier); - sub_type_hash = get_service_request_type_hash(type_support); + sub_type_hash = type_supports->request_typesupport->type_hash; pub_type_support = create_response_type_support( type_support->data, type_support->typesupport_identifier); - pub_type_hash = get_service_response_type_hash(type_support); + pub_type_hash = type_supports->response_typesupport->type_hash; 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); @@ -4863,10 +4804,10 @@ static rmw_ret_t rmw_init_cs( pub_type_support = create_request_type_support( type_support->data, type_support->typesupport_identifier); - pub_type_hash = get_service_request_type_hash(type_support); + pub_type_hash = type_supports->request_typesupport->type_hash; sub_type_support = create_response_type_support( type_support->data, type_support->typesupport_identifier); - sub_type_hash = get_service_response_type_hash(type_support); + sub_type_hash = type_supports->response_typesupport->type_hash; 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); @@ -4908,10 +4849,10 @@ static rmw_ret_t rmw_init_cs( user_data = std::string(is_service ? "serviceid=" : "clientid=") + csid_to_string( cs->id) + std::string(";"); - if ((pub_qos = create_readwrite_qos(qos_policies, pub_type_hash, false, user_data)) == nullptr) { + if ((pub_qos = create_readwrite_qos(qos_policies, *pub_type_hash, false, user_data)) == nullptr) { goto fail_pub_qos; } - if ((sub_qos = create_readwrite_qos(qos_policies, sub_type_hash, false, user_data)) == nullptr) { + if ((sub_qos = create_readwrite_qos(qos_policies, *sub_type_hash, false, user_data)) == nullptr) { goto fail_sub_qos; } From 10938d6f34f4162781359f3625c219b602cac2b5 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Fri, 24 Mar 2023 10:20:21 -0700 Subject: [PATCH 15/15] Free the user data Signed-off-by: Emerson Knapp --- rmw_cyclonedds_cpp/src/rmw_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index 05f5d9df4..eb5966732 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -945,6 +945,7 @@ static void handle_builtintopic_endpoint( void * userdata; size_t userdata_size; if (dds_qget_userdata(s->qos, &userdata, &userdata_size)) { + RCPPUTILS_SCOPE_EXIT(dds_free(userdata)); if (RMW_RET_OK != rmw_dds_common::parse_type_hash_from_user_data( reinterpret_cast(userdata), userdata_size, type_hash)) {