Added ZSTD plugin

Signed-off-by: Alejandro Hernández Cordero <[email protected]>

zstd_point_cloud_transport/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+
+project(zstd_point_cloud_transport)
+
+find_package(ament_cmake REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(point_cloud_interfaces REQUIRED)
+find_package(point_cloud_transport REQUIRED)
+find_package(rclcpp REQUIRED)
+
+set(dependencies
+  pluginlib
+  point_cloud_interfaces
+  point_cloud_transport
+  rclcpp
+)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME}
+  SHARED
+  src/zstd_publisher.cpp
+  src/zstd_subscriber.cpp
+  src/manifest.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} zstd)
+
+ament_target_dependencies(${PROJECT_NAME} ${dependencies})
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+pluginlib_export_plugin_description_file(point_cloud_transport zstd_plugins.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(${dependencies})
+ament_package()

zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_publisher.hpp

@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2023, Open Source Robotics Foundation, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *    * Redistributions of source code must retain the above copyright
+ *      notice, this list of conditions and the following disclaimer.
+ *
+ *    * Redistributions in binary form must reproduce the above copyright
+ *      notice, this list of conditions and the following disclaimer in the
+ *      documentation and/or other materials provided with the distribution.
+ *
+ *    * Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived from
+ *      this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_PUBLISHER_HPP_
+#define ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <point_cloud_transport/point_cloud_transport.hpp>
+
+#include <point_cloud_transport/simple_publisher_plugin.hpp>
+#include <point_cloud_interfaces/msg/compressed_point_cloud2.hpp>
+
+
+namespace zstd_point_cloud_transport
+{
+
+class ZstdPublisher
+  : public point_cloud_transport::SimplePublisherPlugin<
+    point_cloud_interfaces::msg::CompressedPointCloud2>
+{
+public:
+  std::string getTransportName() const override;
+
+  void declareParameters(const std::string & base_topic) override;
+
+  std::string getDataType() const override;
+
+  TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override;
+
+private:
+  int encode_level_{7};
+};
+}  // namespace zstd_point_cloud_transport
+
+#endif  // ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_PUBLISHER_HPP_

zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_subscriber.hpp

@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2023, Open Source Robotics Foundation, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *    * Redistributions of source code must retain the above copyright
+ *      notice, this list of conditions and the following disclaimer.
+ *
+ *    * Redistributions in binary form must reproduce the above copyright
+ *      notice, this list of conditions and the following disclaimer in the
+ *      documentation and/or other materials provided with the distribution.
+ *
+ *    * Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived from
+ *      this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_SUBSCRIBER_HPP_
+#define ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_SUBSCRIBER_HPP_
+
+#include <string>
+
+#include <point_cloud_interfaces/msg/compressed_point_cloud2.hpp>
+
+#include <point_cloud_transport/simple_subscriber_plugin.hpp>
+#include <point_cloud_transport/transport_hints.hpp>
+
+namespace zstd_point_cloud_transport
+{
+
+class ZstdSubscriber
+  : public point_cloud_transport::SimpleSubscriberPlugin<
+    point_cloud_interfaces::msg::CompressedPointCloud2>
+{
+public:
+  std::string getTransportName() const override;
+
+  void declareParameters() override;
+
+  std::string getDataType() const override;
+
+  DecodeResult decodeTyped(const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed)
+  const override;
+};
+}  // namespace zstd_point_cloud_transport
+
+#endif  // ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_SUBSCRIBER_HPP_

zstd_point_cloud_transport/package.xml

@@ -0,0 +1,33 @@
+<package format="3">
+  <name>zstd_point_cloud_transport</name>
+  <version>1.0.5</version>
+  <description>
+    zstd_point_cloud_transport provides a plugin to point_cloud_transport for sending point clouds
+    encoded with zlib
+  </description>
+  <author>Jakub Paplham</author>
+  <maintainer email="[email protected]">Alejandro Hernandez Cordero</maintainer>
+  <license>BSD</license>
+
+  <url type="repository">https://github.com/ctu-vras/draco_point_cloud_transport</url>
+  <url type="website">https://wiki.ros.org/draco_point_cloud_transport</url>
+  <url type="bugtracker">https://github.com/ctu-vras/draco_point_cloud_transport/issues</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>pluginlib</build_depend>
+
+  <exec_depend>pluginlib</exec_depend>
+
+  <depend>point_cloud_interfaces</depend>
+  <depend>point_cloud_transport</depend>
+  <depend>rclcpp</depend>
+  <depend>libzstd-dev</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

zstd_point_cloud_transport/src/manifest.cpp

@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2023, Czech Technical University in Prague
+ * Copyright (c) 2019, paplhjak
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *    * Redistributions of source code must retain the above copyright
+ *      notice, this list of conditions and the following disclaimer.
+ *
+ *    * Redistributions in binary form must reproduce the above copyright
+ *      notice, this list of conditions and the following disclaimer in the
+ *      documentation and/or other materials provided with the distribution.
+ *
+ *    * Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived from
+ *      this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <pluginlib/class_list_macros.hpp>
+
+#include <point_cloud_transport/publisher_plugin.hpp>
+#include <point_cloud_transport/subscriber_plugin.hpp>
+
+#include <zstd_point_cloud_transport/zstd_publisher.hpp>
+#include <zstd_point_cloud_transport/zstd_subscriber.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  zstd_point_cloud_transport::ZstdPublisher,
+  point_cloud_transport::PublisherPlugin)
+PLUGINLIB_EXPORT_CLASS(
+  zstd_point_cloud_transport::ZstdSubscriber,
+  point_cloud_transport::SubscriberPlugin)

zstd_point_cloud_transport/src/zstd_publisher.cpp

@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2023, Open Source Robotics Foundation, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *    * Redistributions of source code must retain the above copyright
+ *      notice, this list of conditions and the following disclaimer.
+ *
+ *    * Redistributions in binary form must reproduce the above copyright
+ *      notice, this list of conditions and the following disclaimer in the
+ *      documentation and/or other materials provided with the distribution.
+ *
+ *    * Neither the name of the copyright holder nor the names of its
+ *      contributors may be used to endorse or promote products derived from
+ *      this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <zstd_point_cloud_transport/zstd_publisher.hpp>
+
+#include "zstd.h"
+
+namespace zstd_point_cloud_transport
+{
+
+void ZstdPublisher::declareParameters(const std::string & base_topic)
+{
+  rcl_interfaces::msg::ParameterDescriptor encode_level_paramDescriptor;
+  encode_level_paramDescriptor.name = "zstd_encode_level";
+  encode_level_paramDescriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+  encode_level_paramDescriptor.description =
+    "0 = minimum compression, but the maximum compression 10";
+  encode_level_paramDescriptor.set__integer_range(
+    {rcl_interfaces::msg::IntegerRange()
+      .set__from_value(-1)
+      .set__to_value(9)
+      .set__step(1)});
+  declareParam<int>(
+    encode_level_paramDescriptor.name, this->encode_level_,
+    encode_level_paramDescriptor);
+
+  auto param_change_callback =
+    [this](const std::vector<rclcpp::Parameter> & parameters) -> rcl_interfaces::msg::
+    SetParametersResult
+    {
+      auto result = rcl_interfaces::msg::SetParametersResult();
+      result.successful = true;
+      for (auto parameter : parameters) {
+        if (parameter.get_name() == "zstd_encode_level") {
+          this->encode_level_ = static_cast<int>(parameter.as_int());
+          if (!(this->encode_level_ >= -1 && this->encode_level_ <= 9)) {
+            RCLCPP_ERROR_STREAM(
+              getLogger(), "encode_level value range should be between [0, 10] ");
+          }
+          return result;
+        }
+      }
+      return result;
+    };
+  setParamCallback(param_change_callback);
+}
+
+std::string ZstdPublisher::getDataType() const
+{
+  return "point_cloud_interfaces/msg/CompressedPointCloud2";
+}
+
+std::string ZstdPublisher::getTransportName() const
+{
+  return "zstd";
+}
+
+ZstdPublisher::TypedEncodeResult ZstdPublisher::encodeTyped(
+  const sensor_msgs::msg::PointCloud2 & raw) const
+{
+  size_t est_compress_size = ZSTD_compressBound(raw.data.size());
+
+  std::string comp_buffer{};
+  comp_buffer.resize(est_compress_size);
+  auto compress_size =
+      ZSTD_compress((void*)comp_buffer.data(), est_compress_size, &raw.data[0],
+                    raw.data.size(), this->encode_level_);
+
+  comp_buffer.resize(compress_size);
+  comp_buffer.shrink_to_fit();
+
+  point_cloud_interfaces::msg::CompressedPointCloud2 compressed;
+  compressed.compressed_data.resize(compress_size);
+  memcpy(&compressed.compressed_data[0], comp_buffer.data(), comp_buffer.size());
+
+  compressed.width = raw.width;
+  compressed.height = raw.height;
+  compressed.row_step = raw.row_step;
+  compressed.point_step = raw.point_step;
+  compressed.is_bigendian = raw.is_bigendian;
+  compressed.is_dense = raw.is_dense;
+  compressed.header = raw.header;
+  compressed.fields = raw.fields;
+  compressed.format = getTransportName();
+
+  return compressed;
+}
+
+}  // namespace zlib_point_cloud_transport