Safely copy GID

Signed-off-by: Yadunund <yadunund@intrinsic.ai>
ros2 · Oct 1, 2024 · 4edbee2 · 4edbee2
1 parent cd27cb5
commit 4edbee2
Show file tree

Hide file tree

Showing 5 changed files with 51 additions and 18 deletions.
diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.cpp
@@ -289,7 +289,16 @@ rmw_ret_t PublisherData::publish(
   const size_t data_length = ser.get_serialized_data_length();
 
   z_owned_bytes_map_t map =
-    create_map_and_set_sequence_num(sequence_number_++, gid_);
+    create_map_and_set_sequence_num(
+    sequence_number_++,
+    [this](z_owned_bytes_map_t * map, const char * key)
+    {
+      // Mutex already locked.
+      z_bytes_t gid_bytes;
+      gid_bytes.len = RMW_GID_STORAGE_SIZE;
+      gid_bytes.start = gid_;
+      z_bytes_map_insert_by_copy(map, z_bytes_new(key), gid_bytes);
+    });
   if (!z_check(map)) {
     // create_map_and_set_sequence_num already set the error
     return RMW_RET_ERROR;
@@ -341,8 +350,16 @@ rmw_ret_t PublisherData::publish_serialized_message(
 
   std::lock_guard<std::mutex> lock(mutex_);
 
-  z_owned_bytes_map_t map =
-    rmw_zenoh_cpp::create_map_and_set_sequence_num(sequence_number_++, gid_);
+  z_owned_bytes_map_t map = rmw_zenoh_cpp::create_map_and_set_sequence_num(
+    sequence_number_++,
+    [this](z_owned_bytes_map_t * map, const char * key)
+    {
+      // Mutex already locked.
+      z_bytes_t gid_bytes;
+      gid_bytes.len = RMW_GID_STORAGE_SIZE;
+      gid_bytes.start = gid_;
+      z_bytes_map_insert_by_copy(map, z_bytes_new(key), gid_bytes);
+    });
 
   if (!z_check(map)) {
     // create_map_and_set_sequence_num already set the error
@@ -391,10 +408,10 @@ liveliness::TopicInfo PublisherData::topic_info() const
 }
 
 ///=============================================================================
-const uint8_t * PublisherData::gid() const
+void PublisherData::copy_gid(rmw_gid_t * gid) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  return gid_;
+  memcpy(gid->data, gid_, RMW_GID_STORAGE_SIZE);
 }
 
 ///=============================================================================

diff --git a/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp b/rmw_zenoh_cpp/src/detail/rmw_publisher_data.hpp
@@ -70,8 +70,8 @@ class PublisherData final
   // Get a copy of the TopicInfo of this PublisherData.
   liveliness::TopicInfo topic_info() const;
 
-  // Get the GID of this PublisherData.
-  const uint8_t * gid() const;
+  // Copy the GID of this PublisherData into an rmw_gid_t.
+  void copy_gid(rmw_gid_t * gid) const;
 
   // Returns true if liveliness token is still valid.
   bool liveliness_is_valid() const;

diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.cpp
@@ -25,7 +25,9 @@ namespace rmw_zenoh_cpp
 {
 ///=============================================================================
 z_owned_bytes_map_t
-create_map_and_set_sequence_num(int64_t sequence_number, const uint8_t gid[RMW_GID_STORAGE_SIZE])
+create_map_and_set_sequence_num(
+  int64_t sequence_number,
+  GIDCopier gid_copier)
 {
   z_owned_bytes_map_t map = z_bytes_map_new();
   if (!z_check(map)) {
@@ -54,12 +56,7 @@ create_map_and_set_sequence_num(int64_t sequence_number, const uint8_t gid[RMW_G
     return z_bytes_map_null();
   }
   z_bytes_map_insert_by_copy(&map, z_bytes_new("source_timestamp"), z_bytes_new(source_ts_str));
-
-  z_bytes_t gid_bytes;
-  gid_bytes.len = RMW_GID_STORAGE_SIZE;
-  gid_bytes.start = gid;
-
-  z_bytes_map_insert_by_copy(&map, z_bytes_new("source_gid"), gid_bytes);
+  gid_copier(&map, "source_gid");
 
   free_attachment_map.cancel();
 

diff --git a/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp b/rmw_zenoh_cpp/src/detail/zenoh_utils.hpp
@@ -17,13 +17,19 @@
 
 #include <zenoh.h>
 
+#include <functional>
+
 #include "rmw/types.h"
 
 namespace rmw_zenoh_cpp
 {
 ///=============================================================================
+// A function to safely copy an entity's GID as a z_bytes_t into a
+// z_owned_bytes_map_t for a given key.
+using GIDCopier = std::function<void (z_owned_bytes_map_t *, const char *)>;
+///=============================================================================
 z_owned_bytes_map_t
-create_map_and_set_sequence_num(int64_t sequence_number, const uint8_t gid[RMW_GID_STORAGE_SIZE]);
+create_map_and_set_sequence_num(int64_t sequence_number, GIDCopier gid_copier);
 
 }  // namespace rmw_zenoh_cpp
 

diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp
@@ -2089,7 +2089,13 @@ rmw_send_request(
 
   z_owned_bytes_map_t map = rmw_zenoh_cpp::create_map_and_set_sequence_num(
     *sequence_id,
-    client_data->client_gid);
+    [client_data](z_owned_bytes_map_t * map, const char * key)
+    {
+      z_bytes_t gid_bytes;
+      gid_bytes.len = RMW_GID_STORAGE_SIZE;
+      gid_bytes.start = client_data->client_gid;
+      z_bytes_map_insert_by_copy(map, z_bytes_new(key), gid_bytes);
+    });
   if (!z_check(map)) {
     // create_map_and_set_sequence_num already set the error
     return RMW_RET_ERROR;
@@ -2781,7 +2787,14 @@ rmw_send_response(
   z_query_reply_options_t options = z_query_reply_options_default();
 
   z_owned_bytes_map_t map = rmw_zenoh_cpp::create_map_and_set_sequence_num(
-    request_header->sequence_number, request_header->writer_guid);
+    request_header->sequence_number,
+    [request_header](z_owned_bytes_map_t * map, const char * key)
+    {
+      z_bytes_t gid_bytes;
+      gid_bytes.len = RMW_GID_STORAGE_SIZE;
+      gid_bytes.start = request_header->writer_guid;
+      z_bytes_map_insert_by_copy(map, z_bytes_new(key), gid_bytes);
+    });
   if (!z_check(map)) {
     // create_map_and_set_sequence_num already set the error
     return RMW_RET_ERROR;
@@ -3438,7 +3451,7 @@ rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid)
   RMW_CHECK_ARGUMENT_FOR_NULL(pub_data, RMW_RET_INVALID_ARGUMENT);
 
   gid->implementation_identifier = rmw_zenoh_cpp::rmw_zenoh_identifier;
-  memcpy(gid->data, pub_data->gid(), RMW_GID_STORAGE_SIZE);
+  pub_data->copy_gid(gid);
 
   return RMW_RET_OK;
 }