From a442bc73a399e6787be6bb6451a215cab7f77cdb Mon Sep 17 00:00:00 2001 From: yuanyuyuan Date: Wed, 11 Dec 2024 00:04:36 +0800 Subject: [PATCH] feat: support ROS2 humble --- .../src/detail/attachment_helpers.cpp | 14 +-- .../src/detail/attachment_helpers.hpp | 6 +- rmw_zenoh_cpp/src/detail/event.cpp | 10 +- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 26 ++--- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 13 +-- rmw_zenoh_cpp/src/detail/liveliness_utils.hpp | 6 +- rmw_zenoh_cpp/src/detail/rmw_client_data.cpp | 43 ++++---- rmw_zenoh_cpp/src/detail/rmw_client_data.hpp | 2 +- .../src/detail/rmw_publisher_data.cpp | 35 +++--- .../src/detail/rmw_publisher_data.hpp | 2 +- rmw_zenoh_cpp/src/detail/rmw_service_data.cpp | 48 +++++---- .../src/detail/rmw_subscription_data.cpp | 29 ++--- rmw_zenoh_cpp/src/detail/zenoh_utils.cpp | 2 +- rmw_zenoh_cpp/src/detail/zenoh_utils.hpp | 4 +- rmw_zenoh_cpp/src/rmw_event.cpp | 22 ++-- rmw_zenoh_cpp/src/rmw_init_options.cpp | 36 ++++--- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 100 +++++++++--------- 17 files changed, 211 insertions(+), 187 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp index da2571ce..e5c9a2f5 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.cpp @@ -31,11 +31,11 @@ namespace rmw_zenoh_cpp AttachmentData::AttachmentData( const int64_t sequence_number, const int64_t source_timestamp, - const uint8_t source_gid[RMW_GID_STORAGE_SIZE]) + const uint8_t source_gid[16]) : sequence_number_(sequence_number), source_timestamp_(source_timestamp) { - memcpy(source_gid_, source_gid, RMW_GID_STORAGE_SIZE); + memcpy(source_gid_, source_gid, 16); gid_hash_ = hash_gid(source_gid_); } @@ -45,7 +45,7 @@ AttachmentData::AttachmentData(AttachmentData && data) source_timestamp_(std::move(data.source_timestamp_)), gid_hash_(std::move(data.gid_hash_)) { - memcpy(source_gid_, data.source_gid_, RMW_GID_STORAGE_SIZE); + memcpy(source_gid_, data.source_gid_, 16); } ///============================================================================= @@ -90,7 +90,7 @@ AttachmentData::AttachmentData(const z_loaned_bytes_t * attachment) if (ze_deserializer_deserialize_slice(&deserializer, &slice)) { throw std::runtime_error("Failed to deserialize the source_gid."); } - if (z_slice_len(z_loan(slice)) != RMW_GID_STORAGE_SIZE) { + if (z_slice_len(z_loan(slice)) != 16) { throw std::runtime_error("The length of source_gid mismatched."); } memcpy(this->source_gid_, z_slice_data(z_loan(slice)), z_slice_len(z_loan(slice))); @@ -111,9 +111,9 @@ int64_t AttachmentData::source_timestamp() const } ///============================================================================= -void AttachmentData::copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const +void AttachmentData::copy_gid(uint8_t out_gid[16]) const { - memcpy(out_gid, source_gid_, RMW_GID_STORAGE_SIZE); + memcpy(out_gid, source_gid_, 16); } ///============================================================================= @@ -132,7 +132,7 @@ void AttachmentData::serialize_to_zbytes(z_owned_bytes_t * attachment) ze_serializer_serialize_str(z_loan_mut(serializer), "source_timestamp"); ze_serializer_serialize_int64(z_loan_mut(serializer), this->source_timestamp_); ze_serializer_serialize_str(z_loan_mut(serializer), "source_gid"); - ze_serializer_serialize_buf(z_loan_mut(serializer), this->source_gid_, RMW_GID_STORAGE_SIZE); + ze_serializer_serialize_buf(z_loan_mut(serializer), this->source_gid_, 16); ze_serializer_finish(z_move(serializer), attachment); } } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp index ec460620..c348919e 100644 --- a/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp +++ b/rmw_zenoh_cpp/src/detail/attachment_helpers.hpp @@ -28,13 +28,13 @@ class AttachmentData final AttachmentData( const int64_t sequence_number, const int64_t source_timestamp, - const uint8_t source_gid[RMW_GID_STORAGE_SIZE]); + const uint8_t source_gid[16]); explicit AttachmentData(const z_loaned_bytes_t *); explicit AttachmentData(AttachmentData && data); int64_t sequence_number() const; int64_t source_timestamp() const; - void copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const; + void copy_gid(uint8_t out_gid[16]) const; size_t gid_hash() const; void serialize_to_zbytes(z_owned_bytes_t *); @@ -42,7 +42,7 @@ class AttachmentData final private: int64_t sequence_number_; int64_t source_timestamp_; - uint8_t source_gid_[RMW_GID_STORAGE_SIZE]; + uint8_t source_gid_[16]; size_t gid_hash_; }; } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/event.cpp b/rmw_zenoh_cpp/src/detail/event.cpp index d03560d6..ce507624 100644 --- a/rmw_zenoh_cpp/src/detail/event.cpp +++ b/rmw_zenoh_cpp/src/detail/event.cpp @@ -31,11 +31,11 @@ static const std::unordered_mapinfo_.type_hash_.c_str(), - &type_hash); - if (RCUTILS_RET_OK == rc_ret) { - ret = rmw_topic_endpoint_info_set_topic_type_hash(&ep, &type_hash); - if (RMW_RET_OK != ret) { - return ret; - } - } - - memset(ep.endpoint_gid, 0, RMW_GID_STORAGE_SIZE); + // rosidl_type_hash_t type_hash; + // rcutils_ret_t rc_ret = rosidl_parse_type_hash_string( + // topic_data->info_.type_hash_.c_str(), + // &type_hash); + // if (RCUTILS_RET_OK == rc_ret) { + // ret = rmw_topic_endpoint_info_set_topic_type_hash(&ep, &type_hash); + // if (RMW_RET_OK != ret) { + // return ret; + // } + // } + + memset(ep.endpoint_gid, 0, 16); entity->copy_gid(ep.endpoint_gid); endpoints.push_back(ep); diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index 52c8ec7f..8a365d49 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -177,8 +177,8 @@ static const std::unordered_map str_to {std::to_string(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC), RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC}, {std::to_string(RMW_QOS_POLICY_LIVELINESS_UNKNOWN), RMW_QOS_POLICY_LIVELINESS_UNKNOWN}, - {std::to_string(RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE), - RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE} + // {std::to_string(RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE), + // RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE} }; std::vector split_keyexpr( @@ -638,9 +638,9 @@ std::string Entity::liveliness_keyexpr() const } ///============================================================================= -void Entity::copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const +void Entity::copy_gid(uint8_t out_gid[16]) const { - memcpy(out_gid, gid_, RMW_GID_STORAGE_SIZE); + memcpy(out_gid, gid_, 16); } ///============================================================================= @@ -679,12 +679,13 @@ std::string demangle_name(const std::string & input) } // namespace liveliness ///============================================================================= -size_t hash_gid(const uint8_t gid[RMW_GID_STORAGE_SIZE]) +size_t hash_gid(const uint8_t gid[16]) +// size_t hash_gid(const int8_t gid[16]) { std::stringstream hash_str; hash_str << std::hex; size_t i = 0; - for (; i < (RMW_GID_STORAGE_SIZE - 1); i++) { + for (; i < (16 - 1); i++) { hash_str << static_cast(gid[i]); } return std::hash{}(hash_str.str()); diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index 895ae604..6c3feabc 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -172,7 +172,7 @@ class Entity // Two entities are equal if their keyexpr_hash are equal. bool operator==(const Entity & other) const; - void copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const; + void copy_gid(uint8_t out_gid[16]) const; private: Entity( @@ -191,7 +191,7 @@ class Entity NodeInfo node_info_; std::optional topic_info_; std::string liveliness_keyexpr_; - uint8_t gid_[RMW_GID_STORAGE_SIZE]; + uint8_t gid_[16]; }; ///============================================================================= @@ -239,7 +239,7 @@ std::string zid_to_str(const z_id_t & id); ///============================================================================= /// Generate a hash for a given GID. -size_t hash_gid(const uint8_t gid[RMW_GID_STORAGE_SIZE]); +size_t hash_gid(const uint8_t gid[16]); } // namespace rmw_zenoh_cpp ///============================================================================= diff --git a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp index f202ad29..5437bfcf 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp @@ -124,9 +124,9 @@ std::shared_ptr ClientData::make( return nullptr; } - rcutils_allocator_t * allocator = &node->context->options.allocator; + // rcutils_allocator_t * allocator = &node->context->options.allocator; - const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); + // const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); auto service_members = static_cast(type_support->data); auto request_members = static_cast( service_members->request_members_->data); @@ -151,19 +151,20 @@ std::shared_ptr ClientData::make( } // Convert the type hash to a string so that it can be included in the keyexpr. - 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; - } - auto free_type_hash_c_str = rcpputils::make_scope_exit( - [&allocator, &type_hash_c_str]() { - allocator->deallocate(type_hash_c_str, allocator->state); - }); + const char * type_hash_c_str = "TypeHashNotSupported"; + // 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; + // } + // auto free_type_hash_c_str = rcpputils::make_scope_exit( + // [&allocator, &type_hash_c_str]() { + // allocator->deallocate(type_hash_c_str, allocator->state); + // }); std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( @@ -296,7 +297,7 @@ bool ClientData::liveliness_is_valid() const } ///============================================================================= -void ClientData::copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const +void ClientData::copy_gid(uint8_t out_gid[16]) const { std::lock_guard lock(mutex_); entity_->copy_gid(out_gid); @@ -386,7 +387,13 @@ rmw_ret_t ClientData::take_response( RMW_SET_ERROR_MSG("Failed to get source_timestamp from client call attachment"); return RMW_RET_ERROR; } - attachment.copy_gid(request_header->request_id.writer_guid); + + // attachment.copy_gid(request_header->request_id.writer_guid); + uint8_t tmp_writer_guid[16]; + attachment.copy_gid(tmp_writer_guid); + for (size_t i = 0; i < 16; ++i) { + request_header->request_id.writer_guid[i] = static_cast(tmp_writer_guid[i]); + } request_header->received_timestamp = latest_reply->get_received_timestamp(); z_drop(z_move(payload)); @@ -446,7 +453,7 @@ rmw_ret_t ClientData::send_request( z_get_options_t opts; z_get_options_default(&opts); z_owned_bytes_t attachment; - uint8_t local_gid[RMW_GID_STORAGE_SIZE]; + uint8_t local_gid[16]; entity_->copy_gid(local_gid); create_map_and_set_sequence_num( &attachment, *sequence_id, diff --git a/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp index 349a26db..cc141bc3 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp @@ -64,7 +64,7 @@ class ClientData final : public std::enable_shared_from_this bool liveliness_is_valid() const; // Copy the GID of this ClientData into an rmw_gid_t. - void copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const; + void copy_gid(uint8_t out_gid[16]) const; // Add a new ZenohReply to the queue. void add_new_reply(std::unique_ptr reply); diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index f54c50ab..19ac1f76 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -60,25 +60,26 @@ std::shared_ptr PublisherData::make( rcutils_allocator_t * allocator = &node->context->options.allocator; - const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); + // const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); auto callbacks = static_cast(type_support->data); auto message_type_support = std::make_unique(callbacks); // Convert the type hash to a string so that it can be included in // the keyexpr. - 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; - } - auto always_free_type_hash_c_str = rcpputils::make_scope_exit( - [&allocator, &type_hash_c_str]() { - allocator->deallocate(type_hash_c_str, allocator->state); - }); + const char * type_hash_c_str = "TypeHashNotSupported"; + // 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; + // } + // auto always_free_type_hash_c_str = rcpputils::make_scope_exit( + // [&allocator, &type_hash_c_str]() { + // allocator->deallocate(type_hash_c_str, allocator->state); + // }); std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( @@ -309,7 +310,7 @@ rmw_ret_t PublisherData::publish( z_publisher_put_options_t options; z_publisher_put_options_default(&options); z_owned_bytes_t attachment; - uint8_t local_gid[RMW_GID_STORAGE_SIZE]; + uint8_t local_gid[16]; entity_->copy_gid(local_gid); create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid); options.attachment = z_move(attachment); @@ -357,7 +358,7 @@ rmw_ret_t PublisherData::publish_serialized_message( // will be encoded with CDR so it does not really matter. z_publisher_put_options_t options; z_publisher_put_options_default(&options); - uint8_t local_gid[RMW_GID_STORAGE_SIZE]; + uint8_t local_gid[16]; entity_->copy_gid(local_gid); z_owned_bytes_t attachment; create_map_and_set_sequence_num(&attachment, sequence_number_++, local_gid); @@ -395,7 +396,7 @@ liveliness::TopicInfo PublisherData::topic_info() const } ///============================================================================= -void PublisherData::copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const +void PublisherData::copy_gid(uint8_t out_gid[16]) const { std::lock_guard lock(mutex_); entity_->copy_gid(out_gid); diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index 1853ff8a..2cec9b7f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -69,7 +69,7 @@ class PublisherData final liveliness::TopicInfo topic_info() const; // Copy the GID of this PublisherData into an rmw_gid_t. - void copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const; + void copy_gid(uint8_t out_gid[16]) const; // Returns true if liveliness token is still valid. bool liveliness_is_valid() const; diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp index 3d456a02..afac1366 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp @@ -85,7 +85,7 @@ std::shared_ptr ServiceData::make( rcutils_allocator_t * allocator = &node->context->options.allocator; - const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); + // const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); auto service_members = static_cast(type_support->data); auto request_members = static_cast( service_members->request_members_->data); @@ -110,19 +110,20 @@ std::shared_ptr ServiceData::make( } // Convert the type hash to a string so that it can be included in the keyexpr. - 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; - } - auto free_type_hash_c_str = rcpputils::make_scope_exit( - [&allocator, &type_hash_c_str]() { - allocator->deallocate(type_hash_c_str, allocator->state); - }); + const char * type_hash_c_str = "TypeHashNotSupported"; + // 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; + // } + // auto free_type_hash_c_str = rcpputils::make_scope_exit( + // [&allocator, &type_hash_c_str]() { + // allocator->deallocate(type_hash_c_str, allocator->state); + // }); std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( @@ -351,7 +352,13 @@ rmw_ret_t ServiceData::take_request( return RMW_RET_ERROR; } - attachment.copy_gid(request_header->request_id.writer_guid); + + // attachment.copy_gid(request_header->request_id.writer_guid); + uint8_t tmp_writer_guid[16]; + attachment.copy_gid(tmp_writer_guid); + for (size_t i = 0; i < 16; ++i) { + request_header->request_id.writer_guid[i] = static_cast(tmp_writer_guid[i]); + } request_header->source_timestamp = attachment.source_timestamp(); if (request_header->source_timestamp < 0) { @@ -361,7 +368,8 @@ rmw_ret_t ServiceData::take_request( request_header->received_timestamp = query->get_received_timestamp(); // Add this query to the map, so that rmw_send_response can quickly look it up later. - const size_t hash = rmw_zenoh_cpp::hash_gid(request_header->request_id.writer_guid); + // const size_t hash = rmw_zenoh_cpp::hash_gid(request_header->request_id.writer_guid); + const size_t hash = rmw_zenoh_cpp::hash_gid(tmp_writer_guid); std::unordered_map::iterator it = sequence_to_query_map_.find(hash); if (it == sequence_to_query_map_.end()) { SequenceToQuery stq; @@ -397,7 +405,11 @@ rmw_ret_t ServiceData::send_response( return RMW_RET_OK; } // Create the queryable payload - const size_t hash = hash_gid(request_id->writer_guid); + uint8_t tmp_writer_guid[16]; + for (size_t i = 0; i < 16; ++i) { + tmp_writer_guid[i] = static_cast(request_id->writer_guid[i]); + } + const size_t hash = hash_gid(tmp_writer_guid); std::unordered_map::iterator it = sequence_to_query_map_.find(hash); if (it == sequence_to_query_map_.end()) { // If there is no data associated with this request, the higher layers of @@ -457,7 +469,7 @@ rmw_ret_t ServiceData::send_response( z_owned_bytes_t attachment; rmw_zenoh_cpp::create_map_and_set_sequence_num( &attachment, request_id->sequence_number, - request_id->writer_guid); + tmp_writer_guid); options.attachment = z_move(attachment); z_owned_bytes_t payload; diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp index 4f21300b..96cf674a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp @@ -113,24 +113,25 @@ std::shared_ptr SubscriptionData::make( rcutils_allocator_t * allocator = &node->context->options.allocator; - const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); + // const rosidl_type_hash_t * type_hash = type_support->get_type_hash_func(type_support); auto callbacks = static_cast(type_support->data); auto message_type_support = std::make_unique(callbacks); // Convert the type hash to a string so that it can be included in the keyexpr. - 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; - } - auto free_type_hash_c_str = rcpputils::make_scope_exit( - [&allocator, &type_hash_c_str]() { - allocator->deallocate(type_hash_c_str, allocator->state); - }); + const char * type_hash_c_str = "TypeHashNotSupported"; + // 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; + // } + // auto free_type_hash_c_str = rcpputils::make_scope_exit( + // [&allocator, &type_hash_c_str]() { + // allocator->deallocate(type_hash_c_str, allocator->state); + // }); // Everything above succeeded and is setup properly. Now declare a subscriber // with Zenoh; after this, callbacks may come in at any time. diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp index be5721b7..4dd8b354 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp @@ -40,7 +40,7 @@ ZenohSession::~ZenohSession() ///============================================================================= void create_map_and_set_sequence_num( - z_owned_bytes_t * out_bytes, int64_t sequence_number, uint8_t gid[RMW_GID_STORAGE_SIZE]) + z_owned_bytes_t * out_bytes, const int64_t sequence_number, const uint8_t gid[16]) { auto now = std::chrono::system_clock::now().time_since_epoch(); auto now_ns = std::chrono::duration_cast(now); diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp index ae2aed3a..9ab03664 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp @@ -43,8 +43,8 @@ class ZenohSession final ///============================================================================= void create_map_and_set_sequence_num( - z_owned_bytes_t * out_bytes, int64_t sequence_number, - uint8_t gid[RMW_GID_STORAGE_SIZE]); + z_owned_bytes_t * out_bytes, const int64_t sequence_number, + const uint8_t gid[16]); ///============================================================================= // A class to store the replies to service requests. diff --git a/rmw_zenoh_cpp/src/rmw_event.cpp b/rmw_zenoh_cpp/src/rmw_event.cpp index 63de9bcb..22235898 100644 --- a/rmw_zenoh_cpp/src/rmw_event.cpp +++ b/rmw_zenoh_cpp/src/rmw_event.cpp @@ -233,17 +233,17 @@ rmw_take_event( *taken = true; return RMW_RET_OK; } - case rmw_zenoh_cpp::ZENOH_EVENT_PUBLICATION_MATCHED: - case rmw_zenoh_cpp::ZENOH_EVENT_SUBSCRIPTION_MATCHED: { - auto ei = static_cast(event_info); - RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); - ei->total_count = st->total_count; - ei->total_count_change = st->total_count_change; - ei->current_count = st->current_count; - ei->current_count_change = st->current_count_change; - *taken = true; - return RMW_RET_OK; - } + // case rmw_zenoh_cpp::ZENOH_EVENT_PUBLICATION_MATCHED: + // case rmw_zenoh_cpp::ZENOH_EVENT_SUBSCRIPTION_MATCHED: { + // auto ei = static_cast(event_info); + // RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); + // ei->total_count = st->total_count; + // ei->total_count_change = st->total_count_change; + // ei->current_count = st->current_count; + // ei->current_count_change = st->current_count_change; + // *taken = true; + // return RMW_RET_OK; + // } case rmw_zenoh_cpp::ZENOH_EVENT_OFFERED_QOS_INCOMPATIBLE: { auto ei = static_cast(event_info); RMW_CHECK_ARGUMENT_FOR_NULL(ei, RMW_RET_INVALID_ARGUMENT); diff --git a/rmw_zenoh_cpp/src/rmw_init_options.cpp b/rmw_zenoh_cpp/src/rmw_init_options.cpp index 783c5a6a..77f9512d 100644 --- a/rmw_zenoh_cpp/src/rmw_init_options.cpp +++ b/rmw_zenoh_cpp/src/rmw_init_options.cpp @@ -49,8 +49,10 @@ rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t all init_options->enclave = nullptr; init_options->domain_id = RMW_DEFAULT_DOMAIN_ID; init_options->security_options = rmw_get_default_security_options(); - init_options->discovery_options = rmw_get_zero_initialized_discovery_options(); - return rmw_discovery_options_init(&(init_options->discovery_options), 0, &allocator); + init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT; + // init_options->discovery_options = rmw_get_zero_initialized_discovery_options(); + // return rmw_discovery_options_init(&(init_options->discovery_options), 0, &allocator); + return RMW_RET_OK; } //============================================================================== @@ -89,19 +91,19 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) [&tmp, allocator]() { rmw_security_options_fini(&tmp.security_options, &allocator); }); - tmp.discovery_options = rmw_get_zero_initialized_discovery_options(); - ret = rmw_discovery_options_copy( - &src->discovery_options, - &allocator, - &tmp.discovery_options); - if (RMW_RET_OK != ret) { - return ret; - } - auto free_discovery_options = rcpputils::make_scope_exit( - [&tmp]() { - rmw_ret_t tmp_ret = rmw_discovery_options_fini(&tmp.discovery_options); - static_cast(tmp_ret); - }); + // tmp.discovery_options = rmw_get_zero_initialized_discovery_options(); + // ret = rmw_discovery_options_copy( + // &src->discovery_options, + // &allocator, + // &tmp.discovery_options); + // if (RMW_RET_OK != ret) { + // return ret; + // } + // auto free_discovery_options = rcpputils::make_scope_exit( + // [&tmp]() { + // rmw_ret_t tmp_ret = rmw_discovery_options_fini(&tmp.discovery_options); + // static_cast(tmp_ret); + // }); tmp.enclave = nullptr; if (nullptr != src->enclave) { tmp.enclave = rcutils_strdup(src->enclave, allocator); @@ -124,7 +126,7 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) *dst = tmp; free_enclave.cancel(); - free_discovery_options.cancel(); + // free_discovery_options.cancel(); free_security_options.cancel(); return RMW_RET_OK; @@ -154,7 +156,7 @@ rmw_init_options_fini(rmw_init_options_t * init_options) return ret; } - ret = rmw_discovery_options_fini(&init_options->discovery_options); + // ret = rmw_discovery_options_fini(&init_options->discovery_options); *init_options = rmw_get_zero_initialized_init_options(); return ret; diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 1e13a710..2ae3e098 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -47,7 +47,7 @@ #include "rcutils/types.h" #include "rmw/allocators.h" -#include "rmw/dynamic_message_type_support.h" +// #include "rmw/dynamic_message_type_support.h" #include "rmw/error_handling.h" #include "rmw/features.h" #include "rmw/impl/cpp/macros.hpp" @@ -139,10 +139,10 @@ bool rmw_feature_supported(rmw_feature_t feature) return false; case RMW_FEATURE_MESSAGE_INFO_RECEPTION_SEQUENCE_NUMBER: return false; - case RMW_MIDDLEWARE_SUPPORTS_TYPE_DISCOVERY: - return true; - case RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_MESSAGE: - return false; + // case RMW_MIDDLEWARE_SUPPORTS_TYPE_DISCOVERY: + // return true; + // case RMW_MIDDLEWARE_CAN_TAKE_DYNAMIC_MESSAGE: + // return false; default: return false; } @@ -497,50 +497,50 @@ rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) return RMW_RET_OK; } -//============================================================================== -rmw_ret_t -rmw_take_dynamic_message( - const rmw_subscription_t * subscription, - rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, - bool * taken, - rmw_subscription_allocation_t * allocation) -{ - static_cast(subscription); - static_cast(dynamic_message); - static_cast(taken); - static_cast(allocation); - return RMW_RET_UNSUPPORTED; -} - -//============================================================================== -rmw_ret_t -rmw_take_dynamic_message_with_info( - const rmw_subscription_t * subscription, - rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, - bool * taken, - rmw_message_info_t * message_info, - rmw_subscription_allocation_t * allocation) -{ - static_cast(subscription); - static_cast(dynamic_message); - static_cast(taken); - static_cast(message_info); - static_cast(allocation); - return RMW_RET_UNSUPPORTED; -} - -//============================================================================== -rmw_ret_t -rmw_serialization_support_init( - const char * serialization_lib_name, - rcutils_allocator_t * allocator, - rosidl_dynamic_typesupport_serialization_support_t * serialization_support) -{ - static_cast(serialization_lib_name); - static_cast(allocator); - static_cast(serialization_support); - return RMW_RET_UNSUPPORTED; -} +// //============================================================================== +// rmw_ret_t +// rmw_take_dynamic_message( +// const rmw_subscription_t * subscription, +// rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, +// bool * taken, +// rmw_subscription_allocation_t * allocation) +// { +// static_cast(subscription); +// static_cast(dynamic_message); +// static_cast(taken); +// static_cast(allocation); +// return RMW_RET_UNSUPPORTED; +// } + +// //============================================================================== +// rmw_ret_t +// rmw_take_dynamic_message_with_info( +// const rmw_subscription_t * subscription, +// rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message, +// bool * taken, +// rmw_message_info_t * message_info, +// rmw_subscription_allocation_t * allocation) +// { +// static_cast(subscription); +// static_cast(dynamic_message); +// static_cast(taken); +// static_cast(message_info); +// static_cast(allocation); +// return RMW_RET_UNSUPPORTED; +// } + +// //============================================================================== +// rmw_ret_t +// rmw_serialization_support_init( +// const char * serialization_lib_name, +// rcutils_allocator_t * allocator, +// rosidl_dynamic_typesupport_serialization_support_t * serialization_support) +// { +// static_cast(serialization_lib_name); +// static_cast(allocator); +// static_cast(serialization_support); +// return RMW_RET_UNSUPPORTED; +// } //============================================================================== /// Borrow a loaned ROS message. @@ -2492,7 +2492,7 @@ rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * re return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); RMW_CHECK_ARGUMENT_FOR_NULL(result, RMW_RET_INVALID_ARGUMENT); - *result = memcmp(gid1->data, gid2->data, RMW_GID_STORAGE_SIZE) == 0; + *result = memcmp(gid1->data, gid2->data, 16) == 0; return RMW_RET_OK; }