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