Plugin idea: Organized transport

.vscode/settings.json

@@ -1,6 +1,91 @@
 {
     "files.associations": {
         "algorithm": "cpp",
-        "memory": "cpp"
+        "memory": "cpp",
+        "compare": "cpp",
+        "array": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "string_view": "cpp",
+        "regex": "cpp",
+        "*.ipp": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "*.tcc": "cpp",
+        "slist": "cpp",
+        "valarray": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "any": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "bitset": "cpp",
+        "cfenv": "cpp",
+        "charconv": "cpp",
+        "chrono": "cpp",
+        "cinttypes": "cpp",
+        "codecvt": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "coroutine": "cpp",
+        "cstdint": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "exception": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "source_location": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "cpp",
+        "ranges": "cpp",
+        "scoped_allocator": "cpp",
+        "semaphore": "cpp",
+        "shared_mutex": "cpp",
+        "span": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "variant": "cpp",
+        "expected": "cpp"
     }
 }

organized_point_cloud_transport/CMakeLists.txt

@@ -0,0 +1,66 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+
+project(organized_point_cloud_transport)
+
+find_package(ament_cmake REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(point_cloud_interfaces REQUIRED)
+find_package(point_cloud_transport REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+set(dependencies
+  pcl_conversions
+  pcl_ros
+  pluginlib
+  point_cloud_interfaces
+  point_cloud_transport
+  rclcpp
+  sensor_msgs
+  tf2_geometry_msgs
+  tf2_ros
+)
+
+
+include_directories(
+  include
+)
+
+add_library(${PROJECT_NAME}
+  SHARED
+  src/manifest.cpp
+  src/organize_cloud.cpp
+  src/organized_publisher.cpp
+  src/organized_subscriber.cpp
+)
+
+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 organized_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()

organized_point_cloud_transport/include/organized_point_cloud_transport/organized_publisher.hpp

@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2023, John D'Angelo
+ * 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 PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_PUBLISHER_HPP_
+#define PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2/utils.h>
+
+#include <point_cloud_interfaces/msg/compressed_point_cloud2.hpp>
+#include <point_cloud_transport/point_cloud_transport.hpp>
+#include <point_cloud_transport/simple_publisher_plugin.hpp>
+
+
+namespace organized_point_cloud_transport
+{
+
+class OrganizedPublisher
+  : public point_cloud_transport::SimplePublisherPlugin<
+    point_cloud_interfaces::msg::CompressedPointCloud2>
+{
+public:
+  std::string getTransportName() const override;
+
+  void declareParameters(const std::string & base_topic) override;
+
+  TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override;
+
+  std::string getDataType() const override
+  {
+    return "point_cloud_interfaces/msg/CompressedPointCloud2";
+  }
+
+private:
+
+  void encodeOrganizedPointCloud2(const sensor_msgs::msg::PointCloud2& cloud, std::vector<uint8_t>& compressed_data) const;
+
+  void organizePointCloud2(const sensor_msgs::msg::PointCloud2& cloud, sensor_msgs::msg::PointCloud2& organized) const;
+
+  // tf2 machinery
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  // params for planar projection
+  sensor_msgs::msg::CameraInfo projector_info_;
+
+  // params for compression
+  int png_level_;
+
+};
+}  // namespace organized_point_cloud_transport
+
+#endif  // PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_PUBLISHER_HPP_

organized_point_cloud_transport/include/organized_point_cloud_transport/organized_subscriber.hpp

@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2023, John D'Angelo
+ * 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 PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_SUBSCRIBER_HPP_
+#define PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_SUBSCRIBER_HPP_
+
+#include <string>
+#include <vector>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#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 organized_point_cloud_transport
+{
+
+class OrganizedSubscriber
+  : public point_cloud_transport::SimpleSubscriberPlugin<
+    point_cloud_interfaces::msg::CompressedPointCloud2>
+{
+public:
+  std::string getTransportName() const override;
+
+  void declareParameters() override;
+
+  DecodeResult decodeTyped(const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed)
+  const override;
+
+  std::string getDataType() const override
+  {
+    return "point_cloud_interfaces/msg/CompressedPointCloud2";
+  }
+
+};
+}  // namespace organized_point_cloud_transport
+
+#endif  // PROJECTED_POINT_CLOUD_TRANSPORT__PROJECTED_SUBSCRIBER_HPP_

organized_point_cloud_transport/organized_plugins.xml

@@ -0,0 +1,23 @@
+<library path="organized_point_cloud_transport">
+    <class
+        name="point_cloud_transport/organized_pub"
+        type="organized_point_cloud_transport::OrganizedPublisher"
+        base_class_type="point_cloud_transport::PublisherPlugin">
+        <description>
+            This plugin publishes a CompressedPointCloud2 message as a png-compressed, organized
+            pointcloud. If the
+            pointcloud is not already organized, it is organized via pin-hole projection. The
+            projection is configurable.
+        </description>
+    </class>
+
+    <class
+        name="point_cloud_transport/organized_sub"
+        type="organized_point_cloud_transport::OrganizedSubscriber"
+        base_class_type="point_cloud_transport::SubscriberPlugin">
+        <description>
+            This plugin decompresses a png-compressed, organized CompressedPointCloud2 message into
+            a PointCloud2 message.
+        </description>
+    </class>
+</library>

organized_point_cloud_transport/package.xml

@@ -0,0 +1,38 @@
+<package format="3">
+  <name>organized_point_cloud_transport</name>
+  <version>1.0.5</version>
+  <description>
+    organized_point_cloud_transport provides a plugin to point_cloud_transport for sending point clouds
+    compressed by projecting the cloud onto an image and compressing the image like its a png image.
+    Adapted from: https://arxiv.org/pdf/2202.00719.pdf
+  </description>
+  <author>John D'Angelo</author>
+  <maintainer email="[email protected]">John D'Angelo</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>libpcl-dev</depend>
+  <depend>pcl_conversions</depend>
+  <depend>point_cloud_interfaces</depend>
+  <depend>point_cloud_transport</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>