TL Detector: Raise classifier confidence threshold and add RED->UNKNO…

…WN transition (#136)
team-fusionx · Jul 9, 2018 · d7520d3 · d7520d3
1 parent a5afd37
commit d7520d3
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 3 deletions.
diff --git a/ros/src/tl_detector/light_classification/tl_classifier.py b/ros/src/tl_detector/light_classification/tl_classifier.py
@@ -106,7 +106,7 @@ def get_classification(self, image):
                       yhat[y_class]*100, dt1.to_sec(), dt2.to_sec())
 
         self.current_light = TrafficLight.UNKNOWN
-        if (yhat[y_class] > 0.5):
+        if (yhat[y_class] > 0.7):
             if y_class == 0:
                 self.current_light = TrafficLight.GREEN
             elif y_class == 2:

diff --git a/ros/src/tl_detector/tl_detector.py b/ros/src/tl_detector/tl_detector.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 import rospy
 from std_msgs.msg import Int32
-from geometry_msgs.msg import PoseStamped, Pose
+from geometry_msgs.msg import PoseStamped, Pose, TwistStamped
 from styx_msgs.msg import TrafficLightArray, TrafficLight
 from styx_msgs.msg import Lane
 from sensor_msgs.msg import Image
@@ -62,6 +62,9 @@ def __init__(self):
         self.L_update = False
         self.light_change_to_red_or_yellow = False
 
+        self.current_velocity = None
+        self.stopped_vel_thresh = 0.3  # m/s
+
         rospy.logdebug('Red: %s', TrafficLight.RED)
         rospy.logdebug('Yellow: %s', TrafficLight.YELLOW)
         rospy.logdebug('Green: %s', TrafficLight.GREEN)
@@ -91,6 +94,10 @@ def __init__(self):
         sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb, queue_size=1)
         sub6 = rospy.Subscriber('/image_color', Image, self.image_cb, queue_size=1)
 
+        # Subscribe to /current_velocity for RED->UNKNOWN state validation
+        rospy.Subscriber('/current_velocity', TwistStamped,
+                         self.current_velocity_cb, queue_size=1)
+
         self.rate = rospy.Rate(UPDATE_RATE)
         rospy.logwarn('TL Detector init complete.')
         self.loop()
@@ -207,6 +214,9 @@ def image_cb(self, msg):
         self.has_image = True
         self.camera_image = msg
 
+    def current_velocity_cb(self, msg):
+        self.current_velocity = msg.twist.linear.x
+
     def get_closest_waypoint(self, x, y):
         """Identifies the closest path waypoint to the given position
             https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
@@ -263,7 +273,7 @@ def process_traffic_lights(self):
 
                 '''
                 Update next Traffic Light state only for Valid Light state transitions		
-                # State 0 - RED -> GREEN/RED
+                # State 0 - RED -> GREEN/RED or UNKNOWN if already stopped
                 # State 1 - YELLOW -> RED/YELLOW/UNKNOWN
                 # State 2 - GREEN -> YELLOW/GREEN/RED(upcoming signal was already RED)/UNKNOWN 
                 # State 4 - Unknown -> RED/YELLOW/GREEN
@@ -285,6 +295,9 @@ def process_traffic_lights(self):
                           and (classified_tl_state == TrafficLight.UNKNOWN)) 
                      or ((self.previous_light_state == TrafficLight.RED) 
                           and (classified_tl_state == TrafficLight.GREEN)) 
+                     or ((self.previous_light_state == TrafficLight.RED)
+                          and (classified_tl_state == TrafficLight.UNKNOWN)
+                          and (self.current_velocity < self.stopped_vel_thresh))
                    ):
                     ntl_state= classified_tl_state
                     self.previous_light_state = ntl_state