diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp index 7ef13cf457c07..6ab9f09064f58 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/node.cpp @@ -201,9 +201,9 @@ void TrafficLightRoiVisualizerNode::imageRoughRoiCallback( // 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)); - + // note: a signal will still be output even if it is undetected + // Its position and size will be set as 0 and the color will be set as unknown + // So a rough roi will always have correspond roi a correspond traffic signal ClassificationResult result; bool has_correspond_traffic_signal = getClassificationResult(tl_rough_roi.traffic_light_id, *input_traffic_signals_msg, result); @@ -211,6 +211,8 @@ void TrafficLightRoiVisualizerNode::imageRoughRoiCallback( bool has_correspond_roi = getRoiFromId(tl_rough_roi.traffic_light_id, input_tl_roi_msg, tl_roi); + createRect(cv_ptr->image, tl_rough_roi, extractShapeInfo(result.label).color); + if (has_correspond_roi && has_correspond_traffic_signal) { // has fine detection and classification results createRect(cv_ptr->image, tl_roi, result); diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp index 8c7a59cbcb003..d3ab30146a8b5 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp @@ -29,6 +29,10 @@ void drawShape( cv::Mat & image, const std::vector & params, int size, const cv::Point & position, const cv::Scalar & color, float probability) { + // skip if the roi position is set as (0,0), which means it is undetected + if (position.x == 0 && position.y == 0) { + return; + } // load concatenated shape image const auto shape_img = loadShapeImage(params, size); @@ -42,14 +46,13 @@ void drawShape( const int fill_rect_h = std::max(shape_img.rows, text_size.height) + 10; const cv::Point rect_position(position.x, position.y - fill_rect_h); - if ( rect_position.x < 0 || rect_position.y < 0 || rect_position.x + fill_rect_w > image.cols || position.y > image.rows) { // TODO(KhalilSelyan): This error message may flood the terminal logs, so commented out // temporarily. Need to consider a better way. - // std::cerr << "Adjusted position is out of image bounds." << std::endl; + std::cerr << "Adjusted position is out of image bounds." << std::endl; return; } cv::Rect rectangle(rect_position.x, rect_position.y, fill_rect_w, fill_rect_h);