From c65a0df4654155c61efe4c58c32ddd87454a4ba3 Mon Sep 17 00:00:00 2001 From: Alireza Moayyedi Date: Thu, 12 Feb 2026 19:19:37 +0100 Subject: [PATCH 1/2] Fixed cmake issues and added some improvements --- cloudini_lib/CMakeLists.txt | 36 ++++++++------------------------ cloudini_lib/package.xml | 5 +++-- cloudini_lib/test/CMakeLists.txt | 2 +- cloudini_ros/CMakeLists.txt | 28 +++++++++++++++---------- 4 files changed, 30 insertions(+), 41 deletions(-) diff --git a/cloudini_lib/CMakeLists.txt b/cloudini_lib/CMakeLists.txt index 0b56f70..d750314 100644 --- a/cloudini_lib/CMakeLists.txt +++ b/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 $ ) -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 $ $ 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() diff --git a/cloudini_lib/package.xml b/cloudini_lib/package.xml index 8bb95f3..0eb71ce 100644 --- a/cloudini_lib/package.xml +++ b/cloudini_lib/package.xml @@ -14,10 +14,11 @@ ament_cmake - lz4 + libpcl-common + libpcl-io libzstd-dev + lz4 mcap_vendor - pcl_ros ament_cmake_gtest diff --git a/cloudini_lib/test/CMakeLists.txt b/cloudini_lib/test/CMakeLists.txt index f8253ac..5420643 100644 --- a/cloudini_lib/test/CMakeLists.txt +++ b/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() diff --git a/cloudini_ros/CMakeLists.txt b/cloudini_ros/CMakeLists.txt index 6ab2a94..afa7b30 100644 --- a/cloudini_ros/CMakeLists.txt +++ b/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 $ $ - ${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() From 10f0fc667bbfde9a6188ac19fc711b9ee64acd16 Mon Sep 17 00:00:00 2001 From: Alireza Moayyedi Date: Fri, 13 Feb 2026 16:42:38 +0100 Subject: [PATCH 2/2] convert PointCloud2 messages to compressed SerializedMessage --- .../include/cloudini_ros/conversion_utils.hpp | 24 +++++++- cloudini_ros/src/conversion_utils.cpp | 59 ++++++++++++++++++- 2 files changed, 80 insertions(+), 3 deletions(-) diff --git a/cloudini_ros/include/cloudini_ros/conversion_utils.hpp b/cloudini_ros/include/cloudini_ros/conversion_utils.hpp index 0745440..c3dc852 100644 --- a/cloudini_ros/include/cloudini_ros/conversion_utils.hpp +++ b/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 +#include "cloudini_lib/cloudini.hpp" +#include "cloudini_lib/ros_msg_utils.hpp" + #include +#include #include 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_ diff --git a/cloudini_ros/src/conversion_utils.cpp b/cloudini_ros/src/conversion_utils.cpp index d7b8efe..cb4c5d4 100644 --- a/cloudini_ros/src/conversion_utils.cpp +++ b/cloudini_ros/src/conversion_utils.cpp @@ -14,7 +14,8 @@ * limitations under the License. */ -#include +#include "cloudini_lib/ros_msg_utils.hpp" +#include "cloudini_ros/conversion_utils.hpp" #include #include @@ -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(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 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