From 020be42b6cb24c862669cd3eaecc1174aedb7607 Mon Sep 17 00:00:00 2001 From: Marcel Zeilinger Date: Wed, 19 Oct 2022 12:34:23 +0200 Subject: [PATCH 1/9] Declare depth compression params in a per-topic namespace --- .../src/compressed_depth_publisher.cpp | 52 +++++++++++++++++-- 1 file changed, 49 insertions(+), 3 deletions(-) diff --git a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp index e9bc29a..285652e 100644 --- a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp +++ b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp @@ -66,9 +66,55 @@ void CompressedDepthPublisher::advertiseImpl( typedef image_transport::SimplePublisherPlugin Base; Base::advertiseImpl(node, base_topic, custom_qos, options); - node->get_parameter_or("png_level", config_.png_level, kDefaultPngLevel); - node->get_parameter_or("depth_max", config_.depth_max, kDefaultDepthMax); - node->get_parameter_or("depth_quantization", config_.depth_max, KDefaultDepthQuantization); + 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 param_name = param_base_name + ".png_level"; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.name = "png_level"; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.description = "PNG compression level"; + descriptor.read_only = false; + try { + config_.png_level = node->declare_parameter(param_name, kDefaultPngLevel, descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + RCLCPP_DEBUG(node->get_logger(), "%s was previously declared", param_name.c_str()); + config_.png_level = node->get_parameter(param_name).get_value(); + } + } + + { + std::string param_name = param_base_name + ".depth_max"; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.name = "depth_max"; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.description = "Maximum depth value"; + descriptor.read_only = false; + try { + config_.depth_max = node->declare_parameter(param_name, kDefaultDepthMax, descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + RCLCPP_DEBUG(node->get_logger(), "%s was previously declared", param_name.c_str()); + config_.depth_max = node->get_parameter(param_name).get_value(); + } + } + + { + std::string param_name = param_base_name + ".depth_quantization"; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.name = "depth_quantization"; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.description = "Depth quantization"; + descriptor.read_only = false; + try { + config_.depth_quantization = node->declare_parameter( + param_name, KDefaultDepthQuantization, descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + RCLCPP_DEBUG(node->get_logger(), "%s was previously declared", param_name.c_str()); + config_.depth_quantization = node->get_parameter(param_name).get_value(); + } + } } void CompressedDepthPublisher::publish( From dfdf48bc85f757434f8eefe6702e84ee21db5a81 Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Thu, 27 Apr 2023 15:52:00 +0200 Subject: [PATCH 2/9] reconfigurable transport scoped parameters for CompressedDepthPublisher - runtime reconfiguration - .. as future replacement of . - e.g. `image.compressedDepth.png_level` instead of `image.png_level` - 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 - add ROS1 like ranges for parameters (min/max) - some cleanup - remove unnecessary includes, using statements, etc. Default png_level was left the same as for compressed_image_transport - this is OpenCV default (3) - the deprecated paramterer name clashes here with compressed_image_transport - I don't really want to consider if there is a race between plugin loading for default value - side notes: - in ROS2 default was 9 - 9 is not usable for real-time processing on most machines - in ROS1 default is now 1 - 3 is typically usable for real-time processing - this should probably be made even with ROS1 after removing deprecated parameters Related to #140 Builds up on #110 --- .../compressed_depth_publisher.h | 32 ++- .../compression_common.h | 12 ++ .../src/compressed_depth_publisher.cpp | 197 ++++++++++++------ 3 files changed, 171 insertions(+), 70 deletions(-) diff --git a/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_publisher.h b/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_publisher.h index 09c0498..e695331 100644 --- a/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_publisher.h +++ b/compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_publisher.h @@ -32,15 +32,26 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ +#include "compressed_depth_image_transport/compression_common.h" + #include #include #include +#include + +#include +#include + namespace compressed_depth_image_transport { -class CompressedDepthPublisher : public image_transport::SimplePublisherPlugin +using CompressedImage = sensor_msgs::msg::CompressedImage; +using ParameterEvent = rcl_interfaces::msg::ParameterEvent; + +class CompressedDepthPublisher : public image_transport::SimplePublisherPlugin { public: + CompressedDepthPublisher(): logger_(rclcpp::get_logger("CompressedDepthPublisher")) {} virtual ~CompressedDepthPublisher() {} virtual std::string getTransportName() const @@ -57,15 +68,20 @@ class CompressedDepthPublisher : public image_transport::SimplePublisherPlugin parameters_; + std::vector deprecatedParameters_; + + rclcpp::Subscription::SharedPtr parameter_subscription_; - struct Config { - int png_level; - double depth_max; - double depth_quantization; - }; + void declareParameter(const std::string &base_name, + const ParameterDefinition &definition); - Config config_; + void onParameterEvent(ParameterEvent::SharedPtr event, std::string full_name, std::string base_name); }; } //namespace compressed_depth_image_transport diff --git a/compressed_depth_image_transport/include/compressed_depth_image_transport/compression_common.h b/compressed_depth_image_transport/include/compressed_depth_image_transport/compression_common.h index 811eed2..fc46d30 100644 --- a/compressed_depth_image_transport/include/compressed_depth_image_transport/compression_common.h +++ b/compressed_depth_image_transport/include/compressed_depth_image_transport/compression_common.h @@ -35,6 +35,9 @@ #ifndef COMPRESSED_DEPTH_IMAGE_TRANSPORT_COMPRESSION_COMMON #define COMPRESSED_DEPTH_IMAGE_TRANSPORT_COMPRESSION_COMMON +#include +#include + namespace compressed_depth_image_transport { @@ -53,6 +56,15 @@ struct ConfigHeader float depthParam[2]; }; +using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor; +using ParameterValue = rclcpp::ParameterValue; + +struct ParameterDefinition +{ + const ParameterValue defaultValue; + const ParameterDescriptor descriptor; +}; + } //namespace compressed_depth_image_transport #endif diff --git a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp index 285652e..30a0743 100644 --- a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp +++ b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp @@ -33,29 +33,64 @@ *********************************************************************/ #include "compressed_depth_image_transport/compressed_depth_publisher.h" -#include -#include -#include - -#include "compressed_depth_image_transport/codec.h" #include "compressed_depth_image_transport/compression_common.h" +#include "compressed_depth_image_transport/codec.h" #include +#include -#include -#include - -constexpr int kDefaultPngLevel = 9; -constexpr double kDefaultDepthMax = 10.0; -constexpr double KDefaultDepthQuantization = 100.0; - -using namespace cv; -using namespace std; +namespace compressed_depth_image_transport +{ -namespace enc = sensor_msgs::image_encodings; +enum compressedDepthParameters +{ + DEPTH_MAX = 0, + DEPTH_QUANTIZATION, + PNG_LEVEL +}; -namespace compressed_depth_image_transport +const struct ParameterDefinition kParameters[] = { + { //DEPTH_MAX - Maximum depth value (meter) + ParameterValue((double)10.0), + ParameterDescriptor() + .set__name("depth_max") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) + .set__description("Maximum depth value (meter)") + .set__read_only(false) + .set__floating_point_range( + {rcl_interfaces::msg::FloatingPointRange() + .set__from_value(1.0) + .set__to_value(100.0) + .set__step(0.0)}) + }, + { //DEPTH_QUANTIZATION - Depth value at which the sensor accuracy is 1 m (Kinect: >75) + ParameterValue((double)100.0), + ParameterDescriptor() + .set__name("depth_quantization") + .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) + .set__description("Depth value at which the sensor accuracy is 1 m (Kinect: >75)") + .set__read_only(false) + .set__floating_point_range( + {rcl_interfaces::msg::FloatingPointRange() + .set__from_value(1.0) + .set__to_value(150.0) + .set__step(0.0)}) + }, + { //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)}) + }, +}; void CompressedDepthPublisher::advertiseImpl( rclcpp::Node * node, @@ -63,73 +98,111 @@ void CompressedDepthPublisher::advertiseImpl( rmw_qos_profile_t custom_qos, rclcpp::PublisherOptions options) { + node_ = node; + typedef image_transport::SimplePublisherPlugin Base; Base::advertiseImpl(node, base_topic, 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 param_name = param_base_name + ".png_level"; - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.name = "png_level"; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.description = "PNG compression level"; - descriptor.read_only = false; - try { - config_.png_level = node->declare_parameter(param_name, kDefaultPngLevel, descriptor); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(node->get_logger(), "%s was previously declared", param_name.c_str()); - config_.png_level = node->get_parameter(param_name).get_value(); - } - } + using callbackT = std::function; + auto callback = std::bind(&CompressedDepthPublisher::onParameterEvent, this, std::placeholders::_1, + node->get_fully_qualified_name(), param_base_name); - { - std::string param_name = param_base_name + ".depth_max"; - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.name = "depth_max"; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.description = "Maximum depth value"; - descriptor.read_only = false; - try { - config_.depth_max = node->declare_parameter(param_name, kDefaultDepthMax, descriptor); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(node->get_logger(), "%s was previously declared", param_name.c_str()); - config_.depth_max = node->get_parameter(param_name).get_value(); - } - } + parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event(node, callback); - { - std::string param_name = param_base_name + ".depth_quantization"; - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.name = "depth_quantization"; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.description = "Depth quantization"; - descriptor.read_only = false; - try { - config_.depth_quantization = node->declare_parameter( - param_name, KDefaultDepthQuantization, descriptor); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { - RCLCPP_DEBUG(node->get_logger(), "%s was previously declared", param_name.c_str()); - config_.depth_quantization = node->get_parameter(param_name).get_value(); - } - } + for(const ParameterDefinition &pd : kParameters) + declareParameter(param_base_name, pd); } void CompressedDepthPublisher::publish( const sensor_msgs::msg::Image& message, const PublishFn& publish_fn) const { + // Fresh Configuration + double cfg_depth_max = node_->get_parameter(parameters_[DEPTH_MAX]).get_value(); + double cfg_depth_quantization = node_->get_parameter(parameters_[DEPTH_QUANTIZATION]).get_value(); + int cfg_png_level = node_->get_parameter(parameters_[PNG_LEVEL]).get_value(); + sensor_msgs::msg::CompressedImage::SharedPtr compressed_image = encodeCompressedDepthImage(message, - config_.depth_max, - config_.depth_quantization, - config_.png_level); + cfg_depth_max, + cfg_depth_quantization, + cfg_png_level); if (compressed_image) { publish_fn(*compressed_image); } } +void CompressedDepthPublisher::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 CompressedDepthPublisher::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_depth_image_transport From 32e8b819f0b967800c3108104e546e855c8efe92 Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Sun, 30 Apr 2023 16:30:51 +0200 Subject: [PATCH 3/9] compressed_depth based coment example using png_level Co-authored-by: Kenji Brameld --- .../src/compressed_depth_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp index 30a0743..53aa161 100644 --- a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp +++ b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp @@ -141,7 +141,7 @@ void CompressedDepthPublisher::publish( void CompressedDepthPublisher::declareParameter(const std::string &base_name, const ParameterDefinition &definition) { - //transport scoped parameter (e.g. image_raw.compressed.format) + //transport scoped parameter (e.g. image_raw.compressed_depth.png_level) const std::string transport_name = getTransportName(); const std::string param_name = base_name + "." + transport_name + "." + definition.descriptor.name; parameters_.push_back(param_name); From 136eb455a0fa1a984c72d532ccfadfd41bb52a10 Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Sun, 30 Apr 2023 16:31:45 +0200 Subject: [PATCH 4/9] compressed_depth based coment deprecated example using png_level Co-authored-by: Kenji Brameld --- .../src/compressed_depth_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp index 53aa161..81da7b2 100644 --- a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp +++ b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp @@ -146,7 +146,7 @@ void CompressedDepthPublisher::declareParameter(const std::string &base_name, 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) + //deprecated non-scoped parameter name (e.g. image_raw.png_level) const std::string deprecated_name = base_name + "." + definition.descriptor.name; deprecatedParameters_.push_back(deprecated_name); From d500eaeccd9d43e40d0c3817ba6b36eea21ea5a6 Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Sun, 30 Apr 2023 17:00:56 +0200 Subject: [PATCH 5/9] comment `color.image_raw.` + `compressed_depth` + `.png_level` Co-authored-by: Kenji Brameld --- .../src/compressed_depth_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp index 81da7b2..2c5b60c 100644 --- a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp +++ b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp @@ -188,7 +188,7 @@ void CompressedDepthPublisher::onParameterEvent(ParameterEvent::SharedPtr event, 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` + //e.g. `color.image_raw.` + `compressed_depth` + `.png_level` std::string recommendedName = name.substr(0, paramNameIndex + 1) + transport + name.substr(paramNameIndex); rclcpp::Parameter recommendedValue = node_->get_parameter(recommendedName); From 9844501e26aa19dc5ead47460ec5d6b31ccd7a9f Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Sun, 30 Apr 2023 17:05:02 +0200 Subject: [PATCH 6/9] compressed_depth -> compressedDepth in comments for transport --- .../src/compressed_depth_publisher.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp index 2c5b60c..ffd1922 100644 --- a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp +++ b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp @@ -141,7 +141,7 @@ void CompressedDepthPublisher::publish( void CompressedDepthPublisher::declareParameter(const std::string &base_name, const ParameterDefinition &definition) { - //transport scoped parameter (e.g. image_raw.compressed_depth.png_level) + //transport scoped parameter (e.g. image_raw.compressedDepth.png_level) const std::string transport_name = getTransportName(); const std::string param_name = base_name + "." + transport_name + "." + definition.descriptor.name; parameters_.push_back(param_name); @@ -188,7 +188,7 @@ void CompressedDepthPublisher::onParameterEvent(ParameterEvent::SharedPtr event, 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_depth` + `.png_level` + //e.g. `color.image_raw.` + `compressedDepth` + `png_level` std::string recommendedName = name.substr(0, paramNameIndex + 1) + transport + name.substr(paramNameIndex); rclcpp::Parameter recommendedValue = node_->get_parameter(recommendedName); From 1e7bad4a876be643dc8f962e8babc74d2a40deb0 Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Tue, 2 May 2023 16:26:52 +0200 Subject: [PATCH 7/9] adjust deprecated warning with "ambiguous" For: - compressed_depth_image_transport - compressed_image_transport --- .../src/compressed_depth_publisher.cpp | 2 +- .../src/compressed_publisher.cpp | 38 ++++++++++++++++++- .../src/compressed_subscriber.cpp | 2 +- 3 files changed, 38 insertions(+), 4 deletions(-) diff --git a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp index ffd1922..1c77dde 100644 --- a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp +++ b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp @@ -197,7 +197,7 @@ void CompressedDepthPublisher::onParameterEvent(ParameterEvent::SharedPtr event, if(it.second->value == recommendedValue.get_value_message()) continue; - RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated" << + RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated and ambiguous" << "; use transport qualified name `" << recommendedName << "`"); node_->set_parameter(rclcpp::Parameter(recommendedName, it.second->value)); diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index 3420a57..604af21 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -301,7 +301,41 @@ void CompressedPublisher::publish( { RCUTILS_LOG_ERROR("%s", e.what()); return; - } + }/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2012, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + + // Publish message publish_fn(compressed); @@ -444,7 +478,7 @@ void CompressedPublisher::onParameterEvent(ParameterEvent::SharedPtr event, std: if(it.second->value == recommendedValue.get_value_message()) continue; - RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated" << + RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated and ambiguous" << "; use transport qualified name `" << recommendedName << "`"); node_->set_parameter(rclcpp::Parameter(recommendedName, it.second->value)); diff --git a/compressed_image_transport/src/compressed_subscriber.cpp b/compressed_image_transport/src/compressed_subscriber.cpp index 7da5117..f9c135d 100644 --- a/compressed_image_transport/src/compressed_subscriber.cpp +++ b/compressed_image_transport/src/compressed_subscriber.cpp @@ -263,7 +263,7 @@ void CompressedSubscriber::onParameterEvent(ParameterEvent::SharedPtr event, std if(it.second->value == recommendedValue.get_value_message()) continue; - RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated" << + RCLCPP_WARN_STREAM(logger_, "parameter `" << name << "` is deprecated and ambiguous" << "; use transport qualified name `" << recommendedName << "`"); node_->set_parameter(rclcpp::Parameter(recommendedName, it.second->value)); From b60491319c3441179a96a1088815eed89937bea3 Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Tue, 2 May 2023 17:12:41 +0200 Subject: [PATCH 8/9] remove excess deprecated value reads for compressed transports For already declared attributes those reads do nothing --- .../src/compressed_depth_publisher.cpp | 1 - compressed_image_transport/src/compressed_publisher.cpp | 1 - compressed_image_transport/src/compressed_subscriber.cpp | 1 - 3 files changed, 3 deletions(-) diff --git a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp index 1c77dde..2dfcd49 100644 --- a/compressed_depth_image_transport/src/compressed_depth_publisher.cpp +++ b/compressed_depth_image_transport/src/compressed_depth_publisher.cpp @@ -164,7 +164,6 @@ void CompressedDepthPublisher::declareParameter(const std::string &base_name, 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(); } } diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index 604af21..635ea75 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -445,7 +445,6 @@ void CompressedPublisher::declareParameter(const std::string &base_name, 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(); } } diff --git a/compressed_image_transport/src/compressed_subscriber.cpp b/compressed_image_transport/src/compressed_subscriber.cpp index f9c135d..3260d2d 100644 --- a/compressed_image_transport/src/compressed_subscriber.cpp +++ b/compressed_image_transport/src/compressed_subscriber.cpp @@ -230,7 +230,6 @@ void CompressedSubscriber::declareParameter(const std::string &base_name, 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(); } } From 992cc64c1c5f3d37e5243c40afd93aa413076d2f Mon Sep 17 00:00:00 2001 From: Bartosz Meglicki Date: Fri, 5 May 2023 09:09:58 +0200 Subject: [PATCH 9/9] remove comment block that slipped inside code Requested in #145 --- .../src/compressed_publisher.cpp | 36 +------------------ 1 file changed, 1 insertion(+), 35 deletions(-) diff --git a/compressed_image_transport/src/compressed_publisher.cpp b/compressed_image_transport/src/compressed_publisher.cpp index 635ea75..8177f49 100644 --- a/compressed_image_transport/src/compressed_publisher.cpp +++ b/compressed_image_transport/src/compressed_publisher.cpp @@ -301,41 +301,7 @@ void CompressedPublisher::publish( { RCUTILS_LOG_ERROR("%s", e.what()); return; - }/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2012, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - - + } // Publish message publish_fn(compressed);