Skip to content

Commit

Permalink
feat(traffic_light): output undetected traffic light as unknown (#5934)
Browse files Browse the repository at this point in the history 
* fix: output undetected traffic light as unknown

Signed-off-by: tzhong518 <sworgun@gmail.com>

* 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 <sworgun@gmail.com>

* style(pre-commit): autofix

* fix: improve readability

Signed-off-by: tzhong518 <sworgun@gmail.com>

* fix: index error

Signed-off-by: tzhong518 <sworgun@gmail.com>

* fix: append the undetected rois at the end so that the order can be easily kept

Signed-off-by: tzhong518 <sworgun@gmail.com>

* fix: false occlusion ratio for undetected signals

Signed-off-by: tzhong518 <sworgun@gmail.com>

---------

Signed-off-by: tzhong518 <sworgun@gmail.com>
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>
  • Loading branch information
@tzhong518 @ktro2828 @pre-commit-ci
3 people authored Jan 9, 2024
1 parent 4a419f8 commit 412b0e7
Show file tree
Hide file tree
Showing 4 changed files with 63 additions and 15 deletions.
30 changes: 24 additions & 6 deletions perception/traffic_light_classifier/src/nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
// limitations under the License.
#include "traffic_light_classifier/nodelet.hpp"

#include <tier4_perception_msgs/msg/traffic_light_element.hpp>

#include <iostream>
#include <memory>
#include <utility>
Expand Down Expand Up @@ -98,16 +100,18 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
output_msg.signals.resize(input_rois_msg->rois.size());

std::vector<cv::Mat> images;
size_t num_valid_roi = 0;
std::vector<size_t> 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;

Expand All @@ -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) {
Expand Down
28 changes: 21 additions & 7 deletions perception/traffic_light_fine_detector/src/nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,14 +272,28 @@ void TrafficLightFineDetectorNodelet::detectionMatch(
}

out_rois.rois.clear();
for (const auto & p : bestDetections) {
std::vector<size_t> 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);
}
}
Expand Down
14 changes: 13 additions & 1 deletion perception/traffic_light_occlusion_predictor/src/nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,20 +129,26 @@ void TrafficLightOcclusionPredictorNodelet::syncCallback(
const uint8_t traffic_light_type)
{
std::vector<int> 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));
}
}

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 =
Expand All @@ -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; })) {
Expand Down
6 changes: 5 additions & 1 deletion perception/traffic_light_occlusion_predictor/src/occlusion_predictor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]);
Expand All @@ -107,7 +111,7 @@ void CloudOcclusionPredictor::predict(
lidar_rays_[static_cast<int>(ray.azimuth)][static_cast<int>(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]);
}
}

Expand Down

0 comments on commit 412b0e7

Please sign in to comment.