feat(traffic_light_visualization): update visualization for w/o fine_…

…detection (autowarefoundation#4967) * feat(traffic_light_visualization): update visualization for w/o fine_detection Signed-off-by: Shunsuke Miura <[email protected]> * add comment Signed-off-by: Shunsuke Miura <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Shunsuke Miura <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
esteve · Sep 13, 2023 · be5fa16 · be5fa16
1 parent dd1ec18
commit be5fa16
Showing 1 changed file with 19 additions and 5 deletions.
diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp
@@ -127,13 +127,25 @@ void TrafficLightRoiVisualizerNodelet::imageRoiCallback(
 {
   cv_bridge::CvImagePtr cv_ptr;
   try {
-    cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding);
+    // try to convert to RGB8 from any input encoding, since createRect() only supports RGB8 based
+    // bbox drawing
+    cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8);
     for (auto tl_roi : input_tl_roi_msg->rois) {
-      createRect(cv_ptr->image, tl_roi, cv::Scalar(0, 255, 0));
+      ClassificationResult result;
+      bool has_correspond_traffic_signal =
+        getClassificationResult(tl_roi.traffic_light_id, *input_traffic_signals_msg, result);
+
+      if (!has_correspond_traffic_signal) {
+        // does not have classification result
+        createRect(cv_ptr->image, tl_roi, cv::Scalar(255, 255, 255));
+      } else {
+        // has classification result
+        createRect(cv_ptr->image, tl_roi, result);
+      }
     }
   } catch (cv_bridge::Exception & e) {
     RCLCPP_ERROR(
-      get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str());
+      get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str());
   }
   image_pub_.publish(cv_ptr->toImageMsg());
 }
@@ -182,7 +194,9 @@ void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback(
 {
   cv_bridge::CvImagePtr cv_ptr;
   try {
-    cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding);
+    // try to convert to RGB8 from any input encoding, since createRect() only supports RGB8 based
+    // bbox drawing
+    cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8);
     for (auto tl_rough_roi : input_tl_rough_roi_msg->rois) {
       // visualize rough roi
       createRect(cv_ptr->image, tl_rough_roi, cv::Scalar(0, 255, 0));
@@ -208,7 +222,7 @@ void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback(
     }
   } catch (cv_bridge::Exception & e) {
     RCLCPP_ERROR(
-      get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str());
+      get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str());
   }
   image_pub_.publish(cv_ptr->toImageMsg());
 }