From 6e4fe3c3586b8285d739e11114dbb5a727ec08cc Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Tue, 9 Jan 2024 11:31:08 +0900 Subject: [PATCH] feat(traffic_light): output undetected traffic light as unknown (#5934) * fix: output undetected traffic light as unknown Signed-off-by: tzhong518 * Update perception/traffic_light_classifier/src/nodelet.cpp Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> * Update perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> * style(pre-commit): autofix * fix: unnecessary count Signed-off-by: tzhong518 * style(pre-commit): autofix * fix: improve readability Signed-off-by: tzhong518 * fix: index error Signed-off-by: tzhong518 * fix: append the undetected rois at the end so that the order can be easily kept Signed-off-by: tzhong518 * fix: false occlusion ratio for undetected signals Signed-off-by: tzhong518 --------- Signed-off-by: tzhong518 Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../traffic_light_classifier/src/nodelet.cpp | 30 +++++++++++++++---- .../src/nodelet.cpp | 28 ++++++++++++----- .../src/nodelet.cpp | 14 ++++++++- .../src/occlusion_predictor.cpp | 6 +++- 4 files changed, 63 insertions(+), 15 deletions(-) diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp index 3a00bb013d867..1ebc14c4e6f93 100644 --- a/perception/traffic_light_classifier/src/nodelet.cpp +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "traffic_light_classifier/nodelet.hpp" +#include + #include #include #include @@ -98,16 +100,18 @@ void TrafficLightClassifierNodelet::imageRoiCallback( output_msg.signals.resize(input_rois_msg->rois.size()); std::vector images; - size_t num_valid_roi = 0; std::vector backlight_indices; for (size_t i = 0; i < input_rois_msg->rois.size(); i++) { - // skip if not the expected type of roi + // skip if the roi is not detected + if (input_rois_msg->rois.at(i).roi.height == 0) { + break; + } if (input_rois_msg->rois.at(i).traffic_light_type != classify_traffic_light_type_) { continue; } - output_msg.signals[num_valid_roi].traffic_light_id = + output_msg.signals[images.size()].traffic_light_id = input_rois_msg->rois.at(i).traffic_light_id; - output_msg.signals[num_valid_roi].traffic_light_type = + output_msg.signals[images.size()].traffic_light_type = input_rois_msg->rois.at(i).traffic_light_type; const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi; @@ -116,15 +120,29 @@ void TrafficLightClassifierNodelet::imageRoiCallback( backlight_indices.emplace_back(i); } images.emplace_back(roi_img); - num_valid_roi++; } - output_msg.signals.resize(num_valid_roi); + output_msg.signals.resize(images.size()); if (!classifier_ptr_->getTrafficSignals(images, output_msg)) { RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback"); return; } + // append the undetected rois as unknown + for (const auto & input_roi : input_rois_msg->rois) { + if (input_roi.roi.height == 0 && input_roi.traffic_light_type == classify_traffic_light_type_) { + tier4_perception_msgs::msg::TrafficSignal tlr_sig; + tlr_sig.traffic_light_id = input_roi.traffic_light_id; + tlr_sig.traffic_light_type = input_roi.traffic_light_type; + tier4_perception_msgs::msg::TrafficLightElement element; + element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN; + element.shape = tier4_perception_msgs::msg::TrafficLightElement::CIRCLE; + element.confidence = 0.0; + tlr_sig.elements.push_back(element); + output_msg.signals.push_back(tlr_sig); + } + } + for (const auto & idx : backlight_indices) { auto & elements = output_msg.signals.at(idx).elements; for (auto & element : elements) { diff --git a/perception/traffic_light_fine_detector/src/nodelet.cpp b/perception/traffic_light_fine_detector/src/nodelet.cpp index 257e9fc7ea99d..8037dc5472fbe 100644 --- a/perception/traffic_light_fine_detector/src/nodelet.cpp +++ b/perception/traffic_light_fine_detector/src/nodelet.cpp @@ -272,14 +272,28 @@ void TrafficLightFineDetectorNodelet::detectionMatch( } out_rois.rois.clear(); - for (const auto & p : bestDetections) { + std::vector invalid_roi_id; + for (const auto & [tlr_id, roi] : id2expectRoi) { + // if matches, update the roi info + if (!bestDetections.count(tlr_id)) { + invalid_roi_id.emplace_back(tlr_id); + continue; + } + TrafficLightRoi tlr; + tlr.traffic_light_id = tlr_id; + const auto & object = bestDetections.at(tlr_id); + tlr.traffic_light_type = roi.traffic_light_type; + tlr.roi.x_offset = object.x_offset; + tlr.roi.y_offset = object.y_offset; + tlr.roi.width = object.width; + tlr.roi.height = object.height; + out_rois.rois.push_back(tlr); + } + // append undetected rois at the end + for (const auto & id : invalid_roi_id) { TrafficLightRoi tlr; - tlr.traffic_light_id = p.first; - tlr.traffic_light_type = id2expectRoi[p.first].traffic_light_type; - tlr.roi.x_offset = p.second.x_offset; - tlr.roi.y_offset = p.second.y_offset; - tlr.roi.width = p.second.width; - tlr.roi.height = p.second.height; + tlr.traffic_light_id = id; + tlr.traffic_light_type = id2expectRoi[id].traffic_light_type; out_rois.rois.push_back(tlr); } } diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 97fa98ab18a72..6429640ff44c1 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -129,12 +129,18 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( const uint8_t traffic_light_type) { std::vector occlusion_ratios; + size_t not_detected_roi = 0; if (in_cloud_msg == nullptr || in_cam_info_msg == nullptr || in_roi_msg == nullptr) { occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { tier4_perception_msgs::msg::TrafficLightRoiArray selected_roi_msg; selected_roi_msg.rois.reserve(in_roi_msg->rois.size()); for (size_t i = 0; i < in_roi_msg->rois.size(); ++i) { + // not detected roi + if (in_roi_msg->rois[i].roi.height == 0) { + not_detected_roi++; + continue; + } if (in_roi_msg->rois.at(i).traffic_light_type == traffic_light_type) { selected_roi_msg.rois.push_back(in_roi_msg->rois.at(i)); } @@ -142,7 +148,7 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( tier4_perception_msgs::msg::TrafficSignalArray out_msg = *in_signal_msg; - if (selected_roi_msg.rois.size() != in_signal_msg->signals.size()) { + if (selected_roi_msg.rois.size() != in_signal_msg->signals.size() - not_detected_roi) { occlusion_ratios.resize(in_signal_msg->signals.size(), 0); } else { auto selected_roi_msg_ptr = @@ -162,6 +168,12 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback( traffic_light_utils::setSignalUnknown(out_msg_.signals.at(predicted_num + i), 0.0); } } + + // push back not detected rois + for (size_t i = occlusion_ratios.size(); i < in_signal_msg->signals.size(); ++i) { + out_msg_.signals.push_back(in_signal_msg->signals[i]); + } + subscribed_.at(traffic_light_type) = true; if (std::all_of(subscribed_.begin(), subscribed_.end(), [](bool v) { return v; })) { diff --git a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp index 7daa0849abe5e..6f96913420bad 100644 --- a/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp +++ b/perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp @@ -88,6 +88,10 @@ void CloudOcclusionPredictor::predict( image_geometry::PinholeCameraModel pinhole_model; pinhole_model.fromCameraInfo(*camera_info_msg); for (size_t i = 0; i < rois_msg->rois.size(); i++) { + // skip if no detection + if (rois_msg->rois[i].roi.height == 0) { + continue; + } calcRoiVector3D( rois_msg->rois[i], pinhole_model, traffic_light_position_map, tf_camera2map, roi_tls[i], roi_brs[i]); @@ -107,7 +111,7 @@ void CloudOcclusionPredictor::predict( lidar_rays_[static_cast(ray.azimuth)][static_cast(ray.elevation)].push_back(ray); } for (size_t i = 0; i < roi_tls.size(); i++) { - occlusion_ratios[i] = predict(roi_tls[i], roi_brs[i]); + occlusion_ratios[i] = rois_msg->rois[i].roi.height == 0 ? 0 : predict(roi_tls[i], roi_brs[i]); } }