diff --git a/draco_point_cloud_transport/CMakeLists.txt b/draco_point_cloud_transport/CMakeLists.txt index f8dffce..48dda28 100644 --- a/draco_point_cloud_transport/CMakeLists.txt +++ b/draco_point_cloud_transport/CMakeLists.txt @@ -56,40 +56,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -# TODO: Fix tests -# if (CATKIN_ENABLE_TESTING) -# find_package(roslint REQUIRED) - -# # catkin_lint - checks validity of package.xml and CMakeLists.txt -# # ROS buildfarm calls this without any environment and with empty rosdep cache, -# # so we have problems reading the list of packages from env -# # see https://github.com/ros-infrastructure/ros_buildfarm/issues/923 -# if(DEFINED ENV{ROS_HOME}) -# #catkin_lint: ignore_once env_var -# set(ROS_HOME "$ENV{ROS_HOME}") -# else() -# #catkin_lint: ignore_once env_var -# set(ROS_HOME "$ENV{HOME}/.ros") -# endif() -# #catkin_lint: ignore_once env_var -# if(DEFINED ENV{ROS_ROOT} AND EXISTS "${ROS_HOME}/rosdep/sources.cache") -# roslint_custom(catkin_lint "-W2" .) -# endif() - -# # Roslint C++ - checks formatting and some other rules for C++ files - -# file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h include/*.hpp) -# file(GLOB_RECURSE ROSLINT_SRC src/*.cpp src/*.hpp src/*.h) -# #file(GLOB_RECURSE ROSLINT_TEST test/*.cpp) - -# set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ -# -build/header_guard,-readability/namespace,-whitespace/braces,-runtime/references,\ -# -build/c++11,-readability/nolint,-readability/todo,-legal/copyright,-build/namespaces") -# roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC}) - -# roslint_add_test() -# endif() - ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies(${dependencies}) diff --git a/draco_point_cloud_transport/README.md b/draco_point_cloud_transport/README.md index 67c99fb..33a9ef2 100644 --- a/draco_point_cloud_transport/README.md +++ b/draco_point_cloud_transport/README.md @@ -42,9 +42,9 @@ By adjusting the **encode_speed** and **dedode_speed** parameters, one can adjus - "ny" - NORMAL - "nz" - NORMAL - all others are encoded as GENERIC - + To specify custom quantization, one can either edit the list of recognized names or use **expert_quantization** and **expert_attribute_type** options. - + ### Expert Quantization **Expert_quantization** option tell the encoder to use custom quantization values for point cloud attributes. Multiple POSITION attribute can therefore be encoded with varying quantization levels. @@ -55,7 +55,7 @@ To set a quantization for a PointField entry "x" of point cloud which will be ad Example: ```bash -$ rosparam set /base_topic/draco/attribute_mapping/quantization_bits/x 16 +ros2 param set //draco/attribute_mapping/quantization_bits/x 16 ``` When using **expert_quantization**, user must specify the quantization bits for all PointField entries of point cloud. @@ -70,11 +70,11 @@ To set a type for a PointField entry "x" of point cloud which will be advertised Example: ```bash -$ rosparam set /base_topic/draco/attribute_mapping/attribute_type/x "'POSITION'" +ros2 param set //draco/attribute_mapping/attribute_type/x "'POSITION'" ``` When using **expert_attribute_types**, user must specify the type for all PointField entries of point cloud. Accepted types are: - - POSITION + - POSITION - NORMAL - COLOR - TEX_COORD @@ -83,7 +83,7 @@ When using **expert_attribute_types**, user must specify the type for all PointF When encoding rgb/rgba COLOR, user can specify to use the common rgba tweak of ROS (encoding rgba as 4 instances of 1 Byte instead of 1 instance of float32). To inform the encoder, that PointField entry "rgb" should be handled with the tweak, set parameter: ```bash -$ rosparam set /base_topic/draco/attribute_mapping/rgba_tweak/rgb true +ros2 param set //draco/attribute_mapping/rgba_tweak/rgb true ``` ## Subscriber diff --git a/draco_point_cloud_transport/cfg/DracoPublisher.cfg b/draco_point_cloud_transport/cfg/DracoPublisher.cfg deleted file mode 100755 index eb05913..0000000 --- a/draco_point_cloud_transport/cfg/DracoPublisher.cfg +++ /dev/null @@ -1,42 +0,0 @@ -#! /usr/bin/env python - -PACKAGE='draco_point_cloud_transport' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - - - -gen.add("encode_speed", int_t, 0, "0 = slowest speed, but the best compression 10 = fastest, but the worst compression.", 7, 0, 10) -gen.add("decode_speed", int_t, 0, "0 = slowest speed, but the best compression 10 = fastest, but the worst compression.", 7, 0, 10) - -method_enum = gen.enum([ gen.const("Auto", int_t, 0, "Draco chooses appropriate compression"), - gen.const("KD_tree", int_t, 1, "Force KD-tree"), - gen.const("Sequential", int_t, 2, "Force Sequential")], - "An enum to set method of encoding") - -gen.add("encode_method", int_t, 0, "Encoding process method, 0 = auto, 1 = KD-tree, 2 = sequential", 0, 0, 2, edit_method=method_enum) - -#deduplicate_enum = gen.enum([ gen.const("Deduplication_OFF", bool_t, False, "Do NOT remove duplicate point entries."), -# gen.const("DEDUPLICATION_ON", bool_t, True, "Remove duplicate point entries.")], -# "An enum to enable/disable deduplication of point entries") - -gen.add("deduplicate", bool_t, 0, "Remove duplicate point entries.", True)#, edit_method=deduplicate_enum) - -#force_quantization_enum = gen.enum([ gen.const("Quantization_OFF", bool_t, False, "Do NOT quantize attribute values."), -# gen.const("Quantization_ON", bool_t, True, "Quantize attribute values.")], -# "An enum to enable/disable quantization of attribute values") - -gen.add("force_quantization", bool_t, 0, "Force attribute quantization. Attributes of type float32 must be quantized for kd-tree encoding.", True)#, edit_method=force_quantization_enum) - -gen.add("quantization_POSITION", int_t, 0, "Number of bits for quantization of POSITION type attributes.", 14, 1, 31) -gen.add("quantization_NORMAL", int_t, 0, "Number of bits for quantization of NORMAL type attributes.", 14, 1, 31) -gen.add("quantization_COLOR", int_t, 0, "Number of bits for quantization of COLOR type attributes.", 14, 1, 31) -gen.add("quantization_TEX_COORD", int_t, 0, "Number of bits for quantization of TEX_COORD type attributes.", 14, 1, 31) -gen.add("quantization_GENERIC", int_t, 0, "Number of bits for quantization of GENERIC type attributes.", 14, 1, 31) - -gen.add("expert_quantization", bool_t, 0, "WARNING: Apply user specified quantization for PointField entries. User must specify all entries at parameter server.", False) -gen.add("expert_attribute_types", bool_t, 0, "WARNING: Apply user specified attribute types for PointField entries. User must specify all entries at parameter server.", False) - -exit(gen.generate(PACKAGE, "DracoPublisher", "DracoPublisher")) diff --git a/draco_point_cloud_transport/cfg/DracoSubscriber.cfg b/draco_point_cloud_transport/cfg/DracoSubscriber.cfg deleted file mode 100755 index cc9fbdf..0000000 --- a/draco_point_cloud_transport/cfg/DracoSubscriber.cfg +++ /dev/null @@ -1,15 +0,0 @@ -#! /usr/bin/env python - -PACKAGE='draco_point_cloud_transport' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("SkipDequantizationPOSITION", bool_t, 0, "Tells decoder to skip dequantization of POSITION attributes", False) -gen.add("SkipDequantizationNORMAL", bool_t, 0, "Tells decoder to skip dequantization of NORMAL attributes", False) -gen.add("SkipDequantizationCOLOR", bool_t, 0, "Tells decoder to skip dequantization of COLOR attributes", False) -gen.add("SkipDequantizationTEX_COORD", bool_t, 0, "Tells decoder to skip dequantization of TEX_COORD attributes", False) -gen.add("SkipDequantizationGENERIC", bool_t, 0, "Tells decoder to skip dequantization of GENERIC attributes", False) - -exit(gen.generate(PACKAGE, "DracoSubscriber", "DracoSubscriber")) diff --git a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp index 9e50077..4ac8f32 100644 --- a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp +++ b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp @@ -54,6 +54,8 @@ class DracoPublisher public: std::string getTransportName() const override; + void declareParameters(const std::string & base_topic) override; + TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override; static void registerPositionField(const std::string & field); diff --git a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp index d71d415..a9d2105 100644 --- a/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp +++ b/draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp @@ -51,6 +51,8 @@ class DracoSubscriber public: std::string getTransportName() const override; + void declareParameters() override; + DecodeResult decodeTyped(const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed) const override; diff --git a/draco_point_cloud_transport/src/draco_publisher.cpp b/draco_point_cloud_transport/src/draco_publisher.cpp index 6cb9a19..619fbb7 100644 --- a/draco_point_cloud_transport/src/draco_publisher.cpp +++ b/draco_point_cloud_transport/src/draco_publisher.cpp @@ -73,6 +73,132 @@ static std::unordered_map attribute {"normal_z", draco::GeometryAttribute::Type::NORMAL}, }; +void DracoPublisher::declareParameters(const std::string & base_topic) +{ + declareParam(std::string("encode_speed"), 7); + declareParam(std::string("decode_speed"), 7); + declareParam(std::string("encode_method"), 0); + declareParam(std::string("deduplicate"), true); + declareParam(std::string("force_quantization"), true); + declareParam(std::string("quantization_POSITION"), 14); + declareParam(std::string("quantization_NORMAL"), 14); + declareParam(std::string("quantization_COLOR"), 14); + declareParam(std::string("quantization_TEX_COORD"), 14); + declareParam(std::string("quantization_GENERIC"), 14); + declareParam(std::string("expert_quantization"), true); + declareParam(std::string("expert_attribute_types"), true); + + declareParam(base_topic + "/attribute_mapping/attribute_type/x", "POSITION"); + declareParam(base_topic + "/attribute_mapping/attribute_type/y", "POSITION"); + declareParam(base_topic + "/attribute_mapping/attribute_type/z", "POSITION"); + declareParam(base_topic + "/attribute_mapping/quantization_bits/x", 16); + declareParam(base_topic + "/attribute_mapping/quantization_bits/y", 16); + declareParam(base_topic + "/attribute_mapping/quantization_bits/z", 16); + declareParam(base_topic + "/attribute_mapping/quantization_bits/rgb", 16); + declareParam(base_topic + "/attribute_mapping/rgba_tweak/rgb", true); + declareParam(base_topic + "/attribute_mapping/rgba_tweak/rgba", false); + + auto param_change_callback = + [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult + { + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + for (auto parameter : parameters) { + if (parameter.get_name() == "expert_quantization") { + config_.expert_quantization = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "force_quantization") { + config_.force_quantization = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "encode_speed") { + int value = parameter.as_int(); + if (value >= 0 && value <= 10) { + config_.encode_speed = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "encode_speed value range should be between [0, 10] "); + } + return result; + } else if (parameter.get_name() == "decode_speed") { + int value = parameter.as_int(); + if (value >= 0 && value <= 10) { + config_.decode_speed = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "decode_speed value range should be between [0, 10] "); + } + return result; + } else if (parameter.get_name() == "method_enum") { + int value = parameter.as_int(); + if (value >= 0 && value <= 2) { + config_.method_enum = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "method_enum value range should be between [0, 2], " + "0 = auto, 1 = KD-tree, 2 = sequential "); + } + return result; + } else if (parameter.get_name() == "encode_method") { + config_.encode_method = parameter.as_int(); + return result; + } else if (parameter.get_name() == "deduplicate") { + config_.deduplicate = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "quantization_POSITION") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) { + config_.quantization_POSITION = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_POSITION value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "quantization_NORMAL") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) { + config_.quantization_NORMAL = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_NORMAL value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "quantization_COLOR") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) { + config_.quantization_COLOR = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_COLOR value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "quantization_TEX_COORD") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) { + config_.quantization_TEX_COORD = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_TEX_COORD value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "quantization_GENERIC") { + int value = parameter.as_int(); + if (value >= 1 && value <= 31) { + config_.quantization_GENERIC = value; + } else { + RCLCPP_ERROR_STREAM( + getLogger(), "quantization_GENERIC value range should be between [1, 31] "); + } + return result; + } else if (parameter.get_name() == "expert_attribute_types") { + config_.expert_attribute_types = parameter.as_bool(); + return result; + } + } + return result; + }; + setParamCallback(param_change_callback); +} + cras::expected, std::string> DracoPublisher::convertPC2toDraco( const sensor_msgs::msg::PointCloud2 & PC2, const std::string & topic, bool deduplicate, bool expert_encoding) const @@ -103,9 +229,8 @@ cras::expected, std::string> DracoPublisher:: for (const auto & field : PC2.fields) { if (expert_encoding) { // find attribute type in user specified parameters rgba_tweak = false; - if (getParam( - topic + "/draco/attribute_mapping/attribute_type/" + field.name, + topic + "/attribute_mapping/attribute_type/" + field.name, expert_attribute_data_type)) { if (expert_attribute_data_type == "POSITION") { // if data type is POSITION @@ -114,7 +239,7 @@ cras::expected, std::string> DracoPublisher:: attribute_type = draco::GeometryAttribute::NORMAL; } else if (expert_attribute_data_type == "COLOR") { // if data type is COLOR attribute_type = draco::GeometryAttribute::COLOR; - getParam(topic + "/draco/attribute_mapping/rgba_tweak/" + field.name, rgba_tweak); + getParam(topic + "/attribute_mapping/rgba_tweak/" + field.name, rgba_tweak); } else if (expert_attribute_data_type == "TEX_COORD") { // if data type is TEX_COORD attribute_type = draco::GeometryAttribute::TEX_COORD; } else if (expert_attribute_data_type == "GENERIC") { // if data type is GENERIC @@ -131,7 +256,7 @@ cras::expected, std::string> DracoPublisher:: "Using regular type recognition instead."); RCLCPP_INFO_STREAM( getLogger(), "To set attribute type for " + field.name + " field entry, set " + topic + - "/draco/attribute_mapping/attribute_type/" + field.name); + "/attribute_mapping/attribute_type/" + field.name); expert_settings_ok = false; } } @@ -280,7 +405,7 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped( for (const auto & field : rawDense.fields) { if (getParam( - base_topic_ + "/draco/attribute_mapping/quantization_bits/" + field.name, + base_topic_ + "/attribute_mapping/quantization_bits/" + field.name, attribute_quantization_bits)) { expert_encoder.SetAttributeQuantization(att_id, attribute_quantization_bits); @@ -290,7 +415,7 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped( " field entry. Using regular encoder instead."); RCLCPP_INFO_STREAM( getLogger(), "To set quantization for " + field.name + " field entry, set " + - base_topic_ + "/draco/attribute_mapping/quantization_bits/" + field.name); + base_topic_ + "/attribute_mapping/quantization_bits/" + field.name); expert_settings_ok = false; } att_id++; @@ -314,7 +439,6 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped( } } // expert encoder end - // regular encoder if ((!config_.expert_quantization) || (!expert_settings_ok)) { draco::Encoder encoder; diff --git a/draco_point_cloud_transport/src/draco_subscriber.cpp b/draco_point_cloud_transport/src/draco_subscriber.cpp index d688404..8c3b974 100644 --- a/draco_point_cloud_transport/src/draco_subscriber.cpp +++ b/draco_point_cloud_transport/src/draco_subscriber.cpp @@ -47,6 +47,41 @@ namespace draco_point_cloud_transport { +void DracoSubscriber::declareParameters() +{ + declareParam(std::string("SkipDequantizationPOSITION"), false); + declareParam(std::string("SkipDequantizationNORMAL"), false); + declareParam(std::string("SkipDequantizationCOLOR"), false); + declareParam(std::string("SkipDequantizationTEX_COORD"), false); + declareParam(std::string("SkipDequantizationGENERIC"), false); + + auto param_change_callback = + [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult + { + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + for (auto parameter : parameters) { + if (parameter.get_name() == "SkipDequantizationPOSITION") { + config_.SkipDequantizationPOSITION = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "SkipDequantizationNORMAL") { + config_.SkipDequantizationNORMAL = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "SkipDequantizationCOLOR") { + config_.SkipDequantizationCOLOR = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "SkipDequantizationTEX_COORD") { + config_.SkipDequantizationTEX_COORD = parameter.as_bool(); + return result; + } else if (parameter.get_name() == "SkipDequantizationGENERIC") { + config_.SkipDequantizationGENERIC = parameter.as_bool(); + return result; + } + } + return result; + }; + setParamCallback(param_change_callback); +} //! Method for converting into sensor_msgs::msg::PointCloud2 cras::expected convertDracoToPC2( diff --git a/pcl_point_cloud_transport/CMakeLists.txt b/pcl_point_cloud_transport/CMakeLists.txt new file mode 100644 index 0000000..8cedad7 --- /dev/null +++ b/pcl_point_cloud_transport/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.5) + +project(pcl_point_cloud_transport) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(PCL REQUIRED QUIET COMPONENTS common io) +find_package(pcl_conversions REQUIRED) +find_package(pluginlib REQUIRED) +find_package(point_cloud_transport REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +set(dependencies + pcl_conversions + pluginlib + point_cloud_transport + rclcpp + sensor_msgs + std_msgs +) + +include_directories(include ${PCL_COMMON_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} + SHARED + src/manifest.cpp + src/pcl_publisher.cpp + src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp + src/pcl_subscriber.cpp +) + +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) + +add_executable(pclcontainer_subscriber + examples/subscriber.cpp + src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp +) +ament_target_dependencies(pclcontainer_subscriber + pcl_conversions + point_cloud_transport + rclcpp + sensor_msgs +) +target_link_libraries(pclcontainer_subscriber ${PCL_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +# Install executables +install( + TARGETS pclcontainer_subscriber + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +pluginlib_export_plugin_description_file(point_cloud_transport pcl_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} PCL) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/pcl_point_cloud_transport/examples/subscriber.cpp b/pcl_point_cloud_transport/examples/subscriber.cpp new file mode 100644 index 0000000..d91e037 --- /dev/null +++ b/pcl_point_cloud_transport/examples/subscriber.cpp @@ -0,0 +1,89 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + pcl_point_cloud_transport::PCLContainer, + sensor_msgs::msg::PointCloud2); + +class ShowPointCloud : public rclcpp::Node +{ +public: + ShowPointCloud() + : Node("showpointcloud") + { + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + initialize(); + } + +private: + void initialize() + { + // Set quality of service profile based on command line options. + auto qos = rclcpp::QoS( + rclcpp::QoSInitialization( + // The history policy determines how messages are saved until taken by + // the reader. + // KEEP_ALL saves all messages until they are taken. + // KEEP_LAST enforces a limit on the number of messages that are saved, + // specified by the "depth" parameter. + history_policy_, + // Depth represents how many messages to store in history when the + // history policy is KEEP_LAST. + depth_ + )); + // The reliability policy can be reliable, meaning that the underlying transport layer will try + // ensure that every message gets received in order, or best effort, meaning that the transport + // makes no guarantees about the order or reliability of delivery. + qos.reliability(reliability_policy_); + auto callback = + [this](const pcl_point_cloud_transport::PCLContainer & container) { + process_pointcloud(container, this->get_logger()); + }; + + RCLCPP_INFO(this->get_logger(), "Subscribing to topic '/pct/point_cloud/pcl'"); + sub_ = create_subscription( + "/pct/point_cloud/pcl", + rclcpp::SensorDataQoS(), callback); + } + + /// Convert the ROS Image message to an OpenCV matrix and display it to the user. + // \param[in] container The image message to show. + void process_pointcloud( + const pcl_point_cloud_transport::PCLContainer & container, rclcpp::Logger logger) + { + RCLCPP_INFO(logger, "Received point_cloud #%s", container.header().frame_id.c_str()); + } + + rclcpp::Subscription::SharedPtr sub_; + size_t depth_ = rmw_qos_profile_default.depth; + rmw_qos_reliability_policy_t reliability_policy_ = rmw_qos_profile_default.reliability; + rmw_qos_history_policy_t history_policy_ = rmw_qos_profile_default.history; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp new file mode 100644 index 0000000..e4789c5 --- /dev/null +++ b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_publisher.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PCL_POINT_CLOUD_TRANSPORT__PCL_PUBLISHER_HPP_ +#define PCL_POINT_CLOUD_TRANSPORT__PCL_PUBLISHER_HPP_ + +#include +#include + +#include + +#include +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +#include + + +namespace pcl_point_cloud_transport +{ + +class PCLPublisher + : public point_cloud_transport::SimplePublisherPlugin< + pcl_point_cloud_transport::PCLContainer> +{ +public: + std::string getTransportName() const override; + + void declareParameters(const std::string & base_topic) override; + + TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override; +}; +} // namespace pcl_point_cloud_transport + +#endif // PCL_POINT_CLOUD_TRANSPORT__PCL_PUBLISHER_HPP_ diff --git a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp new file mode 100644 index 0000000..eda60c7 --- /dev/null +++ b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp @@ -0,0 +1,209 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PCL_POINT_CLOUD_TRANSPORT__PCL_SENSOR_MSGS_POINTCLOUD2_TYPE_ADAPTER_HPP_ +#define PCL_POINT_CLOUD_TRANSPORT__PCL_SENSOR_MSGS_POINTCLOUD2_TYPE_ADAPTER_HPP_ + +#include +#include + +#include + +#include +#include +#include // NOLINT[build/include_order] + +#include +#include + +namespace pcl_point_cloud_transport +{ +namespace detail +{ +// TODO(audrow): Replace with std::endian when C++ 20 is available +// https://en.cppreference.com/w/cpp/types/endian +enum class endian +{ +#ifdef _WIN32 + little = 0, + big = 1, + native = little +#else + little = __ORDER_LITTLE_ENDIAN__, + big = __ORDER_BIG_ENDIAN__, + native = __BYTE_ORDER__ +#endif +}; + +} // namespace detail + +class PCLContainer +{ + static constexpr bool is_bigendian_system = detail::endian::native == detail::endian::big; + +public: + using PointCloud2MsgsImageStorageType = std::variant< + std::nullptr_t, + std::unique_ptr, + std::shared_ptr + >; + + PCLContainer() = default; + + explicit PCLContainer(const PCLContainer & other) + : header_(other.header_), frame_(other.frame_), is_bigendian_(other.is_bigendian_) + { + if (std::holds_alternative>(other.storage_)) { + storage_ = std::get>(other.storage_); + } else if ( // NOLINT + std::holds_alternative>(other.storage_)) + { + storage_ = std::make_unique( + *std::get>(other.storage_)); + } + } + + PCLContainer & operator=(const PCLContainer & other) + { + if (this != &other) { + header_ = other.header_; + pcl::copyPointCloud(other.frame_, frame_); + is_bigendian_ = other.is_bigendian_; + if (std::holds_alternative>(other.storage_)) { + storage_ = std::get>(other.storage_); + } else if ( // NOLINT + std::holds_alternative>(other.storage_)) + { + storage_ = std::make_unique( + *std::get>(other.storage_)); + } else if (std::holds_alternative(other.storage_)) { + storage_ = nullptr; + } + } + return *this; + } + + /// Store an owning pointer to a sensor_msg::msg::PointCloud2, and create a pcl::PCLPointCloud2 + /// that references it. + explicit PCLContainer( + std::unique_ptr unique_sensor_msgs_point_cloud2); + + /// Store an owning pointer to a sensor_msg::msg::PointCloud2, and create a pcl::PCLPointCloud2 + /// that references it. + explicit PCLContainer( + std::shared_ptr shared_sensor_msgs_point_cloud2); + + /// Shallow copy the given pcl::PCLPointCloud2 into this class, but do not own the data directly. + PCLContainer( + const pcl::PCLPointCloud2 & pcl_frame, + const std_msgs::msg::Header & header, + bool is_bigendian = is_bigendian_system); + + /// Move the given pcl::PCLPointCloud2 into this class. + PCLContainer( + pcl::PCLPointCloud2 && pcl_frame, + const std_msgs::msg::Header & header, + bool is_bigendian = is_bigendian_system); + + /// Copy the sensor_msgs::msg::PointCloud2 into this contain and create a pcl::PCLPointCloud2 + /// that references it. + explicit PCLContainer(const sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2); + + /// Return true if this class owns the data the cv_mat references. + /** + * Note that this does not check if the pcl::PCLPointCloud2 owns its own data, only if + * this class owns a sensor_msgs::msg::PointCloud2 that the pcl::PCLPointCloud2 references. + */ + bool + is_owning() const; + + /// Const access the pcl::PCLPointCloud2 in this class. + const pcl::PCLPointCloud2 & + pcl() const; + + /// Get a shallow copy of the pcl::PCLPointCloud2 that is in this class. + /** + * Note that if you want to let this container go out of scope you should + * make a deep copy with pcl::PCLPointCloud2::clone() beforehand. + */ + pcl::PCLPointCloud2 + pcl(); + + /// Const access the ROS Header. + const std_msgs::msg::Header & + header() const; + + /// Access the ROS Header. + std_msgs::msg::Header & + header(); + + /// Get shared const pointer to the sensor_msgs::msg::PointCloud2 if available, otherwise nullptr. + std::shared_ptr + get_sensor_msgs_msg_point_cloud2_pointer() const; + + /// Get copy as a unique pointer to the sensor_msgs::msg::PointCloud2. + std::unique_ptr + get_sensor_msgs_msg_point_cloud2_pointer_copy() const; + + /// Get a copy of the image as a sensor_msgs::msg::PointCloud2. + sensor_msgs::msg::PointCloud2 + get_sensor_msgs_msg_point_cloud2_copy() const; + + /// Get a copy of the image as a sensor_msgs::msg::PointCloud2. + void + get_sensor_msgs_msg_point_cloud2_copy(sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2) + const; + + /// Return true if the data is stored in big endian, otherwise return false. + bool + is_bigendian() const; + +private: + std_msgs::msg::Header header_; + pcl::PCLPointCloud2 frame_; + PointCloud2MsgsImageStorageType storage_; + bool is_bigendian_; +}; + +} // namespace pcl_point_cloud_transport + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = pcl_point_cloud_transport::PCLContainer; + using ros_message_type = sensor_msgs::msg::PointCloud2; + + static + void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + sensor_msgs::msg::PointCloud2 msg; + pcl::PCLPointCloud2 cloud = source.pcl(); + // pcl::toROSMsg(cloud, msg); + pcl_conversions::moveFromPCL(cloud, destination); + } + + static + void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = pcl_point_cloud_transport::PCLContainer(source); + } +}; +#endif // PCL_POINT_CLOUD_TRANSPORT__PCL_SENSOR_MSGS_POINTCLOUD2_TYPE_ADAPTER_HPP_ diff --git a/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_subscriber.hpp b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_subscriber.hpp new file mode 100644 index 0000000..9995bc2 --- /dev/null +++ b/pcl_point_cloud_transport/include/pcl_point_cloud_transport/pcl_subscriber.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PCL_POINT_CLOUD_TRANSPORT__PCL_SUBSCRIBER_HPP_ +#define PCL_POINT_CLOUD_TRANSPORT__PCL_SUBSCRIBER_HPP_ + +#include + +#include +#include + +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + pcl_point_cloud_transport::PCLContainer, + sensor_msgs::msg::PointCloud2); + +namespace pcl_point_cloud_transport +{ + +class PCLSubscriber + : public point_cloud_transport::SimpleSubscriberPlugin< + pcl_point_cloud_transport::PCLContainer> +{ +public: + std::string getTransportName() const override; + + void declareParameters() override; + + DecodeResult decodeTyped(const pcl_point_cloud_transport::PCLContainer & compressed) + const override; +}; +} // namespace pcl_point_cloud_transport + +#endif // PCL_POINT_CLOUD_TRANSPORT__PCL_SUBSCRIBER_HPP_ diff --git a/pcl_point_cloud_transport/package.xml b/pcl_point_cloud_transport/package.xml new file mode 100644 index 0000000..4e7743d --- /dev/null +++ b/pcl_point_cloud_transport/package.xml @@ -0,0 +1,33 @@ + + pcl_point_cloud_transport + 0.0.0 + + pcl_point_cloud_transport provides a plugin to point_cloud_transport for sending point clouds + encoded with PCL + + Jakub Paplham + Alejandro Hernandez Cordero + BSD + + https://github.com/john-maidbot/point_cloud_transport_plugins + https://github.com/john-maidbot/point_cloud_transport_plugins + https://github.com/john-maidbot/point_cloud_transport_plugins/issues + + ament_cmake + + libpcl-common + libpcl-io + pcl_conversions + pluginlib + point_cloud_transport + rclcpp + sensor_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/pcl_point_cloud_transport/pcl_plugins.xml b/pcl_point_cloud_transport/pcl_plugins.xml new file mode 100644 index 0000000..24bb5ae --- /dev/null +++ b/pcl_point_cloud_transport/pcl_plugins.xml @@ -0,0 +1,19 @@ + + + + PCL publisher + + + + + + PCL + + + diff --git a/pcl_point_cloud_transport/src/manifest.cpp b/pcl_point_cloud_transport/src/manifest.cpp new file mode 100644 index 0000000..c2337bc --- /dev/null +++ b/pcl_point_cloud_transport/src/manifest.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include + +PLUGINLIB_EXPORT_CLASS( + pcl_point_cloud_transport::PCLPublisher, + point_cloud_transport::PublisherPlugin) +PLUGINLIB_EXPORT_CLASS( + pcl_point_cloud_transport::PCLSubscriber, + point_cloud_transport::SubscriberPlugin) diff --git a/pcl_point_cloud_transport/src/pcl_publisher.cpp b/pcl_point_cloud_transport/src/pcl_publisher.cpp new file mode 100644 index 0000000..d114686 --- /dev/null +++ b/pcl_point_cloud_transport/src/pcl_publisher.cpp @@ -0,0 +1,47 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + pcl_point_cloud_transport::PCLContainer, + sensor_msgs::msg::PointCloud2); + +namespace pcl_point_cloud_transport +{ + +void PCLPublisher::declareParameters(const std::string & /*base_topic*/) +{ +} + +std::string PCLPublisher::getTransportName() const +{ + return "pcl"; +} + +PCLPublisher::TypedEncodeResult PCLPublisher::encodeTyped( + const sensor_msgs::msg::PointCloud2 & raw) const +{ + pcl_point_cloud_transport::PCLContainer container(raw); + return std::optional(container); +} + +} // namespace pcl_point_cloud_transport diff --git a/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp b/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp new file mode 100644 index 0000000..aadebce --- /dev/null +++ b/pcl_point_cloud_transport/src/pcl_sensor_msgs_pointcloud2_type_adapter.cpp @@ -0,0 +1,151 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include // NOLINT[build/include_order] + +#include +#include + +#include "pcl_point_cloud_transport/pcl_sensor_msgs_pointcloud2_type_adapter.hpp" + +namespace pcl_point_cloud_transport +{ + +namespace +{ +template +struct NotNull +{ + NotNull(const T * pointer_in, const char * msg) + : pointer(pointer_in) + { + if (pointer == nullptr) { + throw std::invalid_argument(msg); + } + } + + const T * pointer; +}; + +} // namespace + +PCLContainer::PCLContainer( + std::unique_ptr unique_sensor_msgs_point_cloud2) +: header_(NotNull( + unique_sensor_msgs_point_cloud2.get(), + "unique_sensor_msgs_point_cloud2 cannot be nullptr" +).pointer->header) +{ + pcl_conversions::toPCL(*unique_sensor_msgs_point_cloud2.get(), frame_); + storage_ = std::move(unique_sensor_msgs_point_cloud2); +} + +PCLContainer::PCLContainer( + std::shared_ptr shared_sensor_msgs_point_cloud2) +: header_(shared_sensor_msgs_point_cloud2->header) +{ + pcl_conversions::toPCL(*shared_sensor_msgs_point_cloud2.get(), frame_); + storage_ = shared_sensor_msgs_point_cloud2; +} + +PCLContainer::PCLContainer( + const pcl::PCLPointCloud2 & pcl_frame, + const std_msgs::msg::Header & header, + bool is_bigendian) +: header_(header), + frame_(pcl_frame), + storage_(nullptr), + is_bigendian_(is_bigendian) +{} + +PCLContainer::PCLContainer( + pcl::PCLPointCloud2 && pcl_frame, + const std_msgs::msg::Header & header, + bool is_bigendian) +: header_(header), + frame_(std::forward(pcl_frame)), + storage_(nullptr), + is_bigendian_(is_bigendian) +{} + +PCLContainer::PCLContainer( + const sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2) +: PCLContainer(std::make_unique(sensor_msgs_point_cloud2)) +{} + +bool +PCLContainer::is_owning() const +{ + return std::holds_alternative(storage_); +} + +const pcl::PCLPointCloud2 & +PCLContainer::pcl() const +{ + return frame_; +} + +pcl::PCLPointCloud2 +PCLContainer::pcl() +{ + return frame_; +} + +const std_msgs::msg::Header & +PCLContainer::header() const +{ + return header_; +} + +std_msgs::msg::Header & +PCLContainer::header() +{ + return header_; +} + +std::shared_ptr +PCLContainer::get_sensor_msgs_msg_point_cloud2_pointer() const +{ + if (!std::holds_alternative>(storage_)) { + return nullptr; + } + return std::get>(storage_); +} + +std::unique_ptr +PCLContainer::get_sensor_msgs_msg_point_cloud2_pointer_copy() const +{ + auto unique_image = std::make_unique(); + this->get_sensor_msgs_msg_point_cloud2_copy(*unique_image); + return unique_image; +} + +void +PCLContainer::get_sensor_msgs_msg_point_cloud2_copy( + sensor_msgs::msg::PointCloud2 & sensor_msgs_point_cloud2) const +{ + // TODO(ahcorde): Implement this emthod +} + +bool +PCLContainer::is_bigendian() const +{ + return is_bigendian_; +} + +} // namespace pcl_point_cloud_transport diff --git a/pcl_point_cloud_transport/src/pcl_subscriber.cpp b/pcl_point_cloud_transport/src/pcl_subscriber.cpp new file mode 100644 index 0000000..d3ac839 --- /dev/null +++ b/pcl_point_cloud_transport/src/pcl_subscriber.cpp @@ -0,0 +1,57 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include + +#include + +#include + +namespace pcl_point_cloud_transport +{ +void PCLSubscriber::declareParameters() +{ +} + +std::string PCLSubscriber::getTransportName() const +{ + return "pcl"; +} + +PCLSubscriber::DecodeResult PCLSubscriber::decodeTyped( + const pcl_point_cloud_transport::PCLContainer & container) const +{ + auto message = std::make_shared(); + + pcl::PCLPointCloud2 pcl_pc2 = container.pcl(); + + std::stringstream ss; + ss << "output.pcd"; + std::cerr << "Data saved to " << ss.str() << std::endl; + + pcl::io::savePCDFile( + ss.str(), pcl_pc2, Eigen::Vector4f::Zero(), + Eigen::Quaternionf::Identity(), true); + + pcl_conversions::moveFromPCL(pcl_pc2, *message.get()); + + return message; +} + +} // namespace pcl_point_cloud_transport