From 58b7f6fd9b34bea6442f6c1207af715c73325fef Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Mon, 17 Apr 2023 12:59:25 +0200 Subject: [PATCH] add transport scoped parameters for CompressedPublisher - .. as future replacement of . - e.g. `image.compressed.format` instead of `image.format` - support both ways for now - emit deprecation warnings on setting through non-transport scoped parameter - synchronize deprecated changes to new ones - this is necessary for dynamic reconfigure Similar changes will be needed for: - other transports - subscribers Related to ros-perception#140 --- .../compressed_publisher.h | 45 ++-- .../compression_common.h | 86 +++++++ .../src/compressed_publisher.cpp | 232 ++++++++---------- 3 files changed, 212 insertions(+), 151 deletions(-) diff --git a/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h b/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h index 7df0c7d..7b6208d 100644 --- a/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h +++ b/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h @@ -40,9 +40,12 @@ #include +#include "compressed_image_transport/compression_common.h" + namespace compressed_image_transport { using CompressedImage = sensor_msgs::msg::CompressedImage; +using ParameterEvent = rcl_interfaces::msg::ParameterEvent; class CompressedPublisher : public image_transport::SimplePublisherPlugin { @@ -66,35 +69,23 @@ class CompressedPublisher : public image_transport::SimplePublisherPlugin parameters_; + std::vector deprecatedParameters_; + + rclcpp::Subscription::SharedPtr parameter_subscription_; + + void declareParameters(rclcpp::Node* node, const std::string& base_topic); + + void declareParameter(rclcpp::Node* node, + const std::string &base_name, + const std::string &transport_name, + const ParameterDefinition &definition); + + void onParameterEvent(ParameterEvent::SharedPtr event, std::string full_name, std::string base_name); }; } //namespace compressed_image_transport diff --git a/compressed_image_transport/include/compressed_image_transport/compression_common.h b/compressed_image_transport/include/compressed_image_transport/compression_common.h index b9d9cf3..cc3c4d2 100644 --- a/compressed_image_transport/include/compressed_image_transport/compression_common.h +++ b/compressed_image_transport/include/compressed_image_transport/compression_common.h @@ -47,6 +47,92 @@ enum compressionFormat TIFF = 2, }; +// Parameters +// Note - what is below would be moved to separate file, e.g. `compressed_publisher_cfg.h` + +enum compressedParameters +{ + FORMAT = 0, + PNG_LEVEL, + JPEG_QUALITY, + TIFF_RESOLUTION_UNIT, + TIFF_XDPI, + TIFF_YDPI +}; + +using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor; +using ParameterValue = rclcpp::ParameterValue; + +struct ParameterDefinition +{ + const ParameterValue defaultValue; + const ParameterDescriptor descriptor; +}; + +const struct ParameterDefinition kParameters[] = +{ + { //FORMAT - Compression format to use "jpeg", "png" or "tiff". + ParameterValue("jpeg"), + ParameterDescriptor() + .set__name("format") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_STRING) + .set__description("Compression method") + .set__read_only(false) + .set__additional_constraints("Supported values: [jpeg, png]") + }, + { //PNG_LEVEL - PNG Compression Level from 0 to 9. A higher value means a smaller size. + ParameterValue((int)3), //Default to OpenCV default of 3 + ParameterDescriptor() + .set__name("png_level") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) + .set__description("Compression level for PNG format") + .set__read_only(false) + .set__integer_range( + {rcl_interfaces::msg::IntegerRange() + .set__from_value(0) + .set__to_value(9) + .set__step(1)}) + }, + { //JPEG_QUALITY - JPEG Quality from 0 to 100 (higher is better quality). + ParameterValue((int)95), //Default to OpenCV default of 95. + ParameterDescriptor() + .set__name("jpeg_quality") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) + .set__description("Image quality for JPEG format") + .set__read_only(false) + .set__integer_range( + {rcl_interfaces::msg::IntegerRange() + .set__from_value(1) + .set__to_value(100) + .set__step(1)}) + }, + { //TIFF_RESOLUTION_UNIT - TIFF resolution unit, can be one of "none", "inch", "centimeter". + ParameterValue("inch"), + ParameterDescriptor() + .set__name("tiff.res_unit") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_STRING) + .set__description("tiff resolution unit") + .set__read_only(false) + .set__additional_constraints("Supported values: [none, inch, centimeter]") + }, + { //TIFF_XDPI + ParameterValue((int)-1), + ParameterDescriptor() + .set__name("tiff.xdpi") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) + .set__description("tiff xdpi") + .set__read_only(false) + }, + { //TIFF_YDPI + ParameterValue((int)-1), + ParameterDescriptor() + .set__name("tiff.ydpi") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) + .set__description("tiff ydpi") + .set__read_only(false) + } +}; + } //namespace compressed_image_transport #endif diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index 13a7cca..7d7709d 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -42,14 +42,11 @@ #include #include +#include #include #include -constexpr const char* kDefaultFormat = "jpeg"; -constexpr int kDefaultPngLevel = 3; -constexpr int kDefaultJpegQuality = 95; - using namespace cv; using namespace std; @@ -68,120 +65,21 @@ void CompressedPublisher::advertiseImpl( typedef image_transport::SimplePublisherPlugin Base; Base::advertiseImpl(node, base_topic, custom_qos, options); - uint ns_len = node->get_effective_namespace().length(); - std::string param_base_name = base_topic.substr(ns_len); - std::replace(param_base_name.begin(), param_base_name.end(), '/', '.'); - - format_param_name_ = param_base_name + ".format"; - rcl_interfaces::msg::ParameterDescriptor format_description; - format_description.name = "format"; - format_description.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - format_description.description = "Compression method"; - format_description.read_only = false; - format_description.additional_constraints = "Supported values: [jpeg, png, tiff]"; - try { - config_.format = node->declare_parameter(format_param_name_, kDefaultFormat, format_description); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(logger_, "%s was previously declared", format_param_name_.c_str()); - config_.format = node->get_parameter(format_param_name_).get_value(); - } - - png_level_param_name_ = param_base_name + ".png_level"; - rcl_interfaces::msg::ParameterDescriptor png_level_description; - png_level_description.name = "png_level"; - png_level_description.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - png_level_description.description = "Compression level for PNG format"; - png_level_description.read_only = false; - rcl_interfaces::msg::IntegerRange png_range; - png_range.from_value = 0; - png_range.to_value = 9; - png_range.step = 1; - png_level_description.integer_range.push_back(png_range); - try { - config_.png_level = node->declare_parameter( - png_level_param_name_, kDefaultPngLevel, png_level_description); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(logger_, "%s was previously declared", png_level_param_name_.c_str()); - config_.png_level = node->get_parameter(png_level_param_name_).get_value(); - } - - jpeg_quality_param_name_ = param_base_name + ".jpeg_quality"; - rcl_interfaces::msg::ParameterDescriptor jpeg_quality_description; - jpeg_quality_description.name = "jpeg_quality"; - jpeg_quality_description.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - jpeg_quality_description.description = "Image quality for JPEG format"; - jpeg_quality_description.read_only = false; - rcl_interfaces::msg::IntegerRange jpeg_range; - jpeg_range.from_value = 1; - jpeg_range.to_value = 100; - jpeg_range.step = 1; - jpeg_quality_description.integer_range.push_back(jpeg_range); - try { - config_.jpeg_quality = node->declare_parameter( - jpeg_quality_param_name_, kDefaultJpegQuality, jpeg_quality_description); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(logger_, "%s was previously declared", jpeg_quality_param_name_.c_str()); - config_.jpeg_quality = node->get_parameter(jpeg_quality_param_name_).get_value(); - } - - tiff_res_unit_param_name_ = param_base_name + ".tiff.res_unit"; - rcl_interfaces::msg::ParameterDescriptor tiff_res_unit_description; - tiff_res_unit_description.description = "tiff resolution unit"; - tiff_res_unit_description.read_only = false; - tiff_res_unit_description.additional_constraints = "Supported values: [none, inch, centimeter]"; - try { - config_.tiff_res_unit = node->declare_parameter( - tiff_res_unit_param_name_, "inch", tiff_res_unit_description); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(logger_, "%s was previously declared", tiff_res_unit_param_name_.c_str()); - config_.tiff_res_unit = node->get_parameter(tiff_res_unit_param_name_).get_value(); - } - - tiff_xdpi_param_name_ = param_base_name + ".tiff.xdpi"; - rcl_interfaces::msg::ParameterDescriptor tiff_xdpi_description; - tiff_xdpi_description.description = "tiff xdpi"; - tiff_xdpi_description.read_only = false; - try { - config_.tiff_xdpi = node->declare_parameter( - tiff_xdpi_param_name_, -1, tiff_xdpi_description); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(logger_, "%s was previously declared", tiff_xdpi_param_name_.c_str()); - config_.tiff_xdpi = node->get_parameter(tiff_xdpi_param_name_).get_value(); - } - - tiff_ydpi_param_name_ = param_base_name + ".tiff.ydpi"; - rcl_interfaces::msg::ParameterDescriptor tiff_ydpi_description; - tiff_ydpi_description.description = "tiff ydpi"; - tiff_ydpi_description.read_only = false; - try { - config_.tiff_ydpi = node->declare_parameter( - tiff_ydpi_param_name_, -1, tiff_ydpi_description); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(logger_, "%s was previously declared", tiff_ydpi_param_name_.c_str()); - config_.tiff_ydpi = node->get_parameter(tiff_ydpi_param_name_).get_value(); - } -} - -CompressedPublisher::Config CompressedPublisher::get_updated_config_from_parameters() const -{ - CompressedPublisher::Config config{config_}; - if (!node_) { - return config; - } - config.format = node_->get_parameter(format_param_name_).get_value(); - config.png_level = node_->get_parameter(png_level_param_name_).get_value(); - config.jpeg_quality = node_->get_parameter(jpeg_quality_param_name_).get_value(); - config.tiff_res_unit = node_->get_parameter(tiff_res_unit_param_name_).get_value(); - config.tiff_xdpi = node_->get_parameter(tiff_xdpi_param_name_).get_value(); - config.tiff_ydpi = node_->get_parameter(tiff_ydpi_param_name_).get_value(); - return config; + declareParameters(node, base_topic); } void CompressedPublisher::publish( const sensor_msgs::msg::Image& message, const PublishFn& publish_fn) const { - auto config = this->get_updated_config_from_parameters(); + // Fresh Configuration + std::string cfg_format = node_->get_parameter(parameters_[FORMAT]).get_value(); + int cfg_png_level = node_->get_parameter(parameters_[PNG_LEVEL]).get_value(); + int cfg_jpeg_quality = node_->get_parameter(parameters_[JPEG_QUALITY]).get_value();; + std::string cfg_tiff_res_unit = node_->get_parameter(parameters_[TIFF_RESOLUTION_UNIT]).get_value(); + int cfg_tiff_xdpi = node_->get_parameter(parameters_[TIFF_XDPI]).get_value(); + int cfg_tiff_ydpi = node_->get_parameter(parameters_[TIFF_YDPI]).get_value(); + // Compressed image message sensor_msgs::msg::CompressedImage compressed; compressed.header = message.header; @@ -192,11 +90,11 @@ void CompressedPublisher::publish( // Get codec configuration compressionFormat encodingFormat = UNDEFINED; - if (config.format == "jpeg") { + if (cfg_format == "jpeg") { encodingFormat = JPEG; - } else if (config.format == "png") { + } else if (cfg_format == "png") { encodingFormat = PNG; - } else if (config.format == "tiff") { + } else if (cfg_format == "tiff") { encodingFormat = TIFF; } @@ -210,7 +108,7 @@ void CompressedPublisher::publish( { params.reserve(2); params.emplace_back(cv::IMWRITE_JPEG_QUALITY); - params.emplace_back(config.jpeg_quality); + params.emplace_back(cfg_jpeg_quality); // Update ros message format header compressed.format += "; jpeg compressed "; @@ -271,7 +169,7 @@ void CompressedPublisher::publish( { params.reserve(2); params.emplace_back(cv::IMWRITE_PNG_COMPRESSION); - params.emplace_back(config.png_level); + params.emplace_back(cfg_png_level); // Update ros message format header compressed.format += "; png compressed "; @@ -334,27 +232,28 @@ void CompressedPublisher::publish( compressed.format += "; tiff compressed "; int res_unit = -1; // See https://gitlab.com/libtiff/libtiff/-/blob/v4.3.0/libtiff/tiff.h#L282-284 - if (config.tiff_res_unit == "inch") { + if (cfg_tiff_res_unit == "inch") { res_unit = 2; - } else if (config.tiff_res_unit == "centimeter") { + } else if (cfg_tiff_res_unit == "centimeter") { res_unit = 3; - } else if (config.tiff_res_unit == "none") { + } else if (cfg_tiff_res_unit == "none") { res_unit = 1; } else { RCLCPP_WARN( logger_, "tiff.res_unit parameter should be either 'inch', 'centimeter' or 'none'; " - "defaulting to 'inch'. Found '%s'", config.tiff_res_unit.c_str()); + "defaulting to 'inch'. Found '%s'", cfg_tiff_res_unit.c_str()); } params.reserve(3); params.emplace_back(cv::IMWRITE_TIFF_XDPI); - params.emplace_back(config.tiff_xdpi); + params.emplace_back(cfg_tiff_xdpi); params.emplace_back(cv::IMWRITE_TIFF_YDPI); - params.emplace_back(config.tiff_ydpi); + params.emplace_back(cfg_tiff_ydpi); params.emplace_back(cv::IMWRITE_TIFF_RESUNIT); params.emplace_back(res_unit); // Check input format + if ((bitDepth == 8) || (bitDepth == 16) || (bitDepth == 32)) { @@ -396,8 +295,93 @@ void CompressedPublisher::publish( } default: - RCUTILS_LOG_ERROR("Unknown compression type '%s', valid options are 'jpeg', 'png' and 'tiff'", config.format.c_str()); + RCUTILS_LOG_ERROR("Unknown compression type '%s', valid options are 'jpeg', 'png' and 'tiff'", cfg_format.c_str()); break; } } + +void CompressedPublisher::declareParameters(rclcpp::Node* node, const std::string& base_topic) +{ + uint ns_len = node->get_effective_namespace().length(); + std::string param_base_name = base_topic.substr(ns_len); + std::replace(param_base_name.begin(), param_base_name.end(), '/', '.'); + const std::string transport_name = getTransportName(); + + using callbackT = std::function; + auto callback = std::bind(&CompressedPublisher::onParameterEvent, this, std::placeholders::_1, + node->get_fully_qualified_name(), param_base_name); + + parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event(node, callback); + + for(const ParameterDefinition &pd : kParameters) + declareParameter(node, param_base_name, transport_name, pd); +} + +void CompressedPublisher::declareParameter(rclcpp::Node* node, + const std::string &base_name, + const std::string &transport_name, + const ParameterDefinition &definition) +{ + //transport scoped parameter (e.g. image_raw.compressed.format) + const std::string param_name = base_name + "." + transport_name + "." + definition.descriptor.name; + parameters_.push_back(param_name); + + //deprecated non-scoped parameter name (e.g. image_raw.format) + const std::string deprecated_name = base_name + "." + definition.descriptor.name; + deprecatedParameters_.push_back(deprecated_name); + + rclcpp::ParameterValue param_value; + + try { + param_value = node->declare_parameter(param_name, definition.defaultValue, definition.descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + RCLCPP_DEBUG(logger_, "%s was previously declared", definition.descriptor.name.c_str()); + param_value = node->get_parameter(param_name).get_parameter_value(); + } + + // transport scoped parameter as default, otherwise we would overwrite + try { + node->declare_parameter(deprecated_name, param_value, definition.descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + RCLCPP_DEBUG(logger_, "%s was previously declared", definition.descriptor.name.c_str()); + node->get_parameter(deprecated_name).get_parameter_value(); + } +} + +void CompressedPublisher::onParameterEvent(ParameterEvent::SharedPtr event, std::string full_name, std::string base_name) +{ + // filter out events from other nodes + if (event->node != full_name) + return; + + // filter out new/changed deprecated parameters + using EventType = rclcpp::ParameterEventsFilter::EventType; + + rclcpp::ParameterEventsFilter filter(event, deprecatedParameters_, {EventType::NEW, EventType::CHANGED}); + + const std::string transport = getTransportName(); + + // emit warnings for deprecated parameters & sync deprecated parameter value to correct + for (auto & it : filter.get_events()) + { + const std::string name = it.second->name; + + size_t baseNameIndex = name.find(base_name); //name was generated from base_name, has to succeed + size_t paramNameIndex = baseNameIndex + base_name.size(); + //e.g. `color.image_raw.` + `compressed` + `format` + std::string recommendedName = name.substr(0, paramNameIndex + 1) + transport + name.substr(paramNameIndex); + + rclcpp::Parameter recommendedValue = node_->get_parameter(recommendedName); + + // do not emit warnings if deprecated value matches + if(it.second->value == recommendedValue.get_value_message()) + continue; + + RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated" << + "; use transport qualified name `" << recommendedName << "`"); + + node_->set_parameter(rclcpp::Parameter(recommendedName, it.second->value)); + } +} + } //namespace compressed_image_transport