Merge pull request #145 from Extend-Robotics/depth-transport-scoped

[ROS2] reconfigurable transport scoped parameters for CompressedDepthPublisher

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 <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <image_transport/simple_publisher_plugin.hpp>
 
+#include <rclcpp/node.hpp>
+
+#include <string>
+#include <vector>
+
 namespace compressed_depth_image_transport {
 
-class CompressedDepthPublisher : public image_transport::SimplePublisherPlugin<sensor_msgs::msg::CompressedImage>
+using CompressedImage = sensor_msgs::msg::CompressedImage;
+using ParameterEvent = rcl_interfaces::msg::ParameterEvent;
+
+class CompressedDepthPublisher : public image_transport::SimplePublisherPlugin<CompressedImage>
 {
 public:
+  CompressedDepthPublisher(): logger_(rclcpp::get_logger("CompressedDepthPublisher")) {}
   virtual ~CompressedDepthPublisher() {}
 
   virtual std::string getTransportName() const
@@ -57,15 +68,20 @@ class CompressedDepthPublisher : public image_transport::SimplePublisherPlugin<s
           rclcpp::PublisherOptions options) override final;
 
   void publish(const sensor_msgs::msg::Image& message,
-                       const PublishFn& publish_fn) const override final;
+               const PublishFn& publish_fn) const override final;
+
+  rclcpp::Logger logger_;
+  rclcpp::Node * node_;
+private:
+  std::vector<std::string> parameters_;
+  std::vector<std::string> deprecatedParameters_;
+
+  rclcpp::Subscription<ParameterEvent>::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

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 <rclcpp/parameter_value.hpp>
+#include <rcl_interfaces/msg/parameter_descriptor.hpp>
+
 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

compressed_depth_image_transport/src/compressed_depth_publisher.cpp

@@ -33,57 +33,175 @@
 *********************************************************************/
 
 #include "compressed_depth_image_transport/compressed_depth_publisher.h"
-#include <cv_bridge/cv_bridge.hpp>
-#include <sensor_msgs/image_encodings.hpp>
-#include <opencv2/highgui/highgui.hpp>
-
-#include "compressed_depth_image_transport/codec.h"
 #include "compressed_depth_image_transport/compression_common.h"
+#include "compressed_depth_image_transport/codec.h"
 
 #include <rclcpp/parameter_client.hpp>
+#include <rclcpp/parameter_events_filter.hpp>
 
-#include <vector>
-#include <sstream>
-
-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,
   const std::string& base_topic,
   rmw_qos_profile_t custom_qos,
   rclcpp::PublisherOptions options)
 {
+  node_ = node;
+
   typedef image_transport::SimplePublisherPlugin<sensor_msgs::msg::CompressedImage> Base;
   Base::advertiseImpl(node, base_topic, custom_qos, options);
 
-  node->get_parameter_or<int>("png_level", config_.png_level, kDefaultPngLevel);
-  node->get_parameter_or<double>("depth_max", config_.depth_max, kDefaultDepthMax);
-  node->get_parameter_or<double>("depth_quantization", config_.depth_max, KDefaultDepthQuantization);
+  // 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<void(ParameterEvent::SharedPtr event)>;
+  auto callback = std::bind(&CompressedDepthPublisher::onParameterEvent, this, std::placeholders::_1,
+                            node->get_fully_qualified_name(), param_base_name);
+
+  parameter_subscription_ = rclcpp::SyncParametersClient::on_parameter_event<callbackT>(node, callback);
+
+  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>();
+  double cfg_depth_quantization = node_->get_parameter(parameters_[DEPTH_QUANTIZATION]).get_value<double>();
+  int cfg_png_level = node_->get_parameter(parameters_[PNG_LEVEL]).get_value<int64_t>();
+
   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.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);
+
+  //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);
+
+  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());
+  }
+}
+
+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.` + `compressedDepth` + `png_level`
+    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 and ambiguous" <<
+                                "; use transport qualified name `" << recommendedName << "`");
+
+    node_->set_parameter(rclcpp::Parameter(recommendedName, it.second->value));
+  }
+}
+
+
 } //namespace compressed_depth_image_transport

compressed_image_transport/src/compressed_publisher.cpp

@@ -411,7 +411,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();
   }
 }
 
@@ -444,7 +443,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));

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();
   }
 }
 
@@ -263,7 +262,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));