[skip-ci] check number of channels of image on which to draw

src/publisher_vision.cpp

@@ -69,9 +69,11 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
         last_capture_(nullptr),
         img_transport_(ros::NodeHandle())
 {
+    WOLF_INFO("\n\n\n\n\n\n\n\nYOOOOOOOOOOOOOOOOOOOO PublisherVisionDebug")
+
     // if user do not provide processor's name, first processor of type PublisherVisionDebug is taken
     auto processor_name                    = getParamWithDefault<std::string>(_server, prefix_ + "/processor_name", "");
-    topic_preprocessor_                    = getParamWithDefault<std::string>(_server, prefix_ + "/topic_preprocessor", "/debug_image_prepocessor");
+    topic_preprocessor_                    = getParamWithDefault<std::string>(_server, prefix_ + "/topic_preprocessor", "/image_vo_tracks_prepocessor");
     //Tracks      
     tracks_.show_id_                       = getParamWithDefault<bool>(_server, prefix_ + "/tracks/show_id", false);
     tracks_.size_id_                       = getParamWithDefault<double>(_server, prefix_ + "/tracks/size_id", 0.5);
@@ -142,6 +144,7 @@ bool PublisherVisionDebug::ready()
 
 void PublisherVisionDebug::publishDerived()
 {
+    WOLF_INFO("YO")
     if (last_capture_ == processor_vision_->getLast() or not processor_vision_->getLast())
         return;
 
@@ -157,9 +160,14 @@ void PublisherVisionDebug::publishDerived()
     // Draw and publish debug image
     try
     {
-        // Extract cv image
-        cv::Mat cv_img_debug;
-        cv::cvtColor(cap_img->getImage(), cv_img_debug, cv::COLOR_GRAY2BGR);
+        // Transform the image to RGB if grayscale so that we can draw detections in color for debugging
+        cv::Mat cv_img_debug; 
+        if (cap_img->getImage().channels() == 1){
+            cv::cvtColor(cap_img->getImage(), cv_img_debug, cv::COLOR_GRAY2BGR);
+        }        
+        else {
+            cv_img_debug = cap_img->getImage().clone();
+        }
 
 
         // Draw all tracks