diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 9e0d7c1e..64b9c03c 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -45,6 +45,8 @@ #include "rcutils/strdup.h" #include "rcutils/types.h" +#include "rosidl_runtime_c/type_hash.h" + #include "rmw/allocators.h" #include "rmw/dynamic_message_type_support.h" #include "rmw/error_handling.h" @@ -83,7 +85,8 @@ namespace z_owned_keyexpr_t ros_topic_name_to_zenoh_key( const std::size_t domain_id, const char * const topic_name, - const char * const topic_type) + const char * const topic_type, + const char * const type_hash) { auto strip_slashes = [](const char * const str) -> std::string @@ -103,7 +106,8 @@ z_owned_keyexpr_t ros_topic_name_to_zenoh_key( const std::string keyexpr_str = std::to_string(domain_id) + "/" + strip_slashes(topic_name) + "/" + - std::string(topic_type); + std::string(topic_type) + "/" + + std::string(type_hash); return z_keyexpr_new(keyexpr_str.c_str()); } @@ -588,10 +592,22 @@ rmw_create_publisher( allocator->deallocate(const_cast(rmw_publisher->topic_name), allocator->state); }); + const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); + char * type_hash_c_str = nullptr; + rcutils_ret_t stringify_ret = rosidl_stringify_type_hash( + type_hash, + *allocator, + &type_hash_c_str); + if (RCUTILS_RET_BAD_ALLOC == stringify_ret) { + RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str."); + return nullptr; + } + z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key( node->context->actual_domain_id, topic_name, - publisher_data->type_support->get_name()); + publisher_data->type_support->get_name(), + type_hash_c_str); auto always_free_ros_keyexpr = rcpputils::make_scope_exit( [&keyexpr]() { z_keyexpr_drop(z_move(keyexpr)); @@ -1378,14 +1394,25 @@ rmw_create_subscription( rmw_subscription->can_loan_messages = false; rmw_subscription->is_cft_enabled = false; + const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); + char * type_hash_c_str = nullptr; + rcutils_ret_t stringify_ret = rosidl_stringify_type_hash( + type_hash, + *allocator, + &type_hash_c_str); + if (RCUTILS_RET_BAD_ALLOC == stringify_ret) { + RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str."); + return nullptr; + } + // Everything above succeeded and is setup properly. Now declare a subscriber // with Zenoh; after this, callbacks may come in at any time. - z_owned_closure_sample_t callback = z_closure(rmw_zenoh_cpp::sub_data_handler, nullptr, sub_data); z_owned_keyexpr_t keyexpr = ros_topic_name_to_zenoh_key( node->context->actual_domain_id, topic_name, - sub_data->type_support->get_name()); + sub_data->type_support->get_name(), + type_hash_c_str); auto always_free_ros_keyexpr = rcpputils::make_scope_exit( [&keyexpr]() { z_keyexpr_drop(z_move(keyexpr)); @@ -2095,11 +2122,22 @@ rmw_create_client( return nullptr; } + const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); + char * type_hash_c_str = nullptr; + rcutils_ret_t stringify_ret = rosidl_stringify_type_hash( + type_hash, + *allocator, + &type_hash_c_str); + if (RCUTILS_RET_BAD_ALLOC == stringify_ret) { + RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str."); + return nullptr; + } client_data->keyexpr = ros_topic_name_to_zenoh_key( node->context->actual_domain_id, rmw_client->service_name, - service_type.c_str()); + service_type.c_str(), + type_hash_c_str); auto free_ros_keyexpr = rcpputils::make_scope_exit( [client_data]() { z_keyexpr_drop(z_move(client_data->keyexpr)); @@ -2635,10 +2673,22 @@ rmw_create_service( return nullptr; } + const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); + char * type_hash_c_str = nullptr; + rcutils_ret_t stringify_ret = rosidl_stringify_type_hash( + type_hash, + *allocator, + &type_hash_c_str); + if (RCUTILS_RET_BAD_ALLOC == stringify_ret) { + RMW_SET_ERROR_MSG("Failed to allocate type_hash_c_str."); + return nullptr; + } + service_data->keyexpr = ros_topic_name_to_zenoh_key( node->context->actual_domain_id, rmw_service->service_name, - service_type.c_str()); + service_type.c_str(), + type_hash_c_str); auto free_ros_keyexpr = rcpputils::make_scope_exit( [service_data]() { if (service_data) {