JPEG only supports 8 bits images

compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_publisher.h

@@ -34,7 +34,7 @@
 
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
-#include "image_transport/simple_publisher_plugin.h"
+#include <image_transport/simple_publisher_plugin.hpp>
 
 namespace compressed_depth_image_transport {
 

compressed_depth_image_transport/include/compressed_depth_image_transport/compressed_depth_subscriber.h

@@ -36,7 +36,7 @@
 #include <rclcpp/node.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
-#include "image_transport/simple_subscriber_plugin.h"
+#include <image_transport/simple_subscriber_plugin.hpp>
 
 namespace compressed_depth_image_transport {
 

compressed_image_transport/include/compressed_image_transport/compressed_subscriber.h

@@ -35,14 +35,15 @@
 #include <string>
 
 #include <rclcpp/node.hpp>
+#include <rclcpp/subscription_options.hpp>
 
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <image_transport/simple_subscriber_plugin.hpp>
 
 namespace compressed_image_transport {
 
-class CompressedSubscriber : public image_transport::SimpleSubscriberPlugin<sensor_msgs::msg::CompressedImage>
+class CompressedSubscriber final : public image_transport::SimpleSubscriberPlugin<sensor_msgs::msg::CompressedImage>
 {
 public:
   CompressedSubscriber(): logger_(rclcpp::get_logger("CompressedSubscriber")) {}
@@ -61,6 +62,13 @@ class CompressedSubscriber : public image_transport::SimpleSubscriberPlugin<sens
       const Callback& callback,
       rmw_qos_profile_t custom_qos) override;
 
+  void subscribeImpl(
+      rclcpp::Node * ,
+      const std::string& base_topic,
+      const Callback& callback,
+      rmw_qos_profile_t custom_qos,
+      rclcpp::SubscriptionOptions options) override;
+
   void internalCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr& message,
                         const Callback& user_cb) override;
 

compressed_image_transport/src/compressed_publisher.cpp

@@ -40,6 +40,7 @@
 
 #include "compressed_image_transport/compression_common.h"
 
+#include <rclcpp/exceptions/exceptions.hpp>
 #include <rclcpp/parameter_client.hpp>
 
 #include <sstream>
@@ -69,59 +70,55 @@ void CompressedPublisher::advertiseImpl(
   std::string param_base_name = base_topic.substr(ns_len);
   std::replace(param_base_name.begin(), param_base_name.end(), '/', '.');
   std::string format_param_name = param_base_name + ".format";
-  if (!node->has_parameter(format_param_name))
-  {
-    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]";
+  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]";
+  try {
     config_.format = node->declare_parameter(format_param_name, kDefaultFormat, format_description);
-  }
-  else
-  {
-    RCLCPP_WARN(logger_, "%s was previously declared", format_param_name.c_str());
+  } 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<std::string>();
   }
 
   std::string png_level_param_name = param_base_name + ".png_level";
-  if (!node->has_parameter(png_level_param_name))
-  {
-    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);
-    config_.png_level = node->declare_parameter(png_level_param_name, kDefaultPngLevel, png_level_description);
-  }
-  else
-  {
-    RCLCPP_WARN(logger_, "%s was previously delared", png_level_param_name.c_str());
+  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<int64_t>();
   }
 
   std::string jpeg_quality_param_name = param_base_name + ".jpeg_quality";
-  if (!node->has_parameter(jpeg_quality_param_name))
-  {
-    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);
-    config_.jpeg_quality = node->declare_parameter(jpeg_quality_param_name, kDefaultJpegQuality, jpeg_quality_description);
-  }
-  else
-  {
-    RCLCPP_WARN(logger_, "%s was previously delared", jpeg_quality_param_name.c_str());
+  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<int64_t>();
   }
 }
 
@@ -169,6 +166,10 @@ void CompressedPublisher::publish(
           // convert color images to BGR8 format
           targetFormat = "bgr8";
           compressed.format += targetFormat;
+        } else {
+          // convert gray images to mono8 format
+          targetFormat = "mono8";
+          compressed.format += targetFormat;
         }
 
         // OpenCV-ros bridge

compressed_image_transport/src/compressed_subscriber.cpp

@@ -62,18 +62,36 @@ void CompressedSubscriber::subscribeImpl(
     const std::string& base_topic,
     const Callback& callback,
     rmw_qos_profile_t custom_qos)
+{
+  this->subscribeImpl(node, base_topic, callback, custom_qos, rclcpp::SubscriptionOptions{});
+}
+
+void CompressedSubscriber::subscribeImpl(
+    rclcpp::Node * node,
+    const std::string& base_topic,
+    const Callback& callback,
+    rmw_qos_profile_t custom_qos,
+    rclcpp::SubscriptionOptions options)
 {
     logger_ = node->get_logger();
     typedef image_transport::SimpleSubscriberPlugin<CompressedImage> Base;
-    Base::subscribeImpl(node, base_topic, callback, custom_qos);
+    Base::subscribeImplWithOptions(node, base_topic, callback, 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(), '/', '.');
+    std::string mode_param_name = param_base_name + ".mode";
+
     std::string mode;
     rcl_interfaces::msg::ParameterDescriptor mode_description;
-    mode_description.name = "mode";
-    mode_description.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
     mode_description.description = "OpenCV imdecode flags to use";
     mode_description.read_only = false;
     mode_description.additional_constraints = "Supported values: [unchanged, gray, color]";
-    mode = node->declare_parameter("mode", kDefaultMode, mode_description);
+    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<std::string>();
+    }
 
     if (mode == "unchanged") {
       config_.imdecode_flag = cv::IMREAD_UNCHANGED;
@@ -155,6 +173,9 @@ void CompressedSubscriber::internalCallback(const CompressedImage::ConstSharedPt
             cv::cvtColor(cv_ptr->image, cv_ptr->image, CV_RGB2RGBA);
         }
       }
+      if (enc::bitDepth(image_encoding) == 16) {
+        cv_ptr->image.convertTo(cv_ptr->image, CV_16U, 256);
+      }
     }
   }
   catch (cv::Exception& e)

theora_image_transport/include/theora_image_transport/theora_publisher.h

@@ -35,7 +35,7 @@
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 
-#include <image_transport/simple_publisher_plugin.h>
+#include <image_transport/simple_publisher_plugin.hpp>
 #include <cv_bridge/cv_bridge.h>
 #include <std_msgs/msg/header.hpp>
 #include <theora_image_transport/msg/packet.hpp>

theora_image_transport/include/theora_image_transport/theora_subscriber.h

@@ -32,7 +32,7 @@
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
-#include <image_transport/simple_subscriber_plugin.h>
+#include <image_transport/simple_subscriber_plugin.hpp>
 #include <theora_image_transport/msg/packet.hpp>
 
 #include <theora/codec.h>