diff --git a/draco_point_cloud_transport/src/draco_publisher.cpp b/draco_point_cloud_transport/src/draco_publisher.cpp index 15dc90f..dcffed5 100644 --- a/draco_point_cloud_transport/src/draco_publisher.cpp +++ b/draco_point_cloud_transport/src/draco_publisher.cpp @@ -229,15 +229,15 @@ void DracoPublisher::declareParameters(const std::string & base_topic) expert_attribute_types_paramDescriptor.name, config_.expert_attribute_types, expert_attribute_types_paramDescriptor); - 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); + declareParam("attribute_mapping.attribute_type.x", "POSITION"); + declareParam("attribute_mapping.attribute_type.y", "POSITION"); + declareParam("attribute_mapping.attribute_type.z", "POSITION"); + declareParam("attribute_mapping.quantization_bits.x", 16); + declareParam("attribute_mapping.quantization_bits.y", 16); + declareParam("attribute_mapping.quantization_bits.z", 16); + declareParam("attribute_mapping.quantization_bits.rgb", 16); + declareParam("attribute_mapping.rgba_tweak.rgb", true); + declareParam("attribute_mapping.rgba_tweak.rgba", false); auto param_change_callback = [this](const std::vector & parameters) -> rcl_interfaces::msg:: @@ -246,68 +246,68 @@ void DracoPublisher::declareParameters(const std::string & base_topic) auto result = rcl_interfaces::msg::SetParametersResult(); result.successful = true; for (auto parameter : parameters) { - if (parameter.get_name() == "expert_quantization") { + if (parameter.get_name().find("expert_quantization") != std::string::npos) { config_.expert_quantization = parameter.as_bool(); return result; - } else if (parameter.get_name() == "force_quantization") { + } else if (parameter.get_name().find("force_quantization") != std::string::npos) { config_.force_quantization = parameter.as_bool(); return result; - } else if (parameter.get_name() == "encode_speed") { + } else if (parameter.get_name().find("encode_speed") != std::string::npos) { config_.encode_speed = static_cast(parameter.as_int()); if (!(config_.encode_speed >= 0 && config_.encode_speed <= 10)) { RCLCPP_ERROR_STREAM( getLogger(), "encode_speed value range should be between [0, 10] "); } return result; - } else if (parameter.get_name() == "decode_speed") { + } else if (parameter.get_name().find("decode_speed") != std::string::npos) { config_.decode_speed = static_cast(parameter.as_int()); if (!(config_.decode_speed >= 0 && config_.decode_speed <= 10)) { RCLCPP_ERROR_STREAM( getLogger(), "decode_speed value range should be between [0, 10] "); } return result; - } else if (parameter.get_name() == "encode_method") { + } else if (parameter.get_name().find("encode_method") != std::string::npos) { config_.encode_method = static_cast(parameter.as_int()); return result; - } else if (parameter.get_name() == "deduplicate") { + } else if (parameter.get_name().find("deduplicate") != std::string::npos) { config_.deduplicate = parameter.as_bool(); return result; - } else if (parameter.get_name() == "quantization_POSITION") { + } else if (parameter.get_name().find("quantization_POSITION") != std::string::npos) { config_.quantization_POSITION = static_cast(parameter.as_int()); if (!(config_.quantization_POSITION >= 1 && config_.quantization_POSITION <= 31)) { RCLCPP_ERROR_STREAM( getLogger(), "quantization_POSITION value range should be between [1, 31] "); } return result; - } else if (parameter.get_name() == "quantization_NORMAL") { + } else if (parameter.get_name().find("quantization_NORMAL") != std::string::npos) { config_.quantization_NORMAL = static_cast(parameter.as_int()); if (!(config_.quantization_NORMAL >= 1 && config_.quantization_NORMAL <= 31)) { RCLCPP_ERROR_STREAM( getLogger(), "quantization_NORMAL value range should be between [1, 31] "); } return result; - } else if (parameter.get_name() == "quantization_COLOR") { + } else if (parameter.get_name().find("quantization_COLOR") != std::string::npos) { config_.quantization_COLOR = static_cast(parameter.as_int()); if (!(config_.quantization_COLOR >= 1 && config_.quantization_COLOR <= 31)) { RCLCPP_ERROR_STREAM( getLogger(), "quantization_COLOR value range should be between [1, 31] "); } return result; - } else if (parameter.get_name() == "quantization_TEX_COORD") { + } else if (parameter.get_name().find("quantization_TEX_COORD") != std::string::npos) { config_.quantization_TEX_COORD = static_cast(parameter.as_int()); if (!(config_.quantization_TEX_COORD >= 1 && config_.quantization_TEX_COORD <= 31)) { RCLCPP_ERROR_STREAM( getLogger(), "quantization_TEX_COORD value range should be between [1, 31] "); } return result; - } else if (parameter.get_name() == "quantization_GENERIC") { + } else if (parameter.get_name().find("quantization_GENERIC") != std::string::npos) { config_.quantization_GENERIC = static_cast(parameter.as_int()); if (!(config_.quantization_GENERIC >= 1 && config_.quantization_GENERIC <= 31)) { RCLCPP_ERROR_STREAM( getLogger(), "quantization_GENERIC value range should be between [1, 31] "); } return result; - } else if (parameter.get_name() == "expert_attribute_types") { + } else if (parameter.get_name().find("expert_attribute_types") != std::string::npos) { config_.expert_attribute_types = parameter.as_bool(); return result; } diff --git a/zlib_point_cloud_transport/src/zlib_publisher.cpp b/zlib_point_cloud_transport/src/zlib_publisher.cpp index de702e5..00f3f2d 100644 --- a/zlib_point_cloud_transport/src/zlib_publisher.cpp +++ b/zlib_point_cloud_transport/src/zlib_publisher.cpp @@ -64,7 +64,7 @@ void ZlibPublisher::declareParameters(const std::string & base_topic) auto result = rcl_interfaces::msg::SetParametersResult(); result.successful = true; for (auto parameter : parameters) { - if (parameter.get_name() == "encode_level") { + if (parameter.get_name().find("encode_level") != std::string::npos) { this->encode_level_ = static_cast(parameter.as_int()); if (!(this->encode_level_ >= -1 && this->encode_level_ <= 9)) { RCLCPP_ERROR_STREAM( diff --git a/zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_publisher.hpp b/zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_publisher.hpp index 363534a..9ac11bf 100644 --- a/zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_publisher.hpp +++ b/zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_publisher.hpp @@ -32,6 +32,8 @@ #ifndef ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_PUBLISHER_HPP_ #define ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_PUBLISHER_HPP_ +#include + #include #include @@ -51,6 +53,8 @@ class ZstdPublisher point_cloud_interfaces::msg::CompressedPointCloud2> { public: + ZstdPublisher(); + std::string getTransportName() const override; void declareParameters(const std::string & base_topic) override; @@ -61,6 +65,12 @@ class ZstdPublisher private: int encode_level_{7}; + + // When compressing many times, + // it is recommended to allocate a context just once, + // and re-use it for each successive compression operation. + // This will make workload friendlier for system's memory. + ZSTD_CCtx * zstd_context_{nullptr}; }; } // namespace zstd_point_cloud_transport diff --git a/zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_subscriber.hpp b/zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_subscriber.hpp index 4a356dd..b5828d3 100644 --- a/zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_subscriber.hpp +++ b/zstd_point_cloud_transport/include/zstd_point_cloud_transport/zstd_subscriber.hpp @@ -32,6 +32,8 @@ #ifndef ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_SUBSCRIBER_HPP_ #define ZSTD_POINT_CLOUD_TRANSPORT__ZSTD_SUBSCRIBER_HPP_ +#include + #include #include @@ -47,6 +49,8 @@ class ZstdSubscriber point_cloud_interfaces::msg::CompressedPointCloud2> { public: + ZstdSubscriber(); + std::string getTransportName() const override; void declareParameters() override; @@ -55,6 +59,9 @@ class ZstdSubscriber DecodeResult decodeTyped(const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed) const override; + +private: + ZSTD_DCtx * zstd_context_{nullptr}; }; } // namespace zstd_point_cloud_transport diff --git a/zstd_point_cloud_transport/src/zstd_publisher.cpp b/zstd_point_cloud_transport/src/zstd_publisher.cpp index 063de8c..9806fb5 100644 --- a/zstd_point_cloud_transport/src/zstd_publisher.cpp +++ b/zstd_point_cloud_transport/src/zstd_publisher.cpp @@ -41,6 +41,11 @@ namespace zstd_point_cloud_transport { +ZstdPublisher::ZstdPublisher() +{ + this->zstd_context_ = ZSTD_createCCtx(); +} + void ZstdPublisher::declareParameters(const std::string & base_topic) { rcl_interfaces::msg::ParameterDescriptor encode_level_paramDescriptor; @@ -64,7 +69,7 @@ void ZstdPublisher::declareParameters(const std::string & base_topic) auto result = rcl_interfaces::msg::SetParametersResult(); result.successful = true; for (auto parameter : parameters) { - if (parameter.get_name() == "zstd_encode_level") { + if (parameter.get_name().find("zstd_encode_level") != std::string::npos) { this->encode_level_ = static_cast(parameter.as_int()); if (!(this->encode_level_ >= -1 && this->encode_level_ <= 9)) { RCLCPP_ERROR_STREAM( @@ -97,7 +102,8 @@ ZstdPublisher::TypedEncodeResult ZstdPublisher::encodeTyped( compressed.compressed_data.resize(est_compress_size); auto compress_size = - ZSTD_compress( + ZSTD_compressCCtx( + this->zstd_context_, static_cast(&compressed.compressed_data[0]), est_compress_size, &raw.data[0], diff --git a/zstd_point_cloud_transport/src/zstd_subscriber.cpp b/zstd_point_cloud_transport/src/zstd_subscriber.cpp index f15d7bf..4523fbd 100644 --- a/zstd_point_cloud_transport/src/zstd_subscriber.cpp +++ b/zstd_point_cloud_transport/src/zstd_subscriber.cpp @@ -42,6 +42,11 @@ namespace zstd_point_cloud_transport { +ZstdSubscriber::ZstdSubscriber() +{ + this->zstd_context_ = ZSTD_createDCtx(); +} + void ZstdSubscriber::declareParameters() { } @@ -66,7 +71,8 @@ ZstdSubscriber::DecodeResult ZstdSubscriber::decodeTyped( result->data.resize(est_decomp_size); - size_t const decomp_size = ZSTD_decompress( + size_t const decomp_size = ZSTD_decompressDCtx( + this->zstd_context_, static_cast(&result->data[0]), est_decomp_size, &msg.compressed_data[0],