From 95a834f1686bece0911c5a3de6335cc650b426ef Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 4 Sep 2024 13:51:51 +0800 Subject: [PATCH 1/7] Store node data within context impl Signed-off-by: Yadunund --- rmw_zenoh_cpp/CMakeLists.txt | 1 + .../src/detail/rmw_context_impl_s.cpp | 95 +++++++++++- .../src/detail/rmw_context_impl_s.hpp | 29 +++- rmw_zenoh_cpp/src/detail/rmw_data_types.hpp | 14 -- rmw_zenoh_cpp/src/detail/rmw_node_data.cpp | 49 +++++++ rmw_zenoh_cpp/src/detail/rmw_node_data.hpp | 64 +++++++++ rmw_zenoh_cpp/src/rmw_zenoh.cpp | 135 ++++++------------ 7 files changed, 275 insertions(+), 112 deletions(-) create mode 100644 rmw_zenoh_cpp/src/detail/rmw_node_data.cpp create mode 100644 rmw_zenoh_cpp/src/detail/rmw_node_data.hpp diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index c548a6a7..95aab3d6 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -40,6 +40,7 @@ add_library(rmw_zenoh_cpp SHARED src/detail/qos.cpp src/detail/rmw_context_impl_s.cpp src/detail/rmw_data_types.cpp + src/detail/rmw_node_data.cpp src/detail/service_type_support.cpp src/detail/type_support.cpp src/detail/type_support_common.cpp diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index d60bece8..166e5bab 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -81,19 +81,22 @@ void rmw_context_impl_s::graph_sub_data_handler(const z_sample_t * sample, void ///============================================================================= rmw_context_impl_s::Data::Data( + std::size_t domain_id, const std::string & enclave, z_owned_session_t session, std::optional shm_manager, const std::string & liveliness_str, std::shared_ptr graph_cache) : enclave_(std::move(enclave)), + domain_id_(std::move(domain_id)), session_(std::move(session)), shm_manager_(std::move(shm_manager)), liveliness_str_(std::move(liveliness_str)), graph_cache_(std::move(graph_cache)), is_shutdown_(false), next_entity_id_(0), - is_initialized_(false) + is_initialized_(false), + nodes_({}) { graph_guard_condition_ = std::make_unique(); graph_guard_condition_->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; @@ -308,6 +311,7 @@ rmw_context_impl_s::rmw_context_impl_s( free_shm_manager.cancel(); data_ = std::make_shared( + domain_id, std::move(enclave), std::move(session), std::move(shm_manager), @@ -349,7 +353,7 @@ rmw_guard_condition_t * rmw_context_impl_s::graph_guard_condition() } ///============================================================================= -size_t rmw_context_impl_s::get_next_entity_id() +std::size_t rmw_context_impl_s::get_next_entity_id() { std::lock_guard lock(data_->mutex_); return data_->next_entity_id_++; @@ -381,3 +385,90 @@ std::shared_ptr rmw_context_impl_s::graph_cache() std::lock_guard lock(data_->mutex_); return data_->graph_cache_; } + +///============================================================================= +bool rmw_context_impl_s::create_node_data( + const rmw_node_t * const node, + const std::string & ns, + const std::string & node_name) +{ + std::lock_guard lock(data_->mutex_); + auto node_insertion = data_->nodes_.insert(std::make_pair(node, nullptr)); + if (!node_insertion.second) { + // Node already exists. + return false; + } + + if (!z_check(data_->session_)) { + return false; + } + + // Create the entity. + z_session_t session = z_loan(data_->session_); + const size_t node_id = this->data_->next_entity_id_++; + auto entity = rmw_zenoh_cpp::liveliness::Entity::make( + z_info_zid(session), + std::to_string(node_id), + std::to_string(node_id), + rmw_zenoh_cpp::liveliness::EntityType::Node, + rmw_zenoh_cpp::liveliness::NodeInfo{ + data_->domain_id_, + ns, + node_name, + data_->enclave_ + } + ); + if (entity == nullptr) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Unable to create node entity."); + return false; + } + + // Create the liveliness token. + zc_owned_liveliness_token_t token = zc_liveliness_declare_token( + session, + z_keyexpr(entity->liveliness_keyexpr().c_str()), + NULL + ); + auto free_token = rcpputils::make_scope_exit( + [&token]() { + z_drop(z_move(token)); + }); + if (!z_check(token)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Unable to create liveliness token for the node."); + return false; + } + free_token.cancel(); + + node_insertion.first->second = std::shared_ptr( + new rmw_zenoh_cpp::NodeData( + std::move(node_id), + std::move(entity), + std::move(token) + ) + ); + + return true; +} + +///============================================================================= +std::shared_ptr rmw_context_impl_s::get_node_data( + const rmw_node_t * const node) +{ + std::lock_guard lock(data_->mutex_); + auto node_it = data_->nodes_.find(node); + if (node_it == data_->nodes_.end()) { + return nullptr; + } + return node_it->second; +} + +///============================================================================= +void rmw_context_impl_s::delete_node_data(const rmw_node_t * const node) +{ + std::lock_guard lock(data_->mutex_); + data_->nodes_.erase(node); +} diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp index 3cb6f474..2f1c5654 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp @@ -22,10 +22,11 @@ #include #include #include +#include #include "graph_cache.hpp" -#include "guard_condition.hpp" #include "liveliness_utils.hpp" +#include "rmw_node_data.hpp" #include "rcutils/types.h" #include "rmw/rmw.h" @@ -34,7 +35,7 @@ class rmw_context_impl_s final { public: - // Constructor that internally initializees the Zenoh session and other artifacts. + // Constructor that internally initializes the Zenoh session and other artifacts. // Throws an std::runtime_error if any of the initializations fail. // The construction will block until a Zenoh router is detected. // TODO(Yadunund): Make this a non-blocking call by checking for the Zenoh @@ -62,7 +63,7 @@ class rmw_context_impl_s final rmw_guard_condition_t * graph_guard_condition(); // Get a unique id for a new entity. - size_t get_next_entity_id(); + std::size_t get_next_entity_id(); // Shutdown the Zenoh session. rmw_ret_t shutdown(); @@ -76,6 +77,21 @@ class rmw_context_impl_s final /// Return a shared_ptr to the GraphCache stored in this context. std::shared_ptr graph_cache(); + /// Create a NodeData and store it within this context. The NodeData can be + /// retrieved using get_node(). + /// Returns false if parameters are invalid. + bool create_node_data( + const rmw_node_t * const node, + const std::string & ns, + const std::string & node_name); + + /// Retrieve the NodeData for a given rmw_node_t if present. + std::shared_ptr get_node_data( + const rmw_node_t * const node); + + /// Delete the NodeData for a given rmw_node_t if present. + void delete_node_data(const rmw_node_t * const node); + private: // Bundle all class members into a data struct which can be passed as a // weak ptr to various threads for thread-safe access without capturing @@ -84,6 +100,7 @@ class rmw_context_impl_s final { // Constructor. Data( + std::size_t domain_id, const std::string & enclave, z_owned_session_t session, std::optional shm_manager, @@ -105,6 +122,8 @@ class rmw_context_impl_s final const rcutils_allocator_t * allocator_; // Enclave, name used to find security artifacts in a sros2 keystore. std::string enclave_; + // The ROS domain id of this context. + std::size_t domain_id_; // An owned session. z_owned_session_t session_; // An optional SHM manager that is initialized of SHM is enabled in the @@ -124,9 +143,11 @@ class rmw_context_impl_s final // Shutdown flag. bool is_shutdown_; // A counter to assign a local id for every entity created in this session. - size_t next_entity_id_; + std::size_t next_entity_id_; // True once graph subscriber is initialized. bool is_initialized_; + // Nodes created from this context. + std::unordered_map> nodes_; }; std::shared_ptr data_{nullptr}; diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp index 5ed480e7..e5097988 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp @@ -44,20 +44,6 @@ namespace rmw_zenoh_cpp { -///============================================================================= -struct rmw_node_data_t -{ - // The Entity generated for the node. - std::shared_ptr entity; - - // Liveliness token for the node. - zc_owned_liveliness_token_t token; - - // The entity id of this node as generated by get_next_entity_id(). - // Every interface created by this node will include this id in its liveliness token. - size_t id; -}; - ///============================================================================= class rmw_publisher_data_t final { diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp new file mode 100644 index 00000000..9096c0d1 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp @@ -0,0 +1,49 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_node_data.hpp" + +#include + +#include "logging_macros.hpp" + +#include "rcpputils/scope_exit.hpp" + +namespace rmw_zenoh_cpp +{ +///============================================================================= +NodeData::NodeData( + std::size_t id, + std::shared_ptr entity, + zc_owned_liveliness_token_t token) +: id_(std::move(id)), + entity_(std::move(entity)), + token_(std::move(token)) +{ + // Do nothing. +} + +///============================================================================= +NodeData::~NodeData() +{ + zc_liveliness_undeclare_token(z_move(token_)); +} + +///============================================================================= +std::size_t NodeData::id() const +{ + std::lock_guard lock(mutex_); + return id_; +} +} // namespace rmw_zenoh_cpp diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp new file mode 100644 index 00000000..3d49f471 --- /dev/null +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp @@ -0,0 +1,64 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DETAIL__RMW_NODE_DATA_HPP_ +#define DETAIL__RMW_NODE_DATA_HPP_ + +#include + +#include +#include +#include + +#include "liveliness_utils.hpp" + +// Forward declare rmw_context_impl_s. +class rmw_context_impl_s; + +namespace rmw_zenoh_cpp +{ +///============================================================================= +// The NodeData can only be created via rmw_context_impl_s::create_node_data(). +class NodeData +{ +public: + // Get the id of this node. + std::size_t id() const; + + // Destructor. + ~NodeData(); + +private: + // Make rmw_context_impl_s a friend so that it can access the constructor. + friend class rmw_context_impl_s; + + // Constructor. + NodeData( + std::size_t id, + std::shared_ptr entity, + zc_owned_liveliness_token_t token); + + // Internal mutex. + mutable std::mutex mutex_; + // The entity id of this node as generated by get_next_entity_id(). + // Every interface created by this node will include this id in its liveliness token. + std::size_t id_; + // The Entity generated for the node. + std::shared_ptr entity_; + // Liveliness token for the node. + zc_owned_liveliness_token_t token_; +}; +} // namespace rmw_zenoh_cpp + +#endif // DETAIL__RMW_NODE_DATA_HPP_ diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 7bffef42..aa445ef4 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -167,7 +167,12 @@ rmw_create_node( context->impl, "expected initialized context", return nullptr); - if (context->impl->is_shutdown()) { + rmw_context_impl_t * context_impl = static_cast(context->impl); + RMW_CHECK_FOR_NULL_WITH_MSG( + context_impl, + "expected initialized context_impl", + return nullptr); + if (context_impl->is_shutdown()) { RCUTILS_SET_ERROR_MSG("context has been shutdown"); return nullptr; } @@ -227,67 +232,18 @@ rmw_create_node( allocator->deallocate(const_cast(node->namespace_), allocator->state); }); - // Put metadata into node->data. - auto node_data = static_cast( - allocator->allocate(sizeof(rmw_zenoh_cpp::rmw_node_data_t), allocator->state)); - RMW_CHECK_FOR_NULL_WITH_MSG( - node_data, - "failed to allocate memory for node data", - return nullptr); - auto free_node_data = rcpputils::make_scope_exit( - [node_data, allocator]() { - allocator->deallocate(node_data, allocator->state); - }); - - RMW_TRY_PLACEMENT_NEW( - node_data, node_data, return nullptr, - rmw_zenoh_cpp::rmw_node_data_t); - auto destruct_node_data = rcpputils::make_scope_exit( - [node_data]() { - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE( - node_data->~rmw_node_data_t(), rmw_zenoh_cpp::rmw_node_data_t); - }); - - // Initialize liveliness token for the node to advertise that a new node is in town. - node_data->id = context->impl->get_next_entity_id(); - z_session_t session = context->impl->session(); - node_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(session), - std::to_string(node_data->id), - std::to_string(node_data->id), - rmw_zenoh_cpp::liveliness::EntityType::Node, - rmw_zenoh_cpp::liveliness::NodeInfo{context->actual_domain_id, namespace_, name, - context->impl->enclave()}); - if (node_data->entity == nullptr) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to generate keyexpr for liveliness token for the node %s.", - name); - return nullptr; - } - node_data->token = zc_liveliness_declare_token( - session, - z_keyexpr(node_data->entity->liveliness_keyexpr().c_str()), - NULL - ); - auto free_token = rcpputils::make_scope_exit( - [node_data]() { - z_drop(z_move(node_data->token)); - }); - if (!z_check(node_data->token)) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to create liveliness token for the node."); + // Create the NodeData for this node. + if (!context_impl->create_node_data(node, namespace_, name)) { + // Error already set. return nullptr; } node->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; node->context = context; - node->data = node_data; + // Store type erased rmw_context_impl_s in node->data so that the NodeData + // can be safely accessed. + node->data = context->impl; - free_token.cancel(); - free_node_data.cancel(); - destruct_node_data.cancel(); free_namespace.cancel(); free_name.cancel(); free_node.cancel(); @@ -313,14 +269,15 @@ rmw_destroy_node(rmw_node_t * node) // Undeclare liveliness token for the node to advertise that the node has ridden // off into the sunset. - rmw_zenoh_cpp::rmw_node_data_t * node_data = - static_cast(node->data); - if (node_data != nullptr) { - zc_liveliness_undeclare_token(z_move(node_data->token)); - RMW_TRY_DESTRUCTOR(node_data->~rmw_node_data_t(), rmw_node_data_t, ); - allocator->deallocate(node_data, allocator->state); + rmw_context_impl_s * context_impl = + static_cast(node->data); + if (context_impl == nullptr) { + RMW_SET_ERROR_MSG("Unable to cast node->data into rmw_context_impl_s."); + return RMW_RET_ERROR; } + context_impl->delete_node_data(node); + allocator->deallocate(const_cast(node->namespace_), allocator->state); allocator->deallocate(const_cast(node->name), allocator->state); allocator->deallocate(node, allocator->state); @@ -431,9 +388,6 @@ rmw_create_publisher( "Strict requirement on unique network flow endpoints for publishers not supported"); return nullptr; } - const rmw_zenoh_cpp::rmw_node_data_t * node_data = - static_cast(node->data); - RMW_CHECK_ARGUMENT_FOR_NULL(node_data, nullptr); // Get the RMW type support. const rosidl_message_type_support_t * type_support = find_message_type_support(type_supports); @@ -567,10 +521,14 @@ rmw_create_publisher( z_session_t session = context_impl->session(); const z_id_t zid = z_info_zid(session); - + auto node_data = context_impl->get_node_data(node); + RMW_CHECK_FOR_NULL_WITH_MSG( + node_data, + "NodeData not found.", + return nullptr); publisher_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( zid, - std::to_string(node_data->id), + std::to_string(node_data->id()), std::to_string( context_impl->get_next_entity_id()), rmw_zenoh_cpp::liveliness::EntityType::Publisher, @@ -1285,10 +1243,6 @@ rmw_create_subscription( } RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - auto node_data = static_cast(node->data); - RMW_CHECK_FOR_NULL_WITH_MSG( - node_data, "unable to create subscription as node_data is invalid.", - return nullptr); // TODO(yadunund): Check if a duplicate entry for the same topic name + topic type // is present in node_data->subscriptions and if so return error; RMW_CHECK_FOR_NULL_WITH_MSG( @@ -1415,12 +1369,16 @@ rmw_create_subscription( }); z_session_t session = context_impl->session(); - + auto node_data = context_impl->get_node_data(node); + RMW_CHECK_FOR_NULL_WITH_MSG( + node_data, + "NodeData not found.", + return nullptr); // Everything above succeeded and is setup properly. Now declare a subscriber // with Zenoh; after this, callbacks may come in at any time. sub_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(session), - std::to_string(node_data->id), + std::to_string(node_data->id()), std::to_string( context_impl->get_next_entity_id()), rmw_zenoh_cpp::liveliness::EntityType::Subscription, @@ -2106,14 +2064,6 @@ rmw_create_client( RMW_SET_ERROR_MSG("zenoh session is invalid"); return nullptr; } - RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - const rmw_zenoh_cpp::rmw_node_data_t * node_data = - static_cast(node->data); - if (node_data == nullptr) { - RMW_SET_ERROR_MSG( - "Unable to create client as node data is invalid."); - return nullptr; - } rcutils_allocator_t * allocator = &node->context->options.allocator; @@ -2288,9 +2238,14 @@ rmw_create_client( }); z_session_t session = context_impl->session(); + auto node_data = context_impl->get_node_data(node); + RMW_CHECK_FOR_NULL_WITH_MSG( + node_data, + "NodeData not found.", + return nullptr); client_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(session), - std::to_string(node_data->id), + std::to_string(node_data->id()), std::to_string( context_impl->get_next_entity_id()), rmw_zenoh_cpp::liveliness::EntityType::Client, @@ -2680,14 +2635,6 @@ rmw_create_service( return nullptr; } } - RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr); - const rmw_zenoh_cpp::rmw_node_data_t * node_data = - static_cast(node->data); - if (node_data == nullptr) { - RMW_SET_ERROR_MSG( - "Unable to create service as node data is invalid."); - return nullptr; - } RMW_CHECK_FOR_NULL_WITH_MSG( node->context, "expected initialized context", @@ -2862,10 +2809,14 @@ rmw_create_service( }); z_session_t session = context_impl->session(); - + auto node_data = context_impl->get_node_data(node); + RMW_CHECK_FOR_NULL_WITH_MSG( + node_data, + "NodeData not found.", + return nullptr); service_data->entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(session), - std::to_string(node_data->id), + std::to_string(node_data->id()), std::to_string( context_impl->get_next_entity_id()), rmw_zenoh_cpp::liveliness::EntityType::Service, From acf4a81cc1ccd62b5235539844c1f7c35ce06d29 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 4 Sep 2024 20:26:00 +0800 Subject: [PATCH 2/7] Make NodeData constructor public Signed-off-by: Yadunund --- .../src/detail/rmw_context_impl_s.cpp | 18 ++++++++---------- rmw_zenoh_cpp/src/detail/rmw_node_data.hpp | 18 ++++++------------ 2 files changed, 14 insertions(+), 22 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index 166e5bab..7cef3527 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -412,10 +412,10 @@ bool rmw_context_impl_s::create_node_data( std::to_string(node_id), rmw_zenoh_cpp::liveliness::EntityType::Node, rmw_zenoh_cpp::liveliness::NodeInfo{ - data_->domain_id_, - ns, - node_name, - data_->enclave_ + data_->domain_id_, + ns, + node_name, + data_->enclave_ } ); if (entity == nullptr) { @@ -443,12 +443,10 @@ bool rmw_context_impl_s::create_node_data( } free_token.cancel(); - node_insertion.first->second = std::shared_ptr( - new rmw_zenoh_cpp::NodeData( - std::move(node_id), - std::move(entity), - std::move(token) - ) + node_insertion.first->second = std::make_shared( + std::move(node_id), + std::move(entity), + std::move(token) ); return true; diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp index 3d49f471..f4c7f56d 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp @@ -23,9 +23,6 @@ #include "liveliness_utils.hpp" -// Forward declare rmw_context_impl_s. -class rmw_context_impl_s; - namespace rmw_zenoh_cpp { ///============================================================================= @@ -33,6 +30,12 @@ namespace rmw_zenoh_cpp class NodeData { public: + // Constructor. + NodeData( + std::size_t id, + std::shared_ptr entity, + zc_owned_liveliness_token_t token); + // Get the id of this node. std::size_t id() const; @@ -40,15 +43,6 @@ class NodeData ~NodeData(); private: - // Make rmw_context_impl_s a friend so that it can access the constructor. - friend class rmw_context_impl_s; - - // Constructor. - NodeData( - std::size_t id, - std::shared_ptr entity, - zc_owned_liveliness_token_t token); - // Internal mutex. mutable std::mutex mutex_; // The entity id of this node as generated by get_next_entity_id(). From 9435cd3affe227da8d0ea2b5d4e5e685537c5cfb Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 6 Sep 2024 14:48:25 +0800 Subject: [PATCH 3/7] Switch to recursive_mutex in rmw_context_impl_s Signed-off-by: Yadunund --- .../src/detail/rmw_context_impl_s.cpp | 32 +++++++++---------- .../src/detail/rmw_context_impl_s.hpp | 2 +- rmw_zenoh_cpp/src/detail/rmw_node_data.hpp | 3 +- 3 files changed, 18 insertions(+), 19 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index 7cef3527..f36f3ce5 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -54,7 +54,7 @@ void rmw_context_impl_s::graph_sub_data_handler(const z_sample_t * sample, void } // Update the graph cache. - std::lock_guard lock(data_ptr->mutex_); + std::lock_guard lock(data_ptr->mutex_); if (data_ptr->is_shutdown_) { return; } @@ -106,7 +106,7 @@ rmw_context_impl_s::Data::Data( ///============================================================================= rmw_ret_t rmw_context_impl_s::Data::subscribe_to_ros_graph() { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); if (is_initialized_) { return RMW_RET_OK; } @@ -148,7 +148,7 @@ rmw_ret_t rmw_context_impl_s::Data::subscribe_to_ros_graph() ///============================================================================= rmw_ret_t rmw_context_impl_s::Data::shutdown() { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); if (is_shutdown_) { return RMW_RET_OK; } @@ -327,35 +327,35 @@ rmw_context_impl_s::rmw_context_impl_s( ///============================================================================= std::string rmw_context_impl_s::enclave() const { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); return data_->enclave_; } ///============================================================================= z_session_t rmw_context_impl_s::session() const { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); return z_loan(data_->session_); } ///============================================================================= std::optional & rmw_context_impl_s::shm_manager() { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); return data_->shm_manager_; } ///============================================================================= rmw_guard_condition_t * rmw_context_impl_s::graph_guard_condition() { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); return data_->graph_guard_condition_.get(); } ///============================================================================= std::size_t rmw_context_impl_s::get_next_entity_id() { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); return data_->next_entity_id_++; } @@ -368,21 +368,21 @@ rmw_ret_t rmw_context_impl_s::shutdown() ///============================================================================= bool rmw_context_impl_s::is_shutdown() const { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); return data_->is_shutdown_; } ///============================================================================= bool rmw_context_impl_s::session_is_valid() const { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); return z_check(data_->session_); } ///============================================================================= std::shared_ptr rmw_context_impl_s::graph_cache() { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); return data_->graph_cache_; } @@ -392,7 +392,7 @@ bool rmw_context_impl_s::create_node_data( const std::string & ns, const std::string & node_name) { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); auto node_insertion = data_->nodes_.insert(std::make_pair(node, nullptr)); if (!node_insertion.second) { // Node already exists. @@ -405,7 +405,7 @@ bool rmw_context_impl_s::create_node_data( // Create the entity. z_session_t session = z_loan(data_->session_); - const size_t node_id = this->data_->next_entity_id_++; + const size_t node_id = this->get_next_entity_id(); auto entity = rmw_zenoh_cpp::liveliness::Entity::make( z_info_zid(session), std::to_string(node_id), @@ -416,7 +416,7 @@ bool rmw_context_impl_s::create_node_data( ns, node_name, data_->enclave_ - } + } ); if (entity == nullptr) { RMW_ZENOH_LOG_ERROR_NAMED( @@ -456,7 +456,7 @@ bool rmw_context_impl_s::create_node_data( std::shared_ptr rmw_context_impl_s::get_node_data( const rmw_node_t * const node) { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); auto node_it = data_->nodes_.find(node); if (node_it == data_->nodes_.end()) { return nullptr; @@ -467,6 +467,6 @@ std::shared_ptr rmw_context_impl_s::get_node_data( ///============================================================================= void rmw_context_impl_s::delete_node_data(const rmw_node_t * const node) { - std::lock_guard lock(data_->mutex_); + std::lock_guard lock(data_->mutex_); data_->nodes_.erase(node); } diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp index 2f1c5654..1476091a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp @@ -117,7 +117,7 @@ class rmw_context_impl_s final ~Data(); // Mutex to lock when accessing members. - mutable std::mutex mutex_; + mutable std::recursive_mutex mutex_; // RMW allocator. const rcutils_allocator_t * allocator_; // Enclave, name used to find security artifacts in a sros2 keystore. diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp index f4c7f56d..fde2840a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp @@ -19,7 +19,6 @@ #include #include -#include #include "liveliness_utils.hpp" @@ -27,7 +26,7 @@ namespace rmw_zenoh_cpp { ///============================================================================= // The NodeData can only be created via rmw_context_impl_s::create_node_data(). -class NodeData +class NodeData final { public: // Constructor. From 03511e26b560eab5fab71d23f1afdb3b6cc1d242 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 6 Sep 2024 15:34:03 +0800 Subject: [PATCH 4/7] NodeData API to construct liveliness token Signed-off-by: Yadunund --- .../src/detail/rmw_context_impl_s.cpp | 50 ++++------------ rmw_zenoh_cpp/src/detail/rmw_node_data.cpp | 57 +++++++++++++++++++ rmw_zenoh_cpp/src/detail/rmw_node_data.hpp | 17 ++++-- 3 files changed, 81 insertions(+), 43 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index f36f3ce5..6d6b2738 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -399,55 +399,27 @@ bool rmw_context_impl_s::create_node_data( return false; } + // Check that the Zenoh session is still valid. if (!z_check(data_->session_)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Unable to create NodeData as Zenoh session is invalid."); return false; } - // Create the entity. - z_session_t session = z_loan(data_->session_); - const size_t node_id = this->get_next_entity_id(); - auto entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(session), - std::to_string(node_id), - std::to_string(node_id), - rmw_zenoh_cpp::liveliness::EntityType::Node, - rmw_zenoh_cpp::liveliness::NodeInfo{ + auto node_data = rmw_zenoh_cpp::NodeData::make( + this->get_next_entity_id(), + z_loan(data_->session_), data_->domain_id_, ns, node_name, - data_->enclave_ - } - ); - if (entity == nullptr) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to create node entity."); - return false; - } - - // Create the liveliness token. - zc_owned_liveliness_token_t token = zc_liveliness_declare_token( - session, - z_keyexpr(entity->liveliness_keyexpr().c_str()), - NULL - ); - auto free_token = rcpputils::make_scope_exit( - [&token]() { - z_drop(z_move(token)); - }); - if (!z_check(token)) { - RMW_ZENOH_LOG_ERROR_NAMED( - "rmw_zenoh_cpp", - "Unable to create liveliness token for the node."); + data_->enclave_); + if (node_data == nullptr) { + // Error already handled. return false; } - free_token.cancel(); - node_insertion.first->second = std::make_shared( - std::move(node_id), - std::move(entity), - std::move(token) - ); + node_insertion.first->second = std::move(node_data); return true; } diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp index 9096c0d1..cbfd0314 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp @@ -14,6 +14,8 @@ #include "rmw_node_data.hpp" +#include +#include #include #include "logging_macros.hpp" @@ -22,6 +24,61 @@ namespace rmw_zenoh_cpp { +///============================================================================= +std::shared_ptr NodeData::make( + std::size_t id, + z_session_t session, + std::size_t domain_id, + const std::string & namespace_, + const std::string & node_name, + const std::string & enclave) +{ + // Create the entity. + auto entity = rmw_zenoh_cpp::liveliness::Entity::make( + z_info_zid(session), + std::to_string(id), + std::to_string(id), + rmw_zenoh_cpp::liveliness::EntityType::Node, + rmw_zenoh_cpp::liveliness::NodeInfo{ + domain_id, + namespace_, + node_name, + enclave + } + ); + if (entity == nullptr) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Unable to make NodeData as node entity is invalid."); + return nullptr; + } + + // Create the liveliness token. + zc_owned_liveliness_token_t token = zc_liveliness_declare_token( + session, + z_keyexpr(entity->liveliness_keyexpr().c_str()), + NULL + ); + auto free_token = rcpputils::make_scope_exit( + [&token]() { + z_drop(z_move(token)); + }); + if (!z_check(token)) { + RMW_ZENOH_LOG_ERROR_NAMED( + "rmw_zenoh_cpp", + "Unable to create liveliness token for the node."); + return nullptr; + } + free_token.cancel(); + + return std::shared_ptr( + new NodeData{ + id, + std::move(entity), + std::move(token) + }); +} + ///============================================================================= NodeData::NodeData( std::size_t id, diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp index fde2840a..737c19b0 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp @@ -19,6 +19,7 @@ #include #include +#include #include "liveliness_utils.hpp" @@ -29,11 +30,14 @@ namespace rmw_zenoh_cpp class NodeData final { public: - // Constructor. - NodeData( + // Make a shared_ptr of NodeData. Returns nullptr if construction fails. + static std::shared_ptr make( std::size_t id, - std::shared_ptr entity, - zc_owned_liveliness_token_t token); + z_session_t session, + std::size_t domain_id, + const std::string & namespace_, + const std::string & node_name, + const std::string & enclave); // Get the id of this node. std::size_t id() const; @@ -42,6 +46,11 @@ class NodeData final ~NodeData(); private: + // Constructor. + NodeData( + std::size_t id, + std::shared_ptr entity, + zc_owned_liveliness_token_t token); // Internal mutex. mutable std::mutex mutex_; // The entity id of this node as generated by get_next_entity_id(). From 24162935bd8e417fd76c48c13c37e0bf69b62ddd Mon Sep 17 00:00:00 2001 From: Yadunund Date: Fri, 6 Sep 2024 15:36:08 +0800 Subject: [PATCH 5/7] Use count to check if node exists Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp index 6d6b2738..875f7c09 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -393,8 +393,7 @@ bool rmw_context_impl_s::create_node_data( const std::string & node_name) { std::lock_guard lock(data_->mutex_); - auto node_insertion = data_->nodes_.insert(std::make_pair(node, nullptr)); - if (!node_insertion.second) { + if (data_->nodes_.count(node) > 0) { // Node already exists. return false; } @@ -419,7 +418,10 @@ bool rmw_context_impl_s::create_node_data( return false; } - node_insertion.first->second = std::move(node_data); + auto node_insertion = data_->nodes_.insert(std::make_pair(node, std::move(node_data))); + if (!node_insertion.second) { + return false; + } return true; } From 01fc6908e99d48dc88ba6f8d0026f06527521c00 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sat, 28 Sep 2024 02:36:47 +0800 Subject: [PATCH 6/7] Fix compilation after rebase Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp index 1476091a..884056e3 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp @@ -25,6 +25,7 @@ #include #include "graph_cache.hpp" +#include "guard_condition.hpp" #include "liveliness_utils.hpp" #include "rmw_node_data.hpp" From 9c100ac4e443f6f8cccec1e6671624ed29d88051 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 1 Oct 2024 00:45:45 +0800 Subject: [PATCH 7/7] Address feedback Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/rmw_node_data.cpp | 2 ++ rmw_zenoh_cpp/src/detail/rmw_node_data.hpp | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp index cbfd0314..d80bd275 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp @@ -14,7 +14,9 @@ #include "rmw_node_data.hpp" +#include #include +#include #include #include diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp index 737c19b0..8d11c34f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Open Source Robotics Foundation, Inc. +// Copyright 2024 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,6 +17,7 @@ #include +#include #include #include #include