From 9cc9441f2d2cef84bd6c43003af70f8bef2eec97 Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Tue, 18 Apr 2023 12:48:46 +0200 Subject: [PATCH] reconfigurable transport scoped parameters for CompressedSubscriber - runtime reconfiguration - .. as future replacement of . - e.g. `image.compressed.mode` instead of `image.mode` - 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 Related to #140 --- .../compressed_publisher.h | 7 +- .../compressed_subscriber.h | 24 ++- .../compression_common.h | 80 +--------- .../src/compressed_publisher.cpp | 119 +++++++++++---- .../src/compressed_subscriber.cpp | 142 ++++++++++++++---- 5 files changed, 232 insertions(+), 140 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 7b6208d..1739beb 100644 --- a/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h +++ b/compressed_image_transport/include/compressed_image_transport/compressed_publisher.h @@ -33,6 +33,7 @@ *********************************************************************/ #include +#include #include #include @@ -78,11 +79,7 @@ class CompressedPublisher : public image_transport::SimplePublisherPlugin::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, + void declareParameter(const std::string &base_name, const ParameterDefinition &definition); void onParameterEvent(ParameterEvent::SharedPtr event, std::string full_name, std::string base_name); diff --git a/compressed_image_transport/include/compressed_image_transport/compressed_subscriber.h b/compressed_image_transport/include/compressed_image_transport/compressed_subscriber.h index f551e61..0c7c936 100644 --- a/compressed_image_transport/include/compressed_image_transport/compressed_subscriber.h +++ b/compressed_image_transport/include/compressed_image_transport/compressed_subscriber.h @@ -33,6 +33,7 @@ *********************************************************************/ #include +#include #include #include @@ -41,8 +42,12 @@ #include #include +#include "compressed_image_transport/compression_common.h" + namespace compressed_image_transport { +using ParameterEvent = rcl_interfaces::msg::ParameterEvent; + class CompressedSubscriber final : public image_transport::SimpleSubscriberPlugin { public: @@ -66,12 +71,21 @@ class CompressedSubscriber final : public image_transport::SimpleSubscriberPlugi void internalCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr& message, const Callback& user_cb) override; - struct Config { - int imdecode_flag; - }; - - Config config_; rclcpp::Logger logger_; + rclcpp::Node * node_; + +private: + std::vector parameters_; + std::vector deprecatedParameters_; + + rclcpp::Subscription::SharedPtr parameter_subscription_; + + int imdecodeFlagFromConfig(); + + void declareParameter(const std::string &base_name, + const ParameterDefinition &definition); + + void onParameterEvent(ParameterEvent::SharedPtr event, std::string full_name, std::string base_name); }; } //namespace 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 4a31645..9e724e0 100644 --- a/compressed_image_transport/include/compressed_image_transport/compression_common.h +++ b/compressed_image_transport/include/compressed_image_transport/compression_common.h @@ -35,6 +35,9 @@ #ifndef COMPRESSED_IMAGE_TRANSPORT_COMPRESSION_COMMON #define COMPRESSED_IMAGE_TRANSPORT_COMPRESSION_COMMON +#include +#include + namespace compressed_image_transport { @@ -47,19 +50,6 @@ 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; @@ -69,70 +59,6 @@ struct ParameterDefinition 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, tiff]") - }, - { //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 7d7709d..3420a57 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -55,6 +55,80 @@ namespace enc = sensor_msgs::image_encodings; namespace compressed_image_transport { +enum compressedParameters +{ + FORMAT = 0, + PNG_LEVEL, + JPEG_QUALITY, + TIFF_RESOLUTION_UNIT, + TIFF_XDPI, + TIFF_YDPI +}; + +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, tiff]") + }, + { //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) + } +}; + void CompressedPublisher::advertiseImpl( rclcpp::Node* node, const std::string& base_topic, @@ -65,7 +139,19 @@ void CompressedPublisher::advertiseImpl( typedef image_transport::SimplePublisherPlugin Base; Base::advertiseImpl(node, base_topic, custom_qos, options); - declareParameters(node, base_topic); + // Declare Parameters + 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(), '/', '.'); + + 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(param_base_name, pd); } void CompressedPublisher::publish( @@ -253,7 +339,6 @@ void CompressedPublisher::publish( params.emplace_back(res_unit); // Check input format - if ((bitDepth == 8) || (bitDepth == 16) || (bitDepth == 32)) { @@ -300,29 +385,11 @@ void CompressedPublisher::publish( } } -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, +void CompressedPublisher::declareParameter(const std::string &base_name, const ParameterDefinition &definition) { //transport scoped parameter (e.g. image_raw.compressed.format) + const std::string transport_name = getTransportName(); const std::string param_name = base_name + "." + transport_name + "." + definition.descriptor.name; parameters_.push_back(param_name); @@ -333,18 +400,18 @@ void CompressedPublisher::declareParameter(rclcpp::Node* node, rclcpp::ParameterValue param_value; try { - param_value = node->declare_parameter(param_name, definition.defaultValue, definition.descriptor); + 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(); + 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); + 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(); + node_->get_parameter(deprecated_name).get_parameter_value(); } } diff --git a/compressed_image_transport/src/compressed_subscriber.cpp b/compressed_image_transport/src/compressed_subscriber.cpp index c0f31bd..7da5117 100644 --- a/compressed_image_transport/src/compressed_subscriber.cpp +++ b/compressed_image_transport/src/compressed_subscriber.cpp @@ -42,12 +42,11 @@ #include "compressed_image_transport/compression_common.h" #include +#include #include #include -constexpr const char* kDefaultMode = "unchanged"; - using namespace cv; namespace enc = sensor_msgs::image_encodings; @@ -57,6 +56,24 @@ using CompressedImage = sensor_msgs::msg::CompressedImage; namespace compressed_image_transport { +enum compressedParameters +{ + MODE = 0, +}; + +const struct ParameterDefinition kParameters[] = +{ + { //MODE - OpenCV imdecode flags to use: [unchanged, gray, color] + ParameterValue("unchanged"), + ParameterDescriptor() + .set__name("mode") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_STRING) + .set__description("OpenCV imdecode flags to use") + .set__read_only(false) + .set__additional_constraints("Supported values: [unchanged, gray, color]") + } +}; + void CompressedSubscriber::subscribeImpl( rclcpp::Node * node, const std::string& base_topic, @@ -64,43 +81,32 @@ void CompressedSubscriber::subscribeImpl( rmw_qos_profile_t custom_qos, rclcpp::SubscriptionOptions options) { + node_ = node; logger_ = node->get_logger(); typedef image_transport::SimpleSubscriberPlugin Base; Base::subscribeImpl(node, base_topic, callback, custom_qos, options); + + // Declare Parameters 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(), '/', '.'); - std::string mode_param_name = param_base_name + ".mode"; - - std::string mode; - rcl_interfaces::msg::ParameterDescriptor mode_description; - mode_description.description = "OpenCV imdecode flags to use"; - mode_description.read_only = false; - mode_description.additional_constraints = "Supported values: [unchanged, gray, color]"; - try { - mode = node->declare_parameter(mode_param_name, kDefaultMode, mode_description); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(logger_, "%s was previously declared", mode_param_name.c_str()); - mode = node->get_parameter(mode_param_name).get_value(); - } - if (mode == "unchanged") { - config_.imdecode_flag = cv::IMREAD_UNCHANGED; - } else if (mode == "gray") { - config_.imdecode_flag = cv::IMREAD_GRAYSCALE; - } else if (mode == "color") { - config_.imdecode_flag = cv::IMREAD_COLOR; - } else { - RCLCPP_ERROR(logger_, "Unknown mode: %s, defaulting to 'unchanged", mode.c_str()); - config_.imdecode_flag = cv::IMREAD_UNCHANGED; - } -} + using paramCallbackT = std::function; + auto paramCallback = std::bind(&CompressedSubscriber::onParameterEvent, this, std::placeholders::_1, + node->get_fully_qualified_name(), param_base_name); + parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event(node, paramCallback); + + for(const ParameterDefinition &pd : kParameters) + declareParameter(param_base_name, pd); +} void CompressedSubscriber::internalCallback(const CompressedImage::ConstSharedPtr& message, const Callback& user_cb) { + int cfg_imdecode_flag = imdecodeFlagFromConfig(); + cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage); // Copy message header @@ -109,7 +115,7 @@ void CompressedSubscriber::internalCallback(const CompressedImage::ConstSharedPt // Decode color/mono image try { - cv_ptr->image = cv::imdecode(cv::Mat(message->data), config_.imdecode_flag); + cv_ptr->image = cv::imdecode(cv::Mat(message->data), cfg_imdecode_flag); // Assign image encoding string const size_t split_pos = message->format.find(';'); @@ -182,4 +188,86 @@ void CompressedSubscriber::internalCallback(const CompressedImage::ConstSharedPt user_cb(cv_ptr->toImageMsg()); } +int CompressedSubscriber::imdecodeFlagFromConfig() +{ + std::string mode = node_->get_parameter(parameters_[MODE]).get_value(); + + if (mode == "unchanged") + return cv::IMREAD_UNCHANGED; + else if (mode == "gray") + return cv::IMREAD_GRAYSCALE; + else if (mode == "color") + return cv::IMREAD_COLOR; + + RCLCPP_ERROR(logger_, "Unknown mode: %s, defaulting to 'unchanged", mode.c_str()); + + return cv::IMREAD_UNCHANGED; +} + +void CompressedSubscriber::declareParameter(const std::string &base_name, + const ParameterDefinition &definition) +{ + //transport scoped parameter (e.g. image_raw.compressed.format) + const std::string transport_name = getTransportName(); + 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 CompressedSubscriber::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