diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp index 74521824de47c2c733f52c2e4c39342c83fb7c51..76e96b7584ed740df4ca77f6512105cf9d27a181 100644 --- a/src/publisher_vision.cpp +++ b/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