[image_tools] Update ROSCVMatContainer and do not convert color space of frames

image_tools/include/image_tools/cv_mat_sensor_msgs_image_type_adapter.hpp

@@ -17,6 +17,7 @@
 
 #include <cstddef>
 #include <memory>
+#include <optional>
 #include <variant>  // NOLINT[build/include_order]
 
 #include "opencv2/core/mat.hpp"
@@ -85,6 +86,7 @@ enum class endian
  */
 class ROSCvMatContainer
 {
+public:
   static constexpr bool is_bigendian_system = detail::endian::native == detail::endian::big;
 
 public:
@@ -141,14 +143,16 @@ class ROSCvMatContainer
   ROSCvMatContainer(
     const cv::Mat & mat_frame,
     const std_msgs::msg::Header & header,
-    bool is_bigendian = is_bigendian_system);
+    bool is_bigendian = is_bigendian_system,
+    std::optional<std::string> encoding_override = std::nullopt);
 
   /// Move the given cv::Mat into this class.
   IMAGE_TOOLS_PUBLIC
   ROSCvMatContainer(
     cv::Mat && mat_frame,
     const std_msgs::msg::Header & header,
-    bool is_bigendian = is_bigendian_system);
+    bool is_bigendian = is_bigendian_system,
+    std::optional<std::string> encoding_override = std::nullopt);
 
   /// Copy the sensor_msgs::msg::Image into this contain and create a cv::Mat that references it.
   IMAGE_TOOLS_PUBLIC
@@ -212,11 +216,17 @@ class ROSCvMatContainer
   bool
   is_bigendian() const;
 
+  /// Return the encoding override if provided.
+  IMAGE_TOOLS_PUBLIC
+  std::optional<std::string>
+  encoding_override() const;
+
 private:
   std_msgs::msg::Header header_;
   cv::Mat frame_;
   SensorMsgsImageStorageType storage_;
   bool is_bigendian_;
+  std::optional<std::string> encoding_override_;
 };
 
 }  // namespace image_tools
@@ -236,7 +246,14 @@ struct rclcpp::TypeAdapter<image_tools::ROSCvMatContainer, sensor_msgs::msg::Ima
   {
     destination.height = source.cv_mat().rows;
     destination.width = source.cv_mat().cols;
-    switch (source.cv_mat().type()) {
+    const auto& encoding_override = source.encoding_override();
+    if (encoding_override.has_value() && !encoding_override.value().empty())
+    {
+      destination.encoding = encoding_override.value();
+    }
+    else
+    {
+      switch (source.cv_mat().type()) {
       case CV_8UC1:
         destination.encoding = "mono8";
         break;
@@ -251,6 +268,7 @@ struct rclcpp::TypeAdapter<image_tools::ROSCvMatContainer, sensor_msgs::msg::Ima
         break;
       default:
         throw std::runtime_error("unsupported encoding type");
+      }
     }
     destination.step = static_cast<sensor_msgs::msg::Image::_step_type>(source.cv_mat().step);
     size_t size = source.cv_mat().step * source.cv_mat().rows;

image_tools/src/cam2image.cpp

@@ -14,6 +14,7 @@
 
 #include <chrono>
 #include <memory>
+#include <optional>
 #include <sstream>
 #include <string>
 #include <utility>
@@ -148,7 +149,12 @@ class Cam2Image : public rclcpp::Node
     std_msgs::msg::Header header;
     header.frame_id = frame_id_;
     header.stamp = this->now();
-    image_tools::ROSCvMatContainer container(frame, header);
+    image_tools::ROSCvMatContainer container(
+      frame,
+      header,
+      ROSCvMatContainer::is_bigendian_system,
+      encoding_override_
+    );
 
     // Publish the image message and increment the publish_number_.
     RCLCPP_INFO(get_logger(), "Publishing image #%zd", publish_number_++);
@@ -249,6 +255,15 @@ class Cam2Image : public rclcpp::Node
     burger_mode_desc.description = "Produce images of burgers rather than connecting to a camera";
     burger_mode_ = this->declare_parameter("burger_mode", false, burger_mode_desc);
     frame_id_ = this->declare_parameter("frame_id", "camera_frame");
+    std::string encoding_override = this->declare_parameter("encoding_override", "");
+    if (encoding_override.empty())
+    {
+      encoding_override_ = std::nullopt;
+    }
+    else
+    {
+      encoding_override_ = std::move(encoding_override);
+    }
   }
 
   cv::VideoCapture cap;
@@ -269,6 +284,7 @@ class Cam2Image : public rclcpp::Node
   bool burger_mode_;
   std::string frame_id_;
   int device_id_;
+  std::optional<std::string> encoding_override_;
 
   /// If true, will cause the incoming camera image message to flip about the y-axis.
   bool is_flipped_;

image_tools/src/cv_mat_sensor_msgs_image_type_adapter.cpp

@@ -100,21 +100,25 @@ ROSCvMatContainer::ROSCvMatContainer(
 ROSCvMatContainer::ROSCvMatContainer(
   const cv::Mat & mat_frame,
   const std_msgs::msg::Header & header,
-  bool is_bigendian)
+  bool is_bigendian,
+  std::optional<std::string> encoding_override)
 : header_(header),
   frame_(mat_frame),
   storage_(nullptr),
-  is_bigendian_(is_bigendian)
+  is_bigendian_(is_bigendian),
+  encoding_override_(encoding_override)
 {}
 
 ROSCvMatContainer::ROSCvMatContainer(
   cv::Mat && mat_frame,
   const std_msgs::msg::Header & header,
-  bool is_bigendian)
+  bool is_bigendian,
+  std::optional<std::string> encoding_override)
 : header_(header),
   frame_(std::forward<cv::Mat>(mat_frame)),
   storage_(nullptr),
-  is_bigendian_(is_bigendian)
+  is_bigendian_(is_bigendian),
+  encoding_override_(encoding_override)
 {}
 
 ROSCvMatContainer::ROSCvMatContainer(
@@ -175,21 +179,28 @@ ROSCvMatContainer::get_sensor_msgs_msg_image_copy(
 {
   sensor_msgs_image.height = frame_.rows;
   sensor_msgs_image.width = frame_.cols;
-  switch (frame_.type()) {
-    case CV_8UC1:
-      sensor_msgs_image.encoding = "mono8";
-      break;
-    case CV_8UC3:
-      sensor_msgs_image.encoding = "bgr8";
-      break;
-    case CV_16SC1:
-      sensor_msgs_image.encoding = "mono16";
-      break;
-    case CV_8UC4:
-      sensor_msgs_image.encoding = "rgba8";
-      break;
-    default:
-      throw std::runtime_error("unsupported encoding type");
+  if (encoding_override_.has_value() && !encoding_override_.value().empty())
+  {
+    sensor_msgs_image.encoding = encoding_override_.value();
+  }
+  else
+  {
+    switch (frame_.type()) {
+      case CV_8UC1:
+        sensor_msgs_image.encoding = "mono8";
+        break;
+      case CV_8UC3:
+        sensor_msgs_image.encoding = "bgr8";
+        break;
+      case CV_16SC1:
+        sensor_msgs_image.encoding = "mono16";
+        break;
+      case CV_8UC4:
+        sensor_msgs_image.encoding = "rgba8";
+        break;
+      default:
+        throw std::runtime_error("unsupported encoding type");
+    }
   }
   sensor_msgs_image.step = static_cast<sensor_msgs::msg::Image::_step_type>(frame_.step);
   size_t size = frame_.step * frame_.rows;
@@ -204,4 +215,10 @@ ROSCvMatContainer::is_bigendian() const
   return is_bigendian_;
 }
 
+std::optional<std::string>
+ROSCvMatContainer::encoding_override() const
+{
+  return encoding_override_;
+}
+
 }  // namespace image_tools

image_tools/src/showimage.cpp

@@ -179,12 +179,12 @@ class ShowImage : public rclcpp::Node
     if (show_image) {
       cv::Mat frame = container.cv_mat();
 
-      if (frame.type() == CV_8UC3 /* rgb8 */) {
-        cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);
-      } else if (frame.type() == CV_8UC2) {
-        container.is_bigendian() ? cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_UYVY) :
-        cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_YUYV);
-      }
+      // if (frame.type() == CV_8UC3 /* rgb8 */) {
+      //   cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);
+      // } else if (frame.type() == CV_8UC2) {
+      //   container.is_bigendian() ? cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_UYVY) :
+      //   cv::cvtColor(frame, frame, cv::COLOR_YUV2BGR_YUYV);
+      // }
 
       // Show the image in a window
       cv::imshow(window_name_, frame);