feat: add occlusion unknown classifier

Signed-off-by: badai-nguyen <[email protected]>

launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_detector.launch.xml

@@ -63,7 +63,7 @@
       <remap from="input/expect_rois" to="$(var input/expect_rois)"/>
       <remap from="output/traffic_light_rois" to="$(var output/traffic_light_rois)"/>
       <remap from="input/camera_info" to="$(var input/camera_info)"/>
-      <param name="max_iou_threshold" value="0.2"/>
+      <param name="max_iou_threshold" value="0.0"/>
       <param name="debug" value="true"/>
     </node>
   </group>

launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py

@@ -130,6 +130,7 @@ def create_parameter_dict(*args):
                 remappings=[
                     ("input/car_signals", "classified/car/traffic_signals"),
                     ("input/pedestrian_signals", "classified/pedestrian/traffic_signals"),
+                    ("input/expect_rois", f"/perception/traffic_light_recognition/{namespace}/detection/expect/rois"),
                     ("output/traffic_light_signals", "traffic_signals"),
                 ],
                 extra_arguments=[

perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.cpp

@@ -14,6 +14,7 @@
 
 #include "traffic_light_signals_merger_node.hpp"
 
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
@@ -28,26 +29,53 @@ TrafficLightSignalsMergerNode::TrafficLightSignalsMergerNode(
   tf_listener_(tf_buffer_),
   car_signal_sub_(this, "input/car_signals", rclcpp::QoS{1}.get_rmw_qos_profile()),
   pedestrian_signal_sub_(this, "input/pedestrian_signals", rclcpp::QoS{1}.get_rmw_qos_profile()),
-  sync_(SyncPolicy(10), car_signal_sub_, pedestrian_signal_sub_)
+  expected_rois_sub_(this, "input/expect_rois", rclcpp::QoS{1}.get_rmw_qos_profile()),
+  sync_(SyncPolicy(10), car_signal_sub_, pedestrian_signal_sub_, expected_rois_sub_)
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
-  sync_.registerCallback(std::bind(&TrafficLightSignalsMergerNode::signalsCallback, this, _1, _2));
+  using std::placeholders::_3;
+  sync_.registerCallback(
+    std::bind(&TrafficLightSignalsMergerNode::signalsCallback, this, _1, _2, _3));
   pub_traffic_light_signals_ =
     create_publisher<TrafficLightArray>("output/traffic_light_signals", rclcpp::QoS{1});
 }
 
 void TrafficLightSignalsMergerNode::signalsCallback(
   const TrafficLightArray::ConstSharedPtr & car_signals_msg,
-  const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg)
+  const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg,
+  const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg)
 {
+  std::map<int, TrafficLightRoi> expected_rois_map;
+  for (const auto & roi : expected_rois_msg->rois) {
+    expected_rois_map[roi.traffic_light_id].traffic_light_id = roi.traffic_light_id;
+    expected_rois_map[roi.traffic_light_id].roi = roi.roi;
+    expected_rois_map[roi.traffic_light_id].traffic_light_type = roi.traffic_light_type;
+  }
+
   TrafficLightArray output;
   output.header = car_signals_msg->header;
   output.signals.insert(
     output.signals.end(), car_signals_msg->signals.begin(), car_signals_msg->signals.end());
   output.signals.insert(
     output.signals.end(), pedestrian_signals_msg->signals.begin(),
     pedestrian_signals_msg->signals.end());
+  for (auto & signal : output.signals) {
+    // remove expected_rois which are already in signals
+    if (expected_rois_map.find(signal.traffic_light_id) != expected_rois_map.end()) {
+      expected_rois_map.erase(signal.traffic_light_id);
+    }
+  }
+  for (const auto & roi : expected_rois_map) {
+    TrafficLight signal;
+    signal.traffic_light_id = roi.first;
+    signal.traffic_light_type = roi.second.traffic_light_type;
+    signal.elements.resize(1);
+    signal.elements[0].shape = TrafficLightElement::UNKNOWN;
+    signal.elements[0].color = TrafficLightElement::UNKNOWN;
+    signal.elements[0].confidence = 1.0;
+    output.signals.push_back(signal);
+  }
   pub_traffic_light_signals_->publish(output);
 }
 

perception/autoware_traffic_light_signals_merger/src/traffic_light_signals_merger_node.hpp

@@ -21,7 +21,11 @@
 #include "message_filters/synchronizer.h"
 #include "rclcpp/rclcpp.hpp"
 
+#include "tier4_perception_msgs/msg/traffic_light.hpp"
 #include "tier4_perception_msgs/msg/traffic_light_array.hpp"
+#include "tier4_perception_msgs/msg/traffic_light_element.hpp"
+#include "tier4_perception_msgs/msg/traffic_light_roi.hpp"
+#include "tier4_perception_msgs/msg/traffic_light_roi_array.hpp"
 
 #include <chrono>
 #include <memory>
@@ -30,7 +34,11 @@
 
 namespace autoware::traffic_light
 {
+using tier4_perception_msgs::msg::TrafficLight;
 using tier4_perception_msgs::msg::TrafficLightArray;
+using tier4_perception_msgs::msg::TrafficLightElement;
+using tier4_perception_msgs::msg::TrafficLightRoi;
+using tier4_perception_msgs::msg::TrafficLightRoiArray;
 
 class TrafficLightSignalsMergerNode : public rclcpp::Node
 {
@@ -44,14 +52,17 @@ class TrafficLightSignalsMergerNode : public rclcpp::Node
 
   message_filters::Subscriber<TrafficLightArray> car_signal_sub_;
   message_filters::Subscriber<TrafficLightArray> pedestrian_signal_sub_;
+  message_filters::Subscriber<TrafficLightRoiArray> expected_rois_sub_;
 
-  typedef message_filters::sync_policies::ApproximateTime<TrafficLightArray, TrafficLightArray>
+  typedef message_filters::sync_policies::ApproximateTime<
+    TrafficLightArray, TrafficLightArray, TrafficLightRoiArray>
     SyncPolicy;
   message_filters::Synchronizer<SyncPolicy> sync_;
 
   void signalsCallback(
     const TrafficLightArray::ConstSharedPtr & car_signals_msg,
-    const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg);
+    const TrafficLightArray::ConstSharedPtr & pedestrian_signals_msg,
+    const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg);
 
   rclcpp::Publisher<TrafficLightArray>::SharedPtr pub_traffic_light_signals_;
 };