From d7520d354b982cd90bcca3572b9d9d5342baae4b Mon Sep 17 00:00:00 2001 From: edufford Date: Mon, 9 Jul 2018 13:05:46 -1000 Subject: [PATCH] TL Detector: Raise classifier confidence threshold and add RED->UNKNOWN transition (#136) --- .../light_classification/tl_classifier.py | 2 +- ros/src/tl_detector/tl_detector.py | 17 +++++++++++++++-- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/ros/src/tl_detector/light_classification/tl_classifier.py b/ros/src/tl_detector/light_classification/tl_classifier.py index 0a96baf543..a7a69142fc 100755 --- 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 index 3342809635..8b5295c871 100755 --- 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