From 9e17da21ff8f487615ae13eebaf8e9637c1e4b16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 16 Dec 2024 11:12:20 +0100 Subject: [PATCH] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 2 +- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 17 --- rmw_zenoh_cpp/src/detail/liveliness_utils.hpp | 5 - rmw_zenoh_cpp/src/detail/rmw_client_data.cpp | 10 +- .../src/detail/rmw_publisher_data.cpp | 7 - .../src/detail/rmw_publisher_data.hpp | 3 - rmw_zenoh_cpp/src/detail/rmw_service_data.cpp | 123 +++++++++--------- .../src/detail/rmw_subscription_data.cpp | 122 +++++++++-------- 8 files changed, 123 insertions(+), 166 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index 5e480071..fef824c8 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -1202,7 +1202,7 @@ rmw_ret_t GraphCache::service_server_is_available( service_it->second.find(client_topic_info.type_); if (type_it != service_it->second.end()) { for (const auto & [_, topic_data] : type_it->second) { - if (topic_data->subs_.size() > 0) { + if (!topic_data->subs_.empty()) { *is_available = true; return RMW_RET_OK; } diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index 0d22d66f..214ab1b1 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -631,11 +631,6 @@ std::string Entity::liveliness_keyexpr() const return this->liveliness_keyexpr_; } -void Entity::copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const -{ - memcpy(out_gid, gid_.data(), RMW_GID_STORAGE_SIZE); -} - ///============================================================================= std::vector Entity::copy_gid() const { @@ -687,16 +682,4 @@ size_t hash_gid(const std::vector gid) } return std::hash{}(hash_str.str()); } - -///============================================================================= -size_t hash_gid_p(const uint8_t gid[RMW_GID_STORAGE_SIZE]) -{ - std::stringstream hash_str; - hash_str << std::hex; - size_t i = 0; - for (; i < (RMW_GID_STORAGE_SIZE - 1); i++) { - hash_str << static_cast(gid[i]); - } - return std::hash{}(hash_str.str()); -} } // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index 1096b45b..8dbadfb8 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -172,8 +172,6 @@ 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; - std::vector copy_gid() const; private: @@ -236,9 +234,6 @@ std::optional keyexpr_to_qos(const std::string & keyexpr); } // namespace liveliness ///============================================================================= -/// Generate a hash for a given GID. -size_t hash_gid_p(const uint8_t gid[RMW_GID_STORAGE_SIZE]); - size_t hash_gid(const std::vector gid); } // 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 68912103..35093240 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp @@ -431,15 +431,7 @@ rmw_ret_t ClientData::send_request( sub_data->add_new_reply( std::make_unique(reply, received_timestamp)); }, - [client_data]() { - auto sub_data = client_data.lock(); - if (sub_data == nullptr) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to obtain ClientData"); - return; - } - }, + zenoh::closures::none, std::move(opts), &result); if (result != Z_OK) { diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index 8b081288..3b112892 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -326,13 +326,6 @@ liveliness::TopicInfo PublisherData::topic_info() const return entity_->topic_info().value(); } -///============================================================================= -void PublisherData::copy_gid(uint8_t out_gid[RMW_GID_STORAGE_SIZE]) const -{ - std::lock_guard lock(mutex_); - entity_->copy_gid(out_gid); -} - std::vector PublisherData::copy_gid() const { std::lock_guard lock(mutex_); diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index cd8b2686..d5e8a502 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -69,9 +69,6 @@ class PublisherData final // Get a copy of the TopicInfo of this PublisherData. 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; - // Return a copy of the GID of this publisher. std::vector copy_gid() const; diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp index 30954901..a5c83c8d 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp @@ -292,76 +292,75 @@ rmw_ret_t ServiceData::take_request( auto payload_data = payload.value().get().as_vector(); - if (payload_data.size() > 0) { - // Object that manages the raw buffer - eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(const_cast(payload_data.data())), payload_data.size()); - - // Object that serializes the data - Cdr deser(fastbuffer); - if (!request_type_support_->deserialize_ros_message( - deser.get_cdr(), - ros_request, - request_type_support_impl_)) - { - RMW_SET_ERROR_MSG("could not deserialize ROS message"); - return RMW_RET_ERROR; - } + if (payload_data.empty()) { + RMW_ZENOH_LOG_DEBUG_NAMED( + "rmw_zenoh_cpp", + "ServiceData not able to get slice data"); + return RMW_RET_ERROR; + } + // Object that manages the raw buffer + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(const_cast(payload_data.data())), payload_data.size()); - // Fill in the request header. - // Get the sequence_number out of the attachment - if (!loaned_query.get_attachment().has_value()) { - RMW_ZENOH_LOG_DEBUG_NAMED( - "rmw_zenoh_cpp", - "ServiceData take_request attachment is empty"); - return RMW_RET_ERROR; - } + // Object that serializes the data + Cdr deser(fastbuffer); + if (!request_type_support_->deserialize_ros_message( + deser.get_cdr(), + ros_request, + request_type_support_impl_)) + { + RMW_SET_ERROR_MSG("could not deserialize ROS message"); + return RMW_RET_ERROR; + } - rmw_zenoh_cpp::AttachmentData attachment(std::move( - loaned_query.get_attachment().value().get())); + // Fill in the request header. + // Get the sequence_number out of the attachment + if (!loaned_query.get_attachment().has_value()) { + RMW_ZENOH_LOG_DEBUG_NAMED( + "rmw_zenoh_cpp", + "ServiceData take_request attachment is empty"); + return RMW_RET_ERROR; + } - request_header->request_id.sequence_number = attachment.sequence_number(); - if (request_header->request_id.sequence_number < 0) { - RMW_SET_ERROR_MSG("Failed to get sequence_number from client call attachment"); - return RMW_RET_ERROR; - } + rmw_zenoh_cpp::AttachmentData attachment(std::move( + loaned_query.get_attachment().value().get())); - auto writter_gid_v = attachment.copy_gid(); - memcpy( - request_header->request_id.writer_guid, - writter_gid_v.data(), - RMW_GID_STORAGE_SIZE); + request_header->request_id.sequence_number = attachment.sequence_number(); + if (request_header->request_id.sequence_number < 0) { + RMW_SET_ERROR_MSG("Failed to get sequence_number from client call attachment"); + return RMW_RET_ERROR; + } - request_header->source_timestamp = attachment.source_timestamp(); - if (request_header->source_timestamp < 0) { - RMW_SET_ERROR_MSG("Failed to get source_timestamp from client call attachment"); - return RMW_RET_ERROR; - } - 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(writter_gid_v); - std::unordered_map::iterator it = sequence_to_query_map_.find(hash); - if (it == sequence_to_query_map_.end()) { - SequenceToQuery stq; - sequence_to_query_map_.insert(std::make_pair(hash, std::move(stq))); - it = sequence_to_query_map_.find(hash); - } else { - // Client already in the map - if (it->second.find(request_header->request_id.sequence_number) != it->second.end()) { - RMW_SET_ERROR_MSG("duplicate sequence number in the map"); - return RMW_RET_ERROR; - } - } + auto writter_gid_v = attachment.copy_gid(); + memcpy( + request_header->request_id.writer_guid, + writter_gid_v.data(), + RMW_GID_STORAGE_SIZE); - it->second.insert(std::make_pair(request_header->request_id.sequence_number, std::move(query))); - *taken = true; - } else { - RMW_ZENOH_LOG_DEBUG_NAMED( - "rmw_zenoh_cpp", - "ServiceData not able to get slice data"); + request_header->source_timestamp = attachment.source_timestamp(); + if (request_header->source_timestamp < 0) { + RMW_SET_ERROR_MSG("Failed to get source_timestamp from client call attachment"); return RMW_RET_ERROR; } + 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(writter_gid_v); + std::unordered_map::iterator it = sequence_to_query_map_.find(hash); + if (it == sequence_to_query_map_.end()) { + SequenceToQuery stq; + sequence_to_query_map_.insert(std::make_pair(hash, std::move(stq))); + it = sequence_to_query_map_.find(hash); + } else { + // Client already in the map + if (it->second.find(request_header->request_id.sequence_number) != it->second.end()) { + RMW_SET_ERROR_MSG("duplicate sequence number in the map"); + return RMW_RET_ERROR; + } + } + + it->second.insert(std::make_pair(request_header->request_id.sequence_number, std::move(query))); + *taken = true; return RMW_RET_OK; } diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp index 96cb33da..f3080490 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp @@ -493,43 +493,42 @@ rmw_ret_t SubscriptionData::take_one_message( auto payload_data = msg_data->payload.as_vector(); - if (payload_data.size() > 0) { - // Object that manages the raw buffer - eprosima::fastcdr::FastBuffer fastbuffer( - reinterpret_cast(const_cast(payload_data.data())), - payload_data.size()); - - // Object that serializes the data - rmw_zenoh_cpp::Cdr deser(fastbuffer); - if (!type_support_->deserialize_ros_message( - deser.get_cdr(), - ros_message, - type_support_impl_)) - { - RMW_SET_ERROR_MSG("could not deserialize ROS message"); - return RMW_RET_ERROR; - } - - if (message_info != nullptr) { - message_info->source_timestamp = msg_data->attachment.source_timestamp(); - message_info->received_timestamp = msg_data->recv_timestamp; - message_info->publication_sequence_number = msg_data->attachment.sequence_number(); - // TODO(clalancette): fill in reception_sequence_number - message_info->reception_sequence_number = 0; - message_info->publisher_gid.implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; - memcpy( - message_info->publisher_gid.data, - msg_data->attachment.copy_gid().data(), - RMW_GID_STORAGE_SIZE); - message_info->from_intra_process = false; - } - *taken = true; - } else { + if (payload_data.empty()) { RMW_ZENOH_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", "SubscriptionData not able to get slice data"); return RMW_RET_ERROR; } + // Object that manages the raw buffer + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(const_cast(payload_data.data())), + payload_data.size()); + + // Object that serializes the data + rmw_zenoh_cpp::Cdr deser(fastbuffer); + if (!type_support_->deserialize_ros_message( + deser.get_cdr(), + ros_message, + type_support_impl_)) + { + RMW_SET_ERROR_MSG("could not deserialize ROS message"); + return RMW_RET_ERROR; + } + + if (message_info != nullptr) { + message_info->source_timestamp = msg_data->attachment.source_timestamp(); + message_info->received_timestamp = msg_data->recv_timestamp; + message_info->publication_sequence_number = msg_data->attachment.sequence_number(); + // TODO(clalancette): fill in reception_sequence_number + message_info->reception_sequence_number = 0; + message_info->publisher_gid.implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; + memcpy( + message_info->publisher_gid.data, + msg_data->attachment.copy_gid().data(), + RMW_GID_STORAGE_SIZE); + message_info->from_intra_process = false; + } + *taken = true; return RMW_RET_OK; } @@ -552,41 +551,40 @@ rmw_ret_t SubscriptionData::take_serialized_message( auto payload_data = msg_data->payload.as_vector(); - if (payload_data.size() > 0) { - if (serialized_message->buffer_capacity < payload_data.size()) { - rmw_ret_t ret = - rmw_serialized_message_resize(serialized_message, payload_data.size()); - if (ret != RMW_RET_OK) { - return ret; // Error message already set - } - } - serialized_message->buffer_length = payload_data.size(); - memcpy( - serialized_message->buffer, - reinterpret_cast(const_cast(payload_data.data())), - payload_data.size()); - - *taken = true; - - if (message_info != nullptr) { - message_info->source_timestamp = msg_data->attachment.source_timestamp(); - message_info->received_timestamp = msg_data->recv_timestamp; - message_info->publication_sequence_number = msg_data->attachment.sequence_number(); - // TODO(clalancette): fill in reception_sequence_number - message_info->reception_sequence_number = 0; - message_info->publisher_gid.implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; - memcpy( - message_info->publisher_gid.data, - msg_data->attachment.copy_gid().data(), - RMW_GID_STORAGE_SIZE); - message_info->from_intra_process = false; - } - } else { + if (payload_data.empty()) { RMW_ZENOH_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", "SubscriptionData not able to get slice data"); return RMW_RET_ERROR; } + if (serialized_message->buffer_capacity < payload_data.size()) { + rmw_ret_t ret = + rmw_serialized_message_resize(serialized_message, payload_data.size()); + if (ret != RMW_RET_OK) { + return ret; // Error message already set + } + } + serialized_message->buffer_length = payload_data.size(); + memcpy( + serialized_message->buffer, + reinterpret_cast(const_cast(payload_data.data())), + payload_data.size()); + + *taken = true; + + if (message_info != nullptr) { + message_info->source_timestamp = msg_data->attachment.source_timestamp(); + message_info->received_timestamp = msg_data->recv_timestamp; + message_info->publication_sequence_number = msg_data->attachment.sequence_number(); + // TODO(clalancette): fill in reception_sequence_number + message_info->reception_sequence_number = 0; + message_info->publisher_gid.implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; + memcpy( + message_info->publisher_gid.data, + msg_data->attachment.copy_gid().data(), + RMW_GID_STORAGE_SIZE); + message_info->from_intra_process = false; + } return RMW_RET_OK; }