Merge pull request #505 from Yadunund/yadu/custom_encoding_for_ROSCvM…

…atContainer Allow users to override encoding string in ROSCvMatContainer
ros-perception · Feb 4, 2023 · b6842b0 · b6842b0
2 parents c620dd9 + 4fd66b3
commit b6842b0
Show file tree

Hide file tree

Showing 3 changed files with 59 additions and 24 deletions.
diff --git a/cv_bridge/CMakeLists.txt b/cv_bridge/CMakeLists.txt
@@ -3,9 +3,9 @@ project(cv_bridge)
 
 find_package(ament_cmake_ros REQUIRED)
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

diff --git a/cv_bridge/include/cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp b/cv_bridge/include/cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp
@@ -26,6 +26,8 @@
 
 #include "cv_bridge/visibility_control.h"
 
+#include <optional>
+
 namespace cv_bridge
 {
 namespace detail
@@ -139,14 +141,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.
   CV_BRIDGE_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.
   CV_BRIDGE_PUBLIC
@@ -209,12 +213,18 @@ class ROSCvMatContainer
   CV_BRIDGE_PUBLIC
   bool
   is_bigendian() const;
+
+  /// Return the encoding override if provided.
+  CV_BRIDGE_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 cv_bridge
@@ -234,7 +244,14 @@ struct rclcpp::TypeAdapter<cv_bridge::ROSCvMatContainer, sensor_msgs::msg::Image
   {
     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;
@@ -249,6 +266,7 @@ struct rclcpp::TypeAdapter<cv_bridge::ROSCvMatContainer, sensor_msgs::msg::Image
         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;

diff --git a/cv_bridge/src/cv_mat_sensor_msgs_image_type_adapter.cpp b/cv_bridge/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 cv_bridge