Fix a number of small issues in rmw_zenoh_cpp (#106)

* Keep a running total of the total number of nodes in the graph. That way we don't have to compute it when the RMW layer asks for it. Signed-off-by: Chris Lalancette <[email protected]> * Make sure to remove an empty namespace from the graph cache. If all nodes from a particular namespace have been removed, make sure to remove that namespace as well. Signed-off-by: Chris Lalancette <[email protected]> * Remove unnecessary TODO comments. Signed-off-by: Chris Lalancette <[email protected]> * Remove PublishToken. While it is a nice idea, there is more than just liveliness that will prevent us from using zenoh-pico currently. Remove this code until we decide to officially support zenoh-pico. Signed-off-by: Chris Lalancette <[email protected]> * Small cleanup for token error handling. We know that the pointer won't be NULL by that point, so we don't need the check. Signed-off-by: Chris Lalancette <[email protected]> * Remove hand-coded rcutils_strdup implementation. There is really no need for it; we can just use rcutils_strdup instead. Signed-off-by: Chris Lalancette <[email protected]> * Speed up ros_topic_name_to_zenoh_key. Use stringstream here is really slow; we can speed it up by about 100% by switching to std::to_string instead. Signed-off-by: Chris Lalancette <[email protected]> * Switch to RAII for saved_msg_data. Signed-off-by: Chris Lalancette <[email protected]> * Implement rmw_compare_gids_equal. Signed-off-by: Chris Lalancette <[email protected]> --------- Signed-off-by: Chris Lalancette <[email protected]>
ros2 · Feb 8, 2024 · 60c4380 · 60c4380
1 parent 1b4a717
commit 60c4380
Show file tree

Hide file tree

Showing 7 changed files with 49 additions and 186 deletions.
diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.cpp b/rmw_zenoh_cpp/src/detail/graph_cache.cpp
@@ -205,6 +205,7 @@ void GraphCache::parse_put(const std::string & keyexpr)
     NodeMap node_map = {
       {entity.node_name(), make_graph_node(entity, *this)}};
     graph_.emplace(std::make_pair(entity.node_namespace(), std::move(node_map)));
+    total_nodes_in_graph_ += 1;
     return;
   }
 
@@ -226,6 +227,7 @@ void GraphCache::parse_put(const std::string & keyexpr)
     // name but unique id.
     NodeMap::iterator insertion_it =
       ns_it->second.insert(std::make_pair(entity.node_name(), make_graph_node(entity, *this)));
+    total_nodes_in_graph_ += 1;
     if (insertion_it == ns_it->second.end()) {
       RCUTILS_LOG_ERROR_NAMED(
         "rmw_zenoh_cpp",
@@ -456,6 +458,10 @@ void GraphCache::parse_del(const std::string & keyexpr)
       remove_topics(graph_node->clients_, EntityType::Client, *this);
     }
     ns_it->second.erase(node_it);
+    total_nodes_in_graph_ -= 1;
+    if (ns_it->second.size() == 0) {
+      graph_.erase(entity.node_namespace());
+    }
     return;
   }
 
@@ -494,13 +500,8 @@ rmw_ret_t GraphCache::get_node_names(
   RCUTILS_CHECK_ALLOCATOR_WITH_MSG(
     allocator, "get_node_names allocator is not valid", return RMW_RET_INVALID_ARGUMENT);
 
-  size_t nodes_number = 0;
-  for (const std::pair<const std::string, NodeMap> & it : graph_) {
-    nodes_number += it.second.size();
-  }
-
   rcutils_ret_t rcutils_ret =
-    rcutils_string_array_init(node_names, nodes_number, allocator);
+    rcutils_string_array_init(node_names, total_nodes_in_graph_, allocator);
   if (rcutils_ret != RCUTILS_RET_OK) {
     return RMW_RET_BAD_ALLOC;
   }
@@ -515,7 +516,7 @@ rmw_ret_t GraphCache::get_node_names(
     });
 
   rcutils_ret =
-    rcutils_string_array_init(node_namespaces, nodes_number, allocator);
+    rcutils_string_array_init(node_namespaces, total_nodes_in_graph_, allocator);
   if (rcutils_ret != RCUTILS_RET_OK) {
     return RMW_RET_BAD_ALLOC;
   }
@@ -541,7 +542,7 @@ rmw_ret_t GraphCache::get_node_names(
   std::shared_ptr<rcpputils::scope_exit<decltype(free_enclaves_lambda)>> free_enclaves{nullptr};
   if (enclaves) {
     rcutils_ret =
-      rcutils_string_array_init(enclaves, nodes_number, allocator);
+      rcutils_string_array_init(enclaves, total_nodes_in_graph_, allocator);
     if (RCUTILS_RET_OK != rcutils_ret) {
       return RMW_RET_BAD_ALLOC;
     }

diff --git a/rmw_zenoh_cpp/src/detail/graph_cache.hpp b/rmw_zenoh_cpp/src/detail/graph_cache.hpp
@@ -180,6 +180,7 @@ class GraphCache final
   using NamespaceMap = std::unordered_map<std::string, NodeMap>;
   // Map namespace to a map of <node_name, GraphNodePtr>.
   NamespaceMap graph_ = {};
+  size_t total_nodes_in_graph_{0};
 
   // Optimize pub/sub lookups across the graph.
   GraphNode::TopicMap graph_topics_ = {};

diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.cpp
@@ -432,67 +432,6 @@ std::string Entity::keyexpr() const
   return this->keyexpr_;
 }
 
-///=============================================================================
-bool PublishToken::put(
-  z_owned_session_t * session,
-  const std::string & token)
-{
-  if (!z_session_check(session)) {
-    RCUTILS_SET_ERROR_MSG("The zenoh session is invalid.");
-    return false;
-  }
-
-  // TODO(Yadunund): z_keyexpr_new creates a copy so find a way to avoid it.
-  z_owned_keyexpr_t keyexpr = z_keyexpr_new(token.c_str());
-  auto drop_keyexpr = rcpputils::make_scope_exit(
-    [&keyexpr]() {
-      z_drop(z_move(keyexpr));
-    });
-  if (!z_keyexpr_check(&keyexpr)) {
-    RCUTILS_SET_ERROR_MSG("invalid keyexpression generation for liveliness publication.");
-    return false;
-  }
-
-  z_put_options_t options = z_put_options_default();
-  options.encoding = z_encoding(Z_ENCODING_PREFIX_EMPTY, NULL);
-  if (z_put(z_loan(*session), z_keyexpr(token.c_str()), nullptr, 0, &options) < 0) {
-    RCUTILS_SET_ERROR_MSG("unable to publish liveliness for node creation");
-    return false;
-  }
-
-  return true;
-}
-
-///=============================================================================
-bool PublishToken::del(
-  z_owned_session_t * session,
-  const std::string & token)
-{
-  if (!z_session_check(session)) {
-    RCUTILS_SET_ERROR_MSG("The zenoh session is invalid.");
-    return false;
-  }
-
-  // TODO(Yadunund): z_keyexpr_new creates a copy so find a way to avoid it.
-  z_owned_keyexpr_t keyexpr = z_keyexpr_new(token.c_str());
-  auto drop_keyexpr = rcpputils::make_scope_exit(
-    [&keyexpr]() {
-      z_drop(z_move(keyexpr));
-    });
-  if (!z_keyexpr_check(&keyexpr)) {
-    RCUTILS_SET_ERROR_MSG("invalid key-expression generation for liveliness publication.");
-    return false;
-  }
-
-  const z_delete_options_t options = z_delete_options_default();
-  if (z_delete(z_loan(*session), z_loan(keyexpr), &options) < 0) {
-    RCUTILS_SET_ERROR_MSG("failed to delete liveliness key");
-    return false;
-  }
-
-  return true;
-}
-
 ///=============================================================================
 std::string mangle_name(const std::string & input)
 {

diff --git a/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp b/rmw_zenoh_cpp/src/detail/liveliness_utils.hpp
@@ -119,21 +119,6 @@ class Entity
   std::string keyexpr_;
 };
 
-///=============================================================================
-/// Helper utilities to put/delete tokens until liveliness is supported in the
-/// zenoh-c bindings.
-class PublishToken
-{
-public:
-  static bool put(
-    z_owned_session_t * session,
-    const std::string & token);
-
-  static bool del(
-    z_owned_session_t * session,
-    const std::string & token);
-};
-
 /// Replace "/" instances with "%".
 std::string mangle_name(const std::string & input);
 

diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.cpp
@@ -31,6 +31,11 @@ saved_msg_data::saved_msg_data(zc_owned_payload_t p, uint64_t recv_ts, const uin
   memcpy(publisher_gid, pub_gid, 16);
 }
 
+saved_msg_data::~saved_msg_data()
+{
+  z_drop(z_move(payload));
+}
+
 void rmw_subscription_data_t::attach_condition(std::condition_variable * condition_variable)
 {
   std::lock_guard<std::mutex> lock(condition_mutex_);
@@ -92,7 +97,6 @@ void rmw_subscription_data_t::add_new_message(
     // queue if it is non-empty.
     if (!message_queue_.empty()) {
       std::unique_ptr<saved_msg_data> old = std::move(message_queue_.front());
-      z_drop(z_move(old->payload));
       message_queue_.pop_front();
     }
   }

diff --git a/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp b/rmw_zenoh_cpp/src/detail/rmw_data_types.hpp
@@ -111,6 +111,8 @@ struct saved_msg_data
 {
   explicit saved_msg_data(zc_owned_payload_t p, uint64_t recv_ts, const uint8_t pub_gid[16]);
 
+  ~saved_msg_data();
+
   zc_owned_payload_t payload;
   uint64_t recv_timestamp;
   uint8_t publisher_gid[16];