Redesigned zlib Datablock to use std::vector

draco_point_cloud_transport/src/cloud.cpp

@@ -29,17 +29,11 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-// HACK: we need to access PointCloud2IteratorBase::data_char_ which is private
 #include <algorithm>
 #include <string>
 #include <sstream>
 #include <cstring>
 
-#define private protected
-#include <sensor_msgs/point_cloud2_iterator.hpp>
-#undef private
-
-
 #include <draco_point_cloud_transport/cloud.hpp>
 
 namespace cras
@@ -54,24 +48,22 @@ bool hasField(const ::cras::Cloud & cloud, const std::string & fieldName)
 
 sensor_msgs::msg::PointField & getField(::cras::Cloud & cloud, const std::string & fieldName)
 {
-  for (auto & field : cloud.fields) {
-    if (field.name == fieldName) {
-      return field;
-    }
+  const auto it = std::ranges::find(cloud.fields, fieldName, &sensor_msgs::msg::PointField::name);
+  if (it == cloud.fields.end()) {
+    throw std::runtime_error(std::string("Field ") + fieldName + " does not exist.");
   }
-  throw std::runtime_error(std::string("Field ") + fieldName + " does not exist.");
+  return *it;
 }
 
 const sensor_msgs::msg::PointField & getField(
   const ::cras::Cloud & cloud,
   const std::string & fieldName)
 {
-  for (const auto & field : cloud.fields) {
-    if (field.name == fieldName) {
-      return field;
-    }
+  const auto it = std::ranges::find(cloud.fields, fieldName, &sensor_msgs::msg::PointField::name);
+  if (it == cloud.fields.end()) {
+    throw std::runtime_error(std::string("Field ") + fieldName + " does not exist.");
   }
-  throw std::runtime_error(std::string("Field ") + fieldName + " does not exist.");
+  return *it;
 }
 
 size_t sizeOfPointField(const ::sensor_msgs::msg::PointField & field)
@@ -132,7 +124,10 @@ GenericCloudIteratorBase<T, TT, U, C, V>::GenericCloudIteratorBase(
 template<typename T, typename TT, typename U, typename C, template<typename> class V>
 U * GenericCloudIteratorBase<T, TT, U, C, V>::rawData() const
 {
-  return this->data_char_;
+  // The current element's address is exactly the raw byte position the base
+  // iterator tracks internally (data_ is kept in sync with its private
+  // data_char_), so we avoid reaching into that private member.
+  return reinterpret_cast<U *>(&this->operator*());
 }
 
 template<typename T>

zlib_point_cloud_transport/src/zlib_cpp.cpp

@@ -41,29 +41,23 @@ const int MAX_CHUNK_SIZE = 1024;
 
 const int WINDOW_BITS = 15;
 
-/// Allocate memory to DataBlock and assign to a shared_ptr object.
+/// Allocate a DataBlock owning `size` bytes in a single allocation.
 std::shared_ptr<DataBlock> AllocateData(std::size_t size)
 {
-  std::shared_ptr<DataBlock> data(new DataBlock, [](DataBlock * p) {
-      delete[] p->ptr;
-      delete p;
-    });
-  data->ptr = new uint8_t[size];
-  data->size = size;
-  return data;
+  return std::make_shared<DataBlock>(size);
 }
 
 std::shared_ptr<DataBlock> ExpandDataList(const std::list<std::shared_ptr<DataBlock>> & data_list)
 {
   std::size_t total_size = 0;
   for (const std::shared_ptr<DataBlock> & this_data : data_list) {
-    total_size += this_data->size;
+    total_size += this_data->size();
   }
   std::shared_ptr<DataBlock> out_data = AllocateData(total_size);
-  uint8_t * this_ptr = out_data->ptr;
+  uint8_t * this_ptr = out_data->ptr();
   for (const std::shared_ptr<DataBlock> & this_data : data_list) {
-    memcpy(this_ptr, this_data->ptr, this_data->size);
-    this_ptr += this_data->size;
+    memcpy(this_ptr, this_data->ptr(), this_data->size());
+    this_ptr += this_data->size();
   }
   return out_data;
 }
@@ -110,7 +104,7 @@ std::list<std::shared_ptr<DataBlock>> Comp::Process(
     std::size_t out_size = MAX_CHUNK_SIZE - zs_.avail_out;
     std::shared_ptr<DataBlock> out_data = AllocateData(out_size);
     // Copy and add to output data list.
-    memcpy(out_data->ptr, out_buffer, out_size);
+    memcpy(out_data->ptr(), out_buffer, out_size);
     out_data_list.push_back(std::move(out_data));
   } while (zs_.avail_out == 0);
   // Done.
@@ -134,8 +128,8 @@ std::list<std::shared_ptr<DataBlock>> Decomp::Process(
   std::list<std::shared_ptr<DataBlock>> out_data_list;
   uint8_t out_buffer[MAX_CHUNK_SIZE];
   // Incoming buffer.
-  zs_.avail_in = static_cast<uInt>(compressed_data->size);
-  zs_.next_in = compressed_data->ptr;
+  zs_.avail_in = static_cast<uInt>(compressed_data->size());
+  zs_.next_in = compressed_data->ptr();
   int ret;
   do {
     // Prepare outcoming buffer and size.
@@ -156,7 +150,7 @@ std::list<std::shared_ptr<DataBlock>> Decomp::Process(
     std::size_t out_size = MAX_CHUNK_SIZE - zs_.avail_out;
     // Allocate outcome buffer.
     std::shared_ptr<DataBlock> out_data = AllocateData(out_size);
-    memcpy(out_data->ptr, out_buffer, out_size);
+    memcpy(out_data->ptr(), out_buffer, out_size);
     out_data_list.push_back(std::move(out_data));
   } while (zs_.avail_out == 0);
   return std::move(out_data_list);

zlib_point_cloud_transport/src/zlib_cpp.hpp

@@ -37,14 +37,25 @@
 #include <list>
 #include <memory>
 #include <tuple>
+#include <vector>
 
 namespace zlib
 {
 
 struct DataBlock
 {
-  uint8_t * ptr;
-  std::size_t size;
+  //! Owned byte buffer; freed automatically with the DataBlock.
+  std::vector<uint8_t> data;
+
+  explicit DataBlock(std::size_t size)
+  : data(size) {}
+
+  //! Pointer to the owned byte buffer.
+  [[nodiscard]] uint8_t * ptr() noexcept {return data.data();}
+  [[nodiscard]] const uint8_t * ptr() const noexcept {return data.data();}
+
+  //! Number of bytes in the buffer.
+  [[nodiscard]] std::size_t size() const noexcept {return data.size();}
 };
 
 std::shared_ptr<DataBlock> AllocateData(std::size_t size);

zlib_point_cloud_transport/src/zlib_publisher.cpp

@@ -90,15 +90,15 @@ ZlibPublisher::TypedEncodeResult ZlibPublisher::encodeTyped(
 
   size_t total_size = 0;
   for (const auto & data : g_compressed_data) {
-    total_size += data->size;
+    total_size += data->size();
   }
 
   compressed.compressed_data.resize(total_size);
 
   size_t index = 0;
   for (const auto & data : g_compressed_data) {
-    memcpy(&compressed.compressed_data[index], data->ptr, data->size);
-    index += data->size;
+    memcpy(&compressed.compressed_data[index], data->ptr(), data->size());
+    index += data->size();
   }
 
   compressed.width = raw.width;

zlib_point_cloud_transport/src/zlib_subscriber.cpp

@@ -54,15 +54,15 @@ ZlibSubscriber::DecodeResult ZlibSubscriber::decodeTyped(
   zlib::Decomp decomp;
 
   std::shared_ptr<zlib::DataBlock> data = zlib::AllocateData(msg.compressed_data.size());
-  memcpy(data->ptr, &msg.compressed_data[0], msg.compressed_data.size());
+  memcpy(data->ptr(), &msg.compressed_data[0], msg.compressed_data.size());
 
   std::list<std::shared_ptr<zlib::DataBlock>> out_data_list;
   out_data_list = decomp.Process(data);
 
   std::shared_ptr<zlib::DataBlock> data2 = zlib::ExpandDataList(out_data_list);
 
-  result->data.resize(data2->size);
-  memcpy(&result->data[0], data2->ptr, data2->size);
+  result->data.resize(data2->size());
+  memcpy(&result->data[0], data2->ptr(), data2->size());
 
   result->width = msg.width;
   result->height = msg.height;

zlib_point_cloud_transport/test/test_zlib_roundtrip.cpp

@@ -58,7 +58,7 @@ std::vector<uint8_t> compress(const std::vector<uint8_t> & in)
   const auto chunks = comp.Process(in.data(), in.size(), true);
   std::vector<uint8_t> out;
   for (const auto & chunk : chunks) {
-    out.insert(out.end(), chunk->ptr, chunk->ptr + chunk->size);
+    out.insert(out.end(), chunk->ptr(), chunk->ptr() + chunk->size());
   }
   return out;
 }
@@ -68,10 +68,10 @@ std::vector<uint8_t> decompress(const std::vector<uint8_t> & in)
 {
   zlib::Decomp decomp;
   auto block = zlib::AllocateData(in.size());
-  std::memcpy(block->ptr, in.data(), in.size());
+  std::memcpy(block->ptr(), in.data(), in.size());
   const auto chunks = decomp.Process(block);
   const auto expanded = zlib::ExpandDataList(chunks);
-  return std::vector<uint8_t>(expanded->ptr, expanded->ptr + expanded->size);
+  return std::vector<uint8_t>(expanded->ptr(), expanded->ptr() + expanded->size());
 }
 
 }  // namespace