From a3fdb1fab86a97933266656c1d9723117dffaf90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 19 Nov 2024 00:43:47 +0100 Subject: [PATCH] zenoh_c to zenoh_cpp 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/CMakeLists.txt | 4 +- rmw_zenoh_cpp/package.xml | 4 +- rmw_zenoh_cpp/src/detail/graph_cache.cpp | 5 +- rmw_zenoh_cpp/src/detail/graph_cache.hpp | 2 +- rmw_zenoh_cpp/src/detail/liveliness_utils.cpp | 35 ++++ rmw_zenoh_cpp/src/detail/liveliness_utils.hpp | 10 +- rmw_zenoh_cpp/src/detail/rmw_client_data.cpp | 6 +- rmw_zenoh_cpp/src/detail/rmw_client_data.hpp | 2 +- .../src/detail/rmw_context_impl_s.cpp | 187 ++++++++++-------- .../src/detail/rmw_context_impl_s.hpp | 1 + rmw_zenoh_cpp/src/detail/rmw_node_data.cpp | 22 +-- rmw_zenoh_cpp/src/detail/rmw_node_data.hpp | 12 +- .../src/detail/rmw_publisher_data.cpp | 10 +- .../src/detail/rmw_publisher_data.hpp | 4 +- rmw_zenoh_cpp/src/detail/rmw_service_data.cpp | 8 +- rmw_zenoh_cpp/src/detail/rmw_service_data.hpp | 2 +- .../src/detail/rmw_subscription_data.cpp | 4 +- .../src/detail/rmw_subscription_data.hpp | 2 +- rmw_zenoh_cpp/src/detail/zenoh_config.cpp | 24 ++- rmw_zenoh_cpp/src/detail/zenoh_config.hpp | 7 +- rmw_zenoh_cpp/src/rmw_zenoh.cpp | 8 +- rmw_zenoh_cpp/src/zenohd/main.cpp | 52 +++-- .../CMakeLists.txt | 15 +- .../package.xml | 2 +- 24 files changed, 250 insertions(+), 178 deletions(-) rename {zenoh_c_vendor => zenoh_cpp_vendor}/CMakeLists.txt (82%) rename {zenoh_c_vendor => zenoh_cpp_vendor}/package.xml (95%) diff --git a/rmw_zenoh_cpp/CMakeLists.txt b/rmw_zenoh_cpp/CMakeLists.txt index 89b5a598..31d541ad 100644 --- a/rmw_zenoh_cpp/CMakeLists.txt +++ b/rmw_zenoh_cpp/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(rcutils REQUIRED) find_package(rosidl_typesupport_fastrtps_c REQUIRED) find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) find_package(rmw REQUIRED) -find_package(zenoh_c_vendor REQUIRED) +find_package(zenoh_cpp_vendor REQUIRED) add_library(rmw_zenoh_cpp SHARED src/detail/attachment_helpers.cpp @@ -69,6 +69,7 @@ target_link_libraries(rmw_zenoh_cpp rosidl_typesupport_fastrtps_cpp::rosidl_typesupport_fastrtps_cpp rmw::rmw zenohc::lib + zenohcxx::zenohc ) configure_rmw_library(rmw_zenoh_cpp) @@ -131,6 +132,7 @@ target_link_libraries(rmw_zenohd rcpputils::rcpputils rmw::rmw zenohc::lib + zenohcxx::zenohc ) install( diff --git a/rmw_zenoh_cpp/package.xml b/rmw_zenoh_cpp/package.xml index e1d2a24a..79272665 100644 --- a/rmw_zenoh_cpp/package.xml +++ b/rmw_zenoh_cpp/package.xml @@ -15,8 +15,8 @@ ament_cmake - zenoh_c_vendor - zenoh_c_vendor + zenoh_cpp_vendor + zenoh_cpp_vendor ament_index_cpp fastcdr diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp index c60256ff..4b570e45 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.cpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.cpp @@ -68,8 +68,8 @@ TopicData::TopicData(ConstEntityPtr entity) } ///============================================================================= -GraphCache::GraphCache(const z_id_t & zid) -: zid_str_(liveliness::zid_to_str(zid)) +GraphCache::GraphCache(const std::string & zid) +: zid_str_(zid) { // Do nothing. } @@ -364,6 +364,7 @@ void GraphCache::parse_put( {entity->node_name(), node}}; graph_.emplace(std::make_pair(entity->node_namespace(), std::move(node_map))); update_topic_maps_for_put(node, entity); + total_nodes_in_graph_ += 1; return; } diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp index 158891de..46efb2f5 100644 --- a/rmw_zenoh_cpp/src/detail/graph_cache.hpp +++ b/rmw_zenoh_cpp/src/detail/graph_cache.hpp @@ -107,7 +107,7 @@ class GraphCache final /// @param id The id of the zenoh session that is building the graph cache. /// This is used to infer which entities originated from the current session /// so that appropriate event callbacks may be triggered. - explicit GraphCache(const z_id_t & zid); + explicit GraphCache(const std::string & zid); // Parse a PUT message over a token's key-expression and update the graph. void parse_put(const std::string & keyexpr, bool ignore_from_current_session = false); diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp index 45918d92..2265fbcd 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp @@ -452,6 +452,41 @@ Entity::Entity( } ///============================================================================= +std::shared_ptr Entity::make( + zenoh::Id zid, + const std::string & nid, + const std::string & id, + EntityType type, + NodeInfo node_info, + std::optional topic_info) +{ + if (id.empty()) { + RCUTILS_SET_ERROR_MSG("Invalid id."); + return nullptr; + } + if (entity_to_str.find(type) == entity_to_str.end()) { + RCUTILS_SET_ERROR_MSG("Invalid entity type."); + return nullptr; + } + if (node_info.ns_.empty() || node_info.name_.empty()) { + RCUTILS_SET_ERROR_MSG("Invalid node_info for entity."); + return nullptr; + } + if (type != EntityType::Node && !topic_info.has_value()) { + RCUTILS_SET_ERROR_MSG("Invalid topic_info for entity."); + return nullptr; + } + + return std::make_shared( + Entity{ + std::string(zid.to_string()), + nid, + id, + std::move(type), + std::move(node_info), + std::move(topic_info)}); +} + std::shared_ptr Entity::make( z_id_t zid, const std::string & nid, diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp index e0c7cf9c..fdd4a928 100644 --- a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp +++ b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp @@ -15,7 +15,7 @@ #ifndef DETAIL__LIVELINESS_UTILS_HPP_ #define DETAIL__LIVELINESS_UTILS_HPP_ -#include +#include #include #include @@ -125,6 +125,14 @@ class Entity /// @param node_info The node information that is required for all entities. /// @param topic_info An optional topic information for relevant entities. /// @return An entity if all inputs are valid. This way no invalid entities can be created. + static EntityPtr make( + zenoh::Id zid, + const std::string & nid, + const std::string & id, + EntityType type, + NodeInfo node_info, + std::optional topic_info = std::nullopt); + static EntityPtr make( z_id_t zid, const std::string & nid, diff --git a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp index ae7396ba..4efdb16f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp @@ -100,7 +100,7 @@ namespace rmw_zenoh_cpp { ///============================================================================= std::shared_ptr ClientData::make( - const z_loaned_session_t * session, + const std::shared_ptr & session, const rmw_node_t * const node, const rmw_client_t * client, liveliness::NodeInfo node_info, @@ -162,7 +162,7 @@ std::shared_ptr ClientData::make( std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( - z_info_zid(session), + session->get_zid(), std::to_string(node_id), std::to_string(service_id), liveliness::EntityType::Client, @@ -193,7 +193,7 @@ std::shared_ptr ClientData::make( response_type_support }); - if (!client_data->init(session)) { + if (!client_data->init(z_loan(session->_0))) { // init() already set the error. return nullptr; } diff --git a/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp index d6650677..4b3398a9 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_client_data.hpp @@ -40,7 +40,7 @@ class ClientData final : public std::enable_shared_from_this public: // Make a shared_ptr of ClientData. static std::shared_ptr make( - const z_loaned_session_t * session, + const std::shared_ptr & session, const rmw_node_t * const node, const rmw_client_t * client, liveliness::NodeInfo node_info, 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 7414b554..21868568 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.cpp @@ -77,33 +77,32 @@ class rmw_context_impl_s::Data final nodes_({}) { // Initialize the zenoh configuration. - z_owned_config_t config; - rmw_ret_t ret; - if ((ret = - rmw_zenoh_cpp::get_z_config( - rmw_zenoh_cpp::ConfigurableEntity::Session, - &config)) != RMW_RET_OK) - { + std::optional config = rmw_zenoh_cpp::get_z_config( + rmw_zenoh_cpp::ConfigurableEntity::Session); + + if (!config.has_value()) { throw std::runtime_error("Error configuring Zenoh session."); } - // Check if shm is enabled. - z_owned_string_t shm_enabled; - zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled); - auto always_free_shm_enabled = rcpputils::make_scope_exit( - [&shm_enabled]() { - z_drop(z_move(shm_enabled)); - }); + // // Check if shm is enabled. + // z_owned_string_t shm_enabled; + // zc_config_get_from_str(z_loan(config), Z_CONFIG_SHARED_MEMORY_KEY, &shm_enabled); + // auto always_free_shm_enabled = rcpputils::make_scope_exit( + // [&shm_enabled]() { + // z_drop(z_move(shm_enabled)); + // }); // Initialize the zenoh session. - if (z_open(&session_, z_move(config), NULL) != Z_OK) { - RMW_SET_ERROR_MSG("Error setting up zenoh session"); - throw std::runtime_error("Error setting up zenoh session."); + zenoh::ZResult result; + session_ = std::make_shared( + std::move(config.value()), + zenoh::Session::SessionOptions::create_default(), + &result); + if(result != Z_OK) { + throw std::runtime_error("Error setting up zenoh session. "); } - auto close_session = rcpputils::make_scope_exit( - [this]() { - z_close(z_loan_mut(session_), NULL); - }); + + rmw_ret_t ret; // TODO(Yadunund) Move this check into a separate thread. // Verify if the zenoh router is running if configured. @@ -114,8 +113,12 @@ class rmw_context_impl_s::Data final uint64_t connection_attempts = 0; // Retry until the connection is successful. while (ret != RMW_RET_OK && connection_attempts < configured_connection_attempts.value()) { - if ((ret = rmw_zenoh_cpp::zenoh_router_check(z_loan(session_))) != RMW_RET_OK) { + zenoh::ZResult err; + this->session_->get_routers_z_id(&err); + if (err != Z_OK) { ++connection_attempts; + } else { + ret = RMW_RET_OK; } std::this_thread::sleep_for(std::chrono::milliseconds(100)); } @@ -128,8 +131,7 @@ class rmw_context_impl_s::Data final } // Initialize the graph cache. - const z_id_t zid = z_info_zid(z_loan(session_)); - graph_cache_ = std::make_shared(zid); + graph_cache_ = std::make_shared(this->session_->get_zid().to_string()); // Setup liveliness subscriptions for discovery. std::string liveliness_str = rmw_zenoh_cpp::liveliness::subscription_token(domain_id); @@ -148,60 +150,62 @@ class rmw_context_impl_s::Data final // the code will be simpler, and if we're just going to spin over the non-blocking // reads until we obtain responses, we'll just be hogging CPU time by convincing // the OS that we're doing actual work when it could instead park the thread. - z_owned_fifo_handler_reply_t handler; - z_owned_closure_reply_t closure; - z_fifo_channel_reply_new(&closure, &handler, SIZE_MAX - 1); - - - z_view_keyexpr_t keyexpr; - z_view_keyexpr_from_str(&keyexpr, liveliness_str.c_str()); - zc_liveliness_get( - z_loan(session_), z_loan(keyexpr), - z_move(closure), NULL); - - z_owned_reply_t reply; - while (z_recv(z_loan(handler), &reply) == Z_OK) { - if (z_reply_is_ok(z_loan(reply))) { - const z_loaned_sample_t * sample = z_reply_ok(z_loan(reply)); - z_view_string_t keystr; - z_keyexpr_as_view_string(z_sample_keyexpr(sample), &keystr); - std::string str(z_string_data(z_loan(keystr)), z_string_len(z_loan(keystr))); - // Ignore tokens from the same session to avoid race conditions from this - // query and the liveliness subscription. - graph_cache_->parse_put(str, true); - } else { - RMW_ZENOH_LOG_DEBUG_NAMED( - "rmw_zenoh_cpp", "[rmw_context_impl_s] z_call received an invalid reply.\n"); - } - z_drop(z_move(reply)); + // z_owned_fifo_handler_reply_t handler; + // z_owned_closure_reply_t closure; + // z_fifo_channel_reply_new(&closure, &handler, SIZE_MAX - 1); + // Intuitively use a FIFO channel rather than building it up with a closure and a handler to FIFO channel + zenoh::KeyExpr keyexpr(liveliness_str); + + zenoh::Session::GetOptions options = zenoh::Session::GetOptions::create_default(); + options.target = zenoh::QueryTarget::Z_QUERY_TARGET_ALL; + options.payload = ""; + + zenoh::ZResult err; + auto replies = session_->liveliness_get( + keyexpr, + zenoh::channels::FifoChannel(SIZE_MAX - 1), + zenoh::Session::LivelinessGetOptions::create_default(), + &err); + + if (err != Z_OK) + { + throw std::runtime_error("Error getting liveliness. "); + } + + for (auto res = replies.recv(); std::holds_alternative(res); res = replies.recv()) { + + const zenoh::Reply &reply = std::get(res); + if (reply.is_ok()) { + const auto &sample = reply.get_ok(); + graph_cache_->parse_put(std::string(sample.get_keyexpr().as_string_view()), true); + } } - z_drop(z_move(handler)); // Initialize the shm manager if shared_memory is enabled in the config. shm_provider_ = std::nullopt; - if (strncmp( - z_string_data(z_loan(shm_enabled)), - "true", - z_string_len(z_loan(shm_enabled))) == 0) - { - // TODO(yuyuan): determine the default alignment of SHM - z_alloc_alignment_t alignment = {5}; - z_owned_memory_layout_t layout; - z_memory_layout_new(&layout, SHM_BUFFER_SIZE_MB * 1024 * 1024, alignment); - - z_owned_shm_provider_t provider; - if (z_posix_shm_provider_new(&provider, z_loan(layout)) != Z_OK) { - RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a SHM provider."); - throw std::runtime_error("Unable to create shm provider."); - } - shm_provider_ = provider; - } - auto free_shm_provider = rcpputils::make_scope_exit( - [this]() { - if (shm_provider_.has_value()) { - z_drop(z_move(shm_provider_.value())); - } - }); + // if (strncmp( + // z_string_data(z_loan(shm_enabled)), + // "true", + // z_string_len(z_loan(shm_enabled))) == 0) + // { + // // TODO(yuyuan): determine the default alignment of SHM + // z_alloc_alignment_t alignment = {5}; + // z_owned_memory_layout_t layout; + // z_memory_layout_new(&layout, SHM_BUFFER_SIZE_MB * 1024 * 1024, alignment); + + // z_owned_shm_provider_t provider; + // if (z_posix_shm_provider_new(&provider, z_loan(layout)) != Z_OK) { + // RMW_ZENOH_LOG_ERROR_NAMED("rmw_zenoh_cpp", "Unable to create a SHM provider."); + // throw std::runtime_error("Unable to create shm provider."); + // } + // shm_provider_ = provider; + // } + // auto free_shm_provider = rcpputils::make_scope_exit( + // [this]() { + // if (shm_provider_.has_value()) { + // z_drop(z_move(shm_provider_.value())); + // } + // }); graph_guard_condition_ = std::make_unique(); graph_guard_condition_->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier; @@ -220,7 +224,7 @@ class rmw_context_impl_s::Data final z_undeclare_subscriber(z_move(this->graph_subscriber_)); }); if (zc_liveliness_declare_subscriber( - z_loan(session_), + z_loan(session_->_0), &graph_subscriber_, z_loan(liveliness_ke), z_move(callback), &sub_options) != Z_OK) { @@ -228,8 +232,6 @@ class rmw_context_impl_s::Data final throw std::runtime_error("Unable to subscribe to ROS graph updates."); } - close_session.cancel(); - free_shm_provider.cancel(); undeclare_z_sub.cancel(); } @@ -266,11 +268,12 @@ class rmw_context_impl_s::Data final // to avoid an AB/BA deadlock if shutdown is racing with graph_sub_data_handler(). } - // Close the zenoh session - if (z_close(z_loan_mut(session_), NULL) != Z_OK) { - RMW_SET_ERROR_MSG("Error while closing zenoh session"); - return RMW_RET_ERROR; - } + // // Close the zenoh session + // if (z_close(z_loan_mut(session_), NULL) != Z_OK) { + // RMW_SET_ERROR_MSG("Error while closing zenoh session"); + // return RMW_RET_ERROR; + // } + session_.reset(); return RMW_RET_OK; } @@ -283,7 +286,13 @@ class rmw_context_impl_s::Data final const z_loaned_session_t * session() const { std::lock_guard lock(mutex_); - return z_loan(session_); + return z_loan(session_->_0); + } + + const std::shared_ptr session_cpp() const + { + std::lock_guard lock(mutex_); + return session_; } std::optional & shm_provider() @@ -313,7 +322,7 @@ class rmw_context_impl_s::Data final bool session_is_valid() const { std::lock_guard lock(mutex_); - return !z_session_is_closed(z_loan(session_)); + return !z_session_is_closed(z_loan(session_->_0)); } std::shared_ptr graph_cache() @@ -344,7 +353,7 @@ class rmw_context_impl_s::Data final auto node_data = rmw_zenoh_cpp::NodeData::make( node, this->get_next_entity_id(), - z_loan(session_), + session_cpp(), domain_id_, ns, node_name, @@ -421,7 +430,7 @@ class rmw_context_impl_s::Data final // Enclave, name used to find security artifacts in a sros2 keystore. std::string enclave_; // An owned session. - z_owned_session_t session_; + std::shared_ptr session_; // An optional SHM manager that is initialized of SHM is enabled in the // zenoh session config. std::optional shm_provider_; @@ -502,6 +511,12 @@ const z_loaned_session_t * rmw_context_impl_s::session() const return data_->session(); } +///============================================================================= +const std::shared_ptr rmw_context_impl_s::session_cpp() const +{ +return data_->session_cpp(); +} + ///============================================================================= std::optional & rmw_context_impl_s::shm_provider() { 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 1e46d6af..08173be8 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_context_impl_s.hpp @@ -50,6 +50,7 @@ class rmw_context_impl_s final // Loan the Zenoh session. const z_loaned_session_t * session() const; + const std::shared_ptr session_cpp() const; // Get a reference to the shm_provider. // Note: This is not thread-safe. // TODO(Yadunund): Remove this API and instead include a publish() API diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp index 2f7187b6..10f3cd1a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.cpp @@ -30,7 +30,7 @@ namespace rmw_zenoh_cpp std::shared_ptr NodeData::make( const rmw_node_t * const node, std::size_t id, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::size_t domain_id, const std::string & namespace_, const std::string & node_name, @@ -38,7 +38,7 @@ std::shared_ptr NodeData::make( { // Create the entity. auto entity = rmw_zenoh_cpp::liveliness::Entity::make( - z_info_zid(session), + session->get_zid(), std::to_string(id), std::to_string(id), rmw_zenoh_cpp::liveliness::EntityType::Node, @@ -65,7 +65,7 @@ std::shared_ptr NodeData::make( [&token]() { z_drop(z_move(token)); }); - if (zc_liveliness_declare_token(session, &token, z_loan(liveliness_ke), NULL) != Z_OK) { + if (zc_liveliness_declare_token(z_loan(session->_0), &token, z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Unable to create liveliness token for the node."); @@ -121,7 +121,7 @@ std::size_t NodeData::id() const ///============================================================================= bool NodeData::create_pub_data( const rmw_publisher_t * const publisher, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::size_t id, const std::string & topic_name, const rosidl_message_type_support_t * type_support, @@ -143,7 +143,7 @@ bool NodeData::create_pub_data( } auto pub_data = PublisherData::make( - std::move(session), + session, node_, entity_->node_info(), id_, @@ -187,7 +187,7 @@ void NodeData::delete_pub_data(const rmw_publisher_t * const publisher) ///============================================================================= bool NodeData::create_sub_data( const rmw_subscription_t * const subscription, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::shared_ptr graph_cache, std::size_t id, const std::string & topic_name, @@ -210,7 +210,7 @@ bool NodeData::create_sub_data( } auto sub_data = SubscriptionData::make( - std::move(session), + session, std::move(graph_cache), node_, entity_->node_info(), @@ -255,7 +255,7 @@ void NodeData::delete_sub_data(const rmw_subscription_t * const subscription) ///============================================================================= bool NodeData::create_service_data( const rmw_service_t * const service, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::size_t id, const std::string & service_name, const rosidl_service_type_support_t * type_supports, @@ -277,7 +277,7 @@ bool NodeData::create_service_data( } auto service_data = ServiceData::make( - std::move(session), + session, node_, entity_->node_info(), id_, @@ -322,7 +322,7 @@ void NodeData::delete_service_data(const rmw_service_t * const service) ///============================================================================= bool NodeData::create_client_data( const rmw_client_t * const client, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::size_t id, const std::string & service_name, const rosidl_service_type_support_t * type_supports, @@ -344,7 +344,7 @@ bool NodeData::create_client_data( } auto client_data = ClientData::make( - std::move(session), + session, node_, client, entity_->node_info(), diff --git a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp index 0ad3bb6e..ecf53718 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_node_data.hpp @@ -15,7 +15,7 @@ #ifndef DETAIL__RMW_NODE_DATA_HPP_ #define DETAIL__RMW_NODE_DATA_HPP_ -#include +#include #include #include @@ -41,7 +41,7 @@ class NodeData final static std::shared_ptr make( const rmw_node_t * const node, std::size_t id, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::size_t domain_id, const std::string & namespace_, const std::string & node_name, @@ -53,7 +53,7 @@ class NodeData final // Create a new PublisherData for a given rmw_publisher_t. bool create_pub_data( const rmw_publisher_t * const publisher, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::size_t id, const std::string & topic_name, const rosidl_message_type_support_t * type_support, @@ -68,7 +68,7 @@ class NodeData final // Create a new SubscriptionData for a given rmw_subscription_t. bool create_sub_data( const rmw_subscription_t * const subscription, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::shared_ptr graph_cache, std::size_t id, const std::string & topic_name, @@ -84,7 +84,7 @@ class NodeData final // Create a new ServiceData for a given rmw_service_t. bool create_service_data( const rmw_service_t * const service, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::size_t id, const std::string & service_name, const rosidl_service_type_support_t * type_support, @@ -99,7 +99,7 @@ class NodeData final // Create a new ClientData for a given rmw_client_t. bool create_client_data( const rmw_client_t * const client, - const z_loaned_session_t * session, + const std::shared_ptr & session, std::size_t id, const std::string & service_name, const rosidl_service_type_support_t * type_support, diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp index 6f2a9284..092df1b2 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp @@ -39,7 +39,7 @@ namespace rmw_zenoh_cpp ///============================================================================= std::shared_ptr PublisherData::make( - const z_loaned_session_t * session, + const std::shared_ptr & session, const rmw_node_t * const node, liveliness::NodeInfo node_info, std::size_t node_id, @@ -79,7 +79,7 @@ std::shared_ptr PublisherData::make( std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( - z_info_zid(session), + session->get_zid(), std::to_string(node_id), std::to_string(publisher_id), liveliness::EntityType::Publisher, @@ -132,7 +132,7 @@ std::shared_ptr PublisherData::make( ze_owned_publication_cache_t pub_cache_; if (ze_declare_publication_cache( - session, &pub_cache_, z_loan(pub_ke), &pub_cache_opts)) + z_loan(session->_0), &pub_cache_, z_loan(pub_ke), &pub_cache_opts)) { RMW_SET_ERROR_MSG("unable to create zenoh publisher cache"); return nullptr; @@ -159,7 +159,7 @@ std::shared_ptr PublisherData::make( z_undeclare_publisher(z_move(pub)); }); if (z_declare_publisher( - session, &pub, z_loan(pub_ke), &opts) != Z_OK) + z_loan(session->_0), &pub, z_loan(pub_ke), &opts) != Z_OK) { RMW_SET_ERROR_MSG("Unable to create Zenoh publisher."); return nullptr; @@ -174,7 +174,7 @@ std::shared_ptr PublisherData::make( z_drop(z_move(token)); }); if (zc_liveliness_declare_token( - session, &token, z_loan(liveliness_ke), + z_loan(session->_0), &token, z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp index 22489298..1893360b 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp @@ -15,7 +15,7 @@ #ifndef DETAIL__RMW_PUBLISHER_DATA_HPP_ #define DETAIL__RMW_PUBLISHER_DATA_HPP_ -#include +#include #include #include @@ -38,7 +38,7 @@ class PublisherData final public: // Make a shared_ptr of PublisherData. static std::shared_ptr make( - const z_loaned_session_t * session, + const std::shared_ptr & session, const rmw_node_t * const node, liveliness::NodeInfo node_info, std::size_t node_id, diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp index 5cb53d58..a32e4e11 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp @@ -59,7 +59,7 @@ void service_data_handler(z_loaned_query_t * query, void * data) ///============================================================================= std::shared_ptr ServiceData::make( - const z_loaned_session_t * session, + const std::shared_ptr & session, const rmw_node_t * const node, liveliness::NodeInfo node_info, std::size_t node_id, @@ -120,7 +120,7 @@ std::shared_ptr ServiceData::make( std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( - z_info_zid(session), + session->get_zid(), std::to_string(node_id), std::to_string(service_id), liveliness::EntityType::Service, @@ -170,7 +170,7 @@ std::shared_ptr ServiceData::make( z_undeclare_queryable(z_move(service_data->qable_)); }); if (z_declare_queryable( - session, &service_data->qable_, z_loan(service_ke), + z_loan(session->_0), &service_data->qable_, z_loan(service_ke), z_move(callback), &qable_options) != Z_OK) { RMW_SET_ERROR_MSG("unable to create zenoh queryable"); @@ -190,7 +190,7 @@ std::shared_ptr ServiceData::make( } }); if (zc_liveliness_declare_token( - session, &service_data->token_, z_loan(liveliness_ke), + z_loan(session->_0), &service_data->token_, z_loan(liveliness_ke), NULL) != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.hpp index a8f9d925..7ba3d276 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_service_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_service_data.hpp @@ -41,7 +41,7 @@ class ServiceData final public: // Make a shared_ptr of ServiceData. static std::shared_ptr make( - const z_loaned_session_t * session, + const std::shared_ptr & session, const rmw_node_t * const node, liveliness::NodeInfo node_info, std::size_t node_id, diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp index 8d2c8805..e40f041f 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp @@ -91,7 +91,7 @@ SubscriptionData::Message::~Message() ///============================================================================= std::shared_ptr SubscriptionData::make( - const z_loaned_session_t * session, + const std::shared_ptr & session, std::shared_ptr graph_cache, const rmw_node_t * const node, liveliness::NodeInfo node_info, @@ -133,7 +133,7 @@ std::shared_ptr SubscriptionData::make( // with Zenoh; after this, callbacks may come in at any time. std::size_t domain_id = node_info.domain_id_; auto entity = liveliness::Entity::make( - z_info_zid(session), + session->get_zid(), std::to_string(node_id), std::to_string(subscription_id), liveliness::EntityType::Subscription, diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp index 9f9f8c02..dd90499a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.hpp @@ -62,7 +62,7 @@ class SubscriptionData final : public std::enable_shared_from_this make( - const z_loaned_session_t * session, + const std::shared_ptr & session, std::shared_ptr graph_cache, const rmw_node_t * const node, liveliness::NodeInfo node_info, diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp index 206e2a92..d4ce5b1e 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.cpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.cpp @@ -21,6 +21,9 @@ #include #include +#include +#include + #include "logging_macros.hpp" #include @@ -43,10 +46,9 @@ static const std::unordered_map _get_z_config( const char * envar_name, - const char * default_uri, - z_owned_config_t * config) + const char * default_uri) { const char * configured_uri; const char * envar_uri; @@ -55,7 +57,7 @@ rmw_ret_t _get_z_config( // NULL is returned if everything is ok. RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Envar %s cannot be read.", envar_name); - return RMW_RET_ERROR; + return std::nullopt; } // If the environment variable is set, try to read the configuration from the file, @@ -63,34 +65,36 @@ rmw_ret_t _get_z_config( configured_uri = envar_uri[0] != '\0' ? envar_uri : default_uri; // Try to read the configuration - if (zc_config_from_file(config, configured_uri) != Z_OK) { + zenoh::ZResult err; + zenoh::Config config = zenoh::Config::from_file(configured_uri, &err); + if (err != Z_OK) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "Invalid configuration file %s", configured_uri); - return RMW_RET_ERROR; + return std::nullopt; } RMW_ZENOH_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", "configured using configuration file %s", configured_uri); - return RMW_RET_OK; + return config; } } // namespace ///============================================================================= -rmw_ret_t get_z_config(const ConfigurableEntity & entity, z_owned_config_t * config) +std::optional get_z_config(const ConfigurableEntity & entity) { auto envar_map_it = envar_map.find(entity); if (envar_map_it == envar_map.end()) { RMW_ZENOH_LOG_ERROR_NAMED( "rmw_zenoh_cpp", "get_z_config called with invalid ConfigurableEntity."); - return RMW_RET_ERROR; + return std::nullopt; } // Get the absolute path to the default configuration file. static const std::string path_to_config_folder = ament_index_cpp::get_package_share_directory("rmw_zenoh_cpp") + "/config/"; const std::string default_config_path = path_to_config_folder + envar_map_it->second.second; - return _get_z_config(envar_map_it->second.first, default_config_path.c_str(), config); + return _get_z_config(envar_map_it->second.first, default_config_path.c_str()); } ///============================================================================= diff --git a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp index 357c7e89..fb07be92 100644 --- a/rmw_zenoh_cpp/src/detail/zenoh_config.hpp +++ b/rmw_zenoh_cpp/src/detail/zenoh_config.hpp @@ -15,7 +15,8 @@ #ifndef DETAIL__ZENOH_CONFIG_HPP_ #define DETAIL__ZENOH_CONFIG_HPP_ -#include +#include +#include #include #include "rmw/ret_types.h" @@ -43,9 +44,9 @@ enum class ConfigurableEntity : uint8_t /// is configured using the rmw_zenoh default configuration file. /// @param entity The zenoh entity to be configured. /// @param config The zenoh configuration to be filled. -/// @returns `RMW_RET_OK` if the configuration was successfully loaded. +/// @returns The zenoh configuration to be filled. [[nodiscard]] -rmw_ret_t get_z_config(const ConfigurableEntity & entity, z_owned_config_t * config); +std::optional get_z_config(const ConfigurableEntity & entity); ///============================================================================= /// Get the number of times rmw_init should try to connect to a zenoh router diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index d1d2fb63..b9f174fc 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -413,7 +413,7 @@ rmw_create_publisher( if (!node_data->create_pub_data( rmw_publisher, - context_impl->session(), + context_impl->session_cpp(), context_impl->get_next_entity_id(), topic_name, type_support, @@ -939,7 +939,7 @@ rmw_create_subscription( if (!node_data->create_sub_data( rmw_subscription, - context_impl->session(), + context_impl->session_cpp(), context_impl->graph_cache(), context_impl->get_next_entity_id(), topic_name, @@ -1416,7 +1416,7 @@ rmw_create_client( if (!node_data->create_client_data( rmw_client, - context_impl->session(), + context_impl->session_cpp(), context_impl->get_next_entity_id(), service_name, type_support, @@ -1660,7 +1660,7 @@ rmw_create_service( if (!node_data->create_service_data( rmw_service, - context_impl->session(), + context_impl->session_cpp(), context_impl->get_next_entity_id(), service_name, type_support, diff --git a/rmw_zenoh_cpp/src/zenohd/main.cpp b/rmw_zenoh_cpp/src/zenohd/main.cpp index f55c3b3a..987aa541 100644 --- a/rmw_zenoh_cpp/src/zenohd/main.cpp +++ b/rmw_zenoh_cpp/src/zenohd/main.cpp @@ -23,16 +23,14 @@ #include #endif -#include -#include +#include +#include #include "../detail/zenoh_config.hpp" -#include "../detail/liveliness_utils.hpp" #include "rmw/error_handling.h" - -#include "rcpputils/scope_exit.hpp" +#include "rcutils/env.h" static bool running = true; static std::mutex run_mutex; @@ -60,40 +58,36 @@ int main(int argc, char ** argv) (void)argc; (void)argv; - // If not already defined, set the logging environment variable for Zenoh router - // to info level by default. - // TODO(Yadunund): Switch to rcutils_get_env once it supports not overwriting values. - if (setenv(ZENOH_LOG_ENV_VAR_STR, ZENOH_LOG_INFO_LEVEL_STR, 0) != 0) { + if (!rcutils_set_env_overwrite(ZENOH_LOG_ENV_VAR_STR, ZENOH_LOG_INFO_LEVEL_STR, 0)) { RMW_SET_ERROR_MSG("Error configuring Zenoh logging."); return 1; } // Enable the zenoh built-in logger - zc_try_init_log_from_env(); - - // Initialize the zenoh configuration for the router. - z_owned_config_t config; - if ((rmw_zenoh_cpp::get_z_config( - rmw_zenoh_cpp::ConfigurableEntity::Router, - &config)) != RMW_RET_OK) - { + zenoh::try_init_log_from_env(); + + std::optional config = rmw_zenoh_cpp::get_z_config( + rmw_zenoh_cpp::ConfigurableEntity::Router); + + if (!config.has_value()) { RMW_SET_ERROR_MSG("Error configuring Zenoh router."); return 1; } - z_owned_session_t session; - if (z_open(&session, z_move(config), NULL) != Z_OK) { - printf("Unable to open router session!\n"); - return 1; + zenoh::ZResult result; + auto session = zenoh::Session::open( + std::move(config.value()), + zenoh::Session::SessionOptions::create_default(), + &result); + if(result != Z_OK) { + std::cout << "Error opening Session!" << "\\n"; + return 1; } - auto always_close_session = rcpputils::make_scope_exit( - [&session]() { - z_close(z_loan_mut(session), NULL); - }); - - printf( - "Started Zenoh router with id %s.\n", - rmw_zenoh_cpp::liveliness::zid_to_str(z_info_zid(z_session_loan(&session))).c_str()); + + std::cout << "Started Zenoh router with id" + << session.get_zid().to_string() + << std::endl; + #ifdef _WIN32 SetConsoleCtrlHandler(quit, TRUE); #else diff --git a/zenoh_c_vendor/CMakeLists.txt b/zenoh_cpp_vendor/CMakeLists.txt similarity index 82% rename from zenoh_c_vendor/CMakeLists.txt rename to zenoh_cpp_vendor/CMakeLists.txt index 086ea5db..ee633afb 100644 --- a/zenoh_c_vendor/CMakeLists.txt +++ b/zenoh_cpp_vendor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(zenoh_c_vendor) +project(zenoh_cpp_vendor) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -26,7 +26,7 @@ set(ZENOHC_CARGO_FLAGS "--no-default-features$--features=shared-memor # - https://github.com/eclipse-zenoh/zenoh-c/pull/620 (fix ze_querying_subscriber_get API to query newly discovered publishers) ament_vendor(zenoh_c_vendor VCS_URL https://github.com/eclipse-zenoh/zenoh-c.git - VCS_VERSION 4b759d4e4d35a97d7b20b5c4003b8b764a10f679 + VCS_VERSION 76c92a738d5fb720a441247c347bdd42090329e7 CMAKE_ARGS "-DZENOHC_CARGO_FLAGS=${ZENOHC_CARGO_FLAGS}" "-DZENOHC_BUILD_WITH_UNSTABLE_API=TRUE" @@ -35,4 +35,15 @@ ament_vendor(zenoh_c_vendor ament_export_dependencies(zenohc) +ament_vendor(zenoh_cpp_vendor + VCS_URL https://github.com/eclipse-zenoh/zenoh-cpp + VCS_VERSION 730f993ade9e715b0e44e623c1477ebe54f1ec7a + CMAKE_ARGS + -DZENOHCXX_ZENOHC=OFF +) + +externalproject_add_stepdependencies(zenoh_cpp_vendor configure zenoh_c_vendor) + +ament_export_dependencies(zenohcxx) + ament_package() diff --git a/zenoh_c_vendor/package.xml b/zenoh_cpp_vendor/package.xml similarity index 95% rename from zenoh_c_vendor/package.xml rename to zenoh_cpp_vendor/package.xml index b907feff..eb2c8c20 100644 --- a/zenoh_c_vendor/package.xml +++ b/zenoh_cpp_vendor/package.xml @@ -1,7 +1,7 @@ - zenoh_c_vendor + zenoh_cpp_vendor 0.0.1 Vendor pkg to install zenoh-c Yadunund