add single channel grayscale parsing for color inputs

Author: Schmluk

hydra_ros/src/input/image_receiver.cpp

@@ -56,11 +56,28 @@ ColorSubscriber::Filter& ColorSubscriber::getFilter() const {
 }
 
 void ColorSubscriber::fillInput(const Image& img, ImageInputPacket& packet) const {
-  try {
-    packet.color = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8)->image;
-  } catch (const cv_bridge::Exception& e) {
-    LOG(ERROR) << "Failed to convert color image: " << e.what();
+  // Allow also mono images to be converted to grayscale.
+  if (sensor_msgs::image_encodings::isColor(img.encoding)) {
+    try {
+      packet.color =
+          cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8)->image;
+      return;
+    } catch (const cv_bridge::Exception& e) {
+      LOG(ERROR) << "Failed to convert mono image as color input: " << e.what();
+      return;
+    }
+  } else if (sensor_msgs::image_encodings::isMono(img.encoding)) {
+    try {
+      cv::Mat mono =
+          cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8)->image;
+      cv::cvtColor(mono, packet.color, cv::COLOR_GRAY2RGB);
+      return;
+    } catch (const cv_bridge::Exception& e) {
+      LOG(ERROR) << "Failed to convert color image: " << e.what();
+      return;
+    }
   }
+  LOG(ERROR) << "Failed to convert color image: unsupported encoding: " << img.encoding;
 }
 
 DepthSubscriber::DepthSubscriber() = default;