Compress and convert sensor_msgs::msg::PointCloud2 to rclcpp::SerializedMessage

cloudini_lib/CMakeLists.txt

@@ -50,21 +50,14 @@ target_include_directories(data_path INTERFACE ${CMAKE_BINARY_DIR}/include)
 
 ##################################################################################
 
-if(ament_cmake_FOUND)
-  message(STATUS "PCL found using ament_cmake")
-  find_package(pcl_ros REQUIRED)
-  set(PCL_SRC src/pcl_conversion.cpp)
+find_package(PCL COMPONENTS common io QUIET)
 
+if(PCL_FOUND)
+  message(STATUS "PCL found in system")
+  add_definitions(${PCL_DEFINITIONS})
+  set(PCL_SRC src/pcl_conversion.cpp)
 else()
-  find_package(PCL QUIET)
-  if(PCL_FOUND)
-    message(STATUS "PCL found in system")
-    add_definitions(${PCL_DEFINITIONS})
-    link_directories(${PCL_LIBRARY_DIRS})
-    set(PCL_SRC src/pcl_conversion.cpp)
-  else()
-    message(STATUS "PCL NOT found")
-  endif()
+  message(STATUS "PCL NOT found")
 endif()
 
 ##################################################################################
@@ -112,22 +105,13 @@ target_include_directories(cloudini_lib
     $<INSTALL_INTERFACE:include>
 )
 
-if(ament_cmake_FOUND)
-  find_package(pcl_ros REQUIRED)
-  target_link_libraries(cloudini_lib PUBLIC ${pcl_ros_TARGETS})
-else()
-  if(PCL_FOUND)
-    target_include_directories(cloudini_lib PUBLIC ${PCL_INCLUDE_DIRS})
-    target_link_libraries(cloudini_lib PUBLIC ${PCL_LIBRARIES})
-  endif()
-endif()
-
 target_link_libraries(cloudini_lib
   PRIVATE
     $<BUILD_INTERFACE:LZ4::lz4_static>
     $<BUILD_INTERFACE:zstd::libzstd_static>
   PUBLIC
-    ${PCL_LIBRARIES}
+    ${PCL_COMMON_LIBRARIES}
+    ${PCL_IO_LIBRARIES}
 )
 
 if(NOT EMSCRIPTEN)
@@ -211,8 +195,6 @@ INSTALL( DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/
 
 if(ament_cmake_FOUND)
   ament_export_targets(cloudini_libTargets HAS_LIBRARY_TARGET)
-  ament_export_libraries(cloudini_lib)
-  ament_export_dependencies(pcl_ros)
-  ament_export_include_directories(include)
+  ament_export_dependencies(PCL)
   ament_package()
 endif()

cloudini_lib/package.xml

@@ -14,10 +14,11 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>lz4</depend>
+  <depend>libpcl-common</depend>
+  <depend>libpcl-io</depend>
   <depend>libzstd-dev</depend>
+  <depend>lz4</depend>
   <depend>mcap_vendor</depend>
-  <depend>pcl_ros</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
 

cloudini_lib/test/CMakeLists.txt

@@ -32,7 +32,7 @@ else()
       GTest::gtest_main)
 
     if(PCL_FOUND)
-      target_link_libraries(test_cloudini ${PCL_LIBRARIES} )
+      target_link_libraries(test_cloudini ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES})
     endif()
 
 endif()

cloudini_ros/CMakeLists.txt

@@ -12,8 +12,8 @@ find_package(pluginlib REQUIRED)
 find_package(point_cloud_interfaces REQUIRED)
 find_package(point_cloud_transport REQUIRED)
 find_package(Draco QUIET)
+find_package(PCL COMPONENTS common REQUIRED)
 find_package(pcl_conversions REQUIRED)
-find_package(pcl_ros REQUIRED)
 
 set(PLUGIN_DEPS
   pluginlib::pluginlib
@@ -36,16 +36,16 @@ target_include_directories(cloudini_ros
   PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
-    ${PCL_INCLUDE_DIRS}
 )
 
 target_link_libraries(cloudini_ros
   PUBLIC
     ${PLUGIN_DEPS}
-    ${PCL_LIBRARIES}
+    ${PCL_COMMON_LIBRARIES}
 )
 
 install(TARGETS cloudini_ros
+  EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/cloudini_ros
@@ -104,7 +104,7 @@ add_executable(test_cloudini_subscriber test/test_cloudini_subscriber.cpp)
 
 target_link_libraries(test_cloudini_subscriber
   cloudini_ros
-  ${PCL_LIBRARIES}
+  ${PCL_COMMON_LIBRARIES}
 )
 
 target_include_directories(test_cloudini_subscriber
@@ -123,15 +123,13 @@ add_executable(test_plugin_subscriber test/test_plugin_subscriber.cpp)
 
 target_link_libraries(test_plugin_subscriber
   cloudini_ros
-  ${PCL_LIBRARIES}
+  ${PCL_COMMON_LIBRARIES}
   ${point_cloud_interfaces_TARGETS}
-  ${pcl_conversions_TARGETS}
   point_cloud_transport::point_cloud_transport
 )
 
 target_include_directories(test_plugin_subscriber
   PRIVATE
-    ${PCL_INCLUDE_DIRS}
     ${pcl_conversions_INCLUDE_DIRS}
 )
 
@@ -178,6 +176,7 @@ target_include_directories(${PLUGIN_NAME} PRIVATE
 )
 
 install(TARGETS ${PLUGIN_NAME}
+  EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PLUGIN_NAME}
@@ -188,10 +187,17 @@ install(
   DESTINATION include/${PLUGIN_NAME}
 )
 
-
 pluginlib_export_plugin_description_file(point_cloud_transport cloudini_plugins.xml)
 
-ament_export_include_directories(include)
-ament_export_libraries(cloudini_ros ${PLUGIN_NAME})
-ament_export_dependencies(${PLUGIN_DEPS})
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  pluginlib
+  rosbag2_cpp
+  point_cloud_transport
+  rclcpp
+  cloudini_lib
+  sensor_msgs
+  point_cloud_interfaces
+  PCL
+)
 ament_package()

cloudini_ros/include/cloudini_ros/conversion_utils.hpp

@@ -17,14 +17,17 @@
 #ifndef CLOUDINI_ROS__CONVERSION_UTILS_HPP_
 #define CLOUDINI_ROS__CONVERSION_UTILS_HPP_
 
-#include <cloudini_lib/cloudini.hpp>
+#include "cloudini_lib/cloudini.hpp"
+#include "cloudini_lib/ros_msg_utils.hpp"
+
 #include <point_cloud_interfaces/msg/compressed_point_cloud2.hpp>
+#include <rclcpp/serialized_message.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 namespace Cloudini {
 
 /**
- * @bried Convert a PointCloud2 message to EncodingInfo
+ * @brief Convert a PointCloud2 message to EncodingInfo
  * Default options (that can be overwitten later) are:
  * - encoding_opt = LOSSY
  * - compression_opt = ZSTD
@@ -37,5 +40,22 @@ EncodingInfo ConvertToEncodingInfo(const sensor_msgs::msg::PointCloud2& msg, flo
 
 EncodingInfo ReadEncodingInfo(const point_cloud_interfaces::msg::CompressedPointCloud2& msg);
 
+/**
+ * @brief Convert a sensor_msgs::msg::PointCloud2 message to cloudini_ros::RosPointCloud2 structure
+ *
+ * @param msg The sensor_msgs::msg::PointCloud2 message to convert
+ * @return The RosPointCloud2 structure
+ */
+cloudini_ros::RosPointCloud2 ConvertToRosPointCloud2(const sensor_msgs::msg::PointCloud2& msg);
+
+/**
+ * @brief Compress and convert a sensor_msgs::msg::PointCloud2 message to a serialized message
+ *
+ * @param msg The input sensor_msgs::msg::PointCloud2 message to compress and convert
+ * @param resolution The resolution to use
+ * @return The compressed pointcloud as a serialized message
+ */
+rclcpp::SerializedMessage ConvertToCompressedCloud(const sensor_msgs::msg::PointCloud2& msg, float resolution);
+
 }  // namespace Cloudini
 #endif  // CLOUDINI_ROS__CONVERSION_UTILS_HPP_

cloudini_ros/src/conversion_utils.cpp

@@ -14,7 +14,8 @@
  * limitations under the License.
  */
 
-#include <cloudini_ros/conversion_utils.hpp>
+#include "cloudini_lib/ros_msg_utils.hpp"
+#include "cloudini_ros/conversion_utils.hpp"
 #include <exception>
 #include <optional>
 
@@ -48,4 +49,60 @@ EncodingInfo ReadEncodingInfo(const point_cloud_interfaces::msg::CompressedPoint
   return DecodeHeader(data);
 }
 
+cloudini_ros::RosPointCloud2 ConvertToRosPointCloud2(const sensor_msgs::msg::PointCloud2& msg) {
+  cloudini_ros::RosPointCloud2 result;
+
+  // Convert header
+  result.ros_header.stamp_sec = msg.header.stamp.sec;
+  result.ros_header.stamp_nsec = msg.header.stamp.nanosec;
+  result.ros_header.frame_id = msg.header.frame_id;
+
+  // Copy point cloud metadata
+  result.height = msg.height;
+  result.width = msg.width;
+  result.point_step = msg.point_step;
+  result.row_step = msg.row_step;
+  result.is_bigendian = msg.is_bigendian;
+  result.is_dense = msg.is_dense;
+
+  // Convert fields
+  result.fields.reserve(msg.fields.size());
+  for (const auto& field : msg.fields) {
+    Cloudini::PointField pf;
+    pf.name = field.name;
+    pf.offset = field.offset;
+    pf.type = static_cast<Cloudini::FieldType>(field.datatype);
+    result.fields.push_back(std::move(pf));
+  }
+
+  // Set data view (no copy, just a view)
+  result.data = Cloudini::ConstBufferView(msg.data.data(), msg.data.size());
+
+  return result;
+}
+
+rclcpp::SerializedMessage ConvertToCompressedCloud(const sensor_msgs::msg::PointCloud2& msg, float resolution) {
+  // Convert to RosPointCloud2
+  auto pc_info = ConvertToRosPointCloud2(msg);
+
+  // Apply resolution profile
+  cloudini_ros::applyResolutionProfile(
+    cloudini_ros::ResolutionProfile{}, pc_info.fields,
+    resolution);
+
+  // Create encoding info and compress
+  const auto encoding_info = cloudini_ros::toEncodingInfo(pc_info);
+  std::vector<uint8_t> compressed_buffer;
+  cloudini_ros::convertPointCloud2ToCompressedCloud(pc_info, encoding_info, compressed_buffer);
+
+  // Create serialized message
+  rclcpp::SerializedMessage output_message(compressed_buffer.size());
+  auto& rcl_msg = output_message.get_rcl_serialized_message();
+  std::memcpy(rcl_msg.buffer, compressed_buffer.data(), compressed_buffer.size());
+  rcl_msg.buffer_length = compressed_buffer.size();
+
+  return output_message;
+}
+
+
 }  // namespace Cloudini