class TLDetector(object): def __init__(self): rospy.init_node('tl_detector') sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb) #sub6 = rospy.Subscriber('/image_raw', Image, self.image_cb) sub6 = rospy.Subscriber('/image_color', Image, self.image_cb) self.pose = None self.waypoints = None self.camera_image = None self.lights = [] config_string = rospy.get_param("/traffic_light_config") self.config = yaml.load(config_string) self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1) self.light_positions = self.config['stop_line_positions'] self.light_classifier = TLClassifier() self.camera_image = None self.state = TrafficLight.UNKNOWN self.last_state = TrafficLight.UNKNOWN self.last_wp = -1 self.bridge = CvBridge() self.state_count = 0 self.image_count = 0 self.image_period = 6 # processes every sixth image for latency. rospy.spin() def pose_cb(self, msg): self.pose = msg def waypoints_cb(self, waypoints): self.waypoints = waypoints def traffic_cb(self, msg): self.lights = msg.lights def image_cb(self, msg): """Identifies red lights in the incoming camera image and publishes the index of the waypoint closest to the red light's stop line to /traffic_waypoint Args: msg (Image): image from car-mounted camera """ self.has_image = True self.camera_image = msg self.image_count = self.image_count + 1 # Process images only once in image period to counter latency if self.image_count % self.image_period == 0: light_wp, state = self.process_traffic_lights() if self.state != state: self.state_count = 0 self.state = state elif self.state_count >= STATE_COUNT_THRESHOLD: self.last_state = self.state light_wp = light_wp if state == TrafficLight.RED \ or state == TrafficLight.YELLOW else -1 self.last_wp = light_wp self.upcoming_red_light_pub.publish(Int32(light_wp)) else: self.upcoming_red_light_pub.publish(Int32(self.last_wp)) self.state_count = self.state_count + 1 def get_closest_waypoint(self, pose): """Identifies the closest path waypoint to the given position https://en.wikipedia.org/wiki/Closest_pair_of_points_problem Args: pose (Pose): position to match a waypoint to Returns: int: index of the closest waypoint in self.waypoints """ waypoint = 0 waypoints_list = self.waypoints.waypoints dist = float('inf') for i in range(len(waypoints_list)): new_dist = self.calc_distance_points_3D( pose.position, waypoints_list[i].pose.pose.position) if dist > new_dist: dist = new_dist waypoint = i return waypoint def get_light_state(self): """ Get current color of the traffic light """ if self.camera_image is None: return False else: self.camera_image.encoding = "rgb8" cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8") #Perform classification here state = self.light_classifier.classify(cv_image) # Use last state if classifier is not sure if state == TrafficLight.UNKNOWN and self.last_state: state = self.last_state return state def process_traffic_lights(self): """Finds closest visible traffic light, if one exists, and determines its location and color Returns: int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists) int: ID of traffic light color (specified in styx_msgs/TrafficLight) """ light = None if hasattr(self, 'waypoints') and hasattr(self, 'pose'): min_dist = float('inf') car_wp = self.get_closest_waypoint(self.pose.pose) light_positions = self.light_positions i = 0 for light_pos in light_positions: light_wp, tl_candid, tl_dist = self.calclulate_distance_to_traffic_light( car_wp, light_pos) if (tl_dist < min_dist) \ and (car_wp % len(self.waypoints.waypoints)) < (light_wp % len(self.waypoints.waypoints)) \ and (tl_dist < OBSERV_DIST) : closest_light_wp = light_wp min_dist = tl_dist light = tl_candid i += 1 state = TrafficLight.UNKNOWN light_wp = -1 if light: state = self.get_light_state() light_wp = closest_light_wp else: light_wp = -1 state = TrafficLight.RED return light_wp, state def calclulate_distance_to_traffic_light(self, car_waypoint, light_position): tl_candid = self.create_tl(0.0, TrafficLight.UNKNOWN, light_position[0], light_position[1], 0.0) light_waypoint = self.get_closest_waypoint(tl_candid.pose.pose) tl_dist = self.calc_distance_coords_2D( self.waypoints.waypoints[car_waypoint].pose.pose.position.x, self.waypoints.waypoints[car_waypoint].pose.pose.position.y, self.waypoints.waypoints[light_waypoint].pose.pose.position.x, self.waypoints.waypoints[light_waypoint].pose.pose.position.y) return light_waypoint, tl_candid, tl_dist def calc_distance_coords_2D(self, x1, y1, x2, y2): return math.sqrt((x2 - x1)**2 + (y2 - y1)**2) def calc_distance_points_3D(self, a, b): return math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 + (a.z - b.z)**2) def create_tl(self, yaw, state, x, y, z): traffic_light = TrafficLight() traffic_light.header = Header() traffic_light.pose.header = Header() traffic_light.pose = PoseStamped() traffic_light.state = state traffic_light.pose.pose.position.x = x traffic_light.pose.pose.position.y = y traffic_light.pose.pose.position.z = z traffic_light.pose.header.stamp = rospy.Time.now() traffic_light.pose.header.frame_id = 'world' traffic_light.header.stamp = rospy.Time.now() traffic_light.header.frame_id = 'world' q = tf.transformations.quaternion_from_euler(0.0, 0.0, math.pi * yaw / 180.0) traffic_light.pose.pose.orientation = Quaternion(*q) return traffic_light
class TLDetector(object): def __init__(self): rospy.init_node('tl_detector') self.pose = None self.waypoints = None self.camera_image = None self.lights = [] sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb) sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb) ''' /vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and helps you acquire an accurate ground truth data source for the traffic light classifier by sending the current color state of all traffic lights in the simulator. When testing on the vehicle, the color state will not be available. You'll need to rely on the position of the light and the camera image to predict it. ''' sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb) sub6 = rospy.Subscriber('/image_color', Image, self.image_cb) config_string = rospy.get_param("/traffic_light_config") self.config = yaml.load(config_string) self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1) self.light_positions = self.config['stop_line_positions'] self.light_classifier = TLClassifier() self.camera_image = None self.state = TrafficLight.UNKNOWN self.last_state = TrafficLight.UNKNOWN self.last_wp = -1 self.state_count = 0 self.bridge = CvBridge() rospy.spin() def pose_cb(self, msg): self.pose = msg def waypoints_cb(self, waypoints): self.waypoints = waypoints def traffic_cb(self, msg): self.lights = msg.lights def image_cb(self, msg): """Identifies red lights in the incoming camera image and publishes the index of the waypoint closest to the red light's stop line to /traffic_waypoint Args: msg (Image): image from car-mounted camera """ self.has_image = True self.camera_image = msg light_wp, state = self.process_traffic_lights() ''' Publish upcoming red lights at camera frequency. Each predicted state has to occur `STATE_COUNT_THRESHOLD` number of times till we start using it. Otherwise the previous stable state is used. ''' if self.state != state: self.state_count = 0 self.state = state elif self.state_count >= STATE_COUNT_THRESHOLD: self.last_state = self.state light_wp = light_wp if state == TrafficLight.RED or state == TrafficLight.YELLOW else -1 self.last_wp = light_wp self.upcoming_red_light_pub.publish(Int32(light_wp)) else: self.upcoming_red_light_pub.publish(Int32(self.last_wp)) self.state_count += 1 def get_closest_waypoint(self, pose): """Identifies the closest path waypoint to the given position https://en.wikipedia.org/wiki/Closest_pair_of_points_problem Args: pose (Pose): position to match a waypoint to Returns: int: index of the closest waypoint in self.waypoints """ #TODO implement waypoint = 0 waypoints_list = self.waypoints.waypoints dist = float('inf') for i in range(len(waypoints_list)): new_dist = self.calc_distance_points_3D( pose.position, waypoints_list[i].pose.pose.position) if dist > new_dist: dist = new_dist waypoint = i return waypoint def get_light_state(self): """Determines the current color of the traffic light Args: light (TrafficLight): light to classify Returns: int: ID of traffic light color (specified in styx_msgs/TrafficLight) """ if self.camera_image is None: return False else: self.camera_image.encoding = "rgb8" cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8") #!!!perform classification here!!! """classificate""" state = self.light_classifier.classify(cv_image) # when classifier is not sure then use the last state if state == TrafficLight.UNKNOWN and self.last_state: state = self.last_state return state #Get classification def process_traffic_lights(self): """Finds closest visible traffic light, if one exists, and determines its location and color Returns: int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists) int: ID of traffic light color (specified in styx_msgs/TrafficLight) """ light = None if hasattr(self, 'waypoints') and hasattr(self, 'pose'): min_dist = float('inf') car_wp = self.get_closest_waypoint(self.pose.pose) light_positions = self.light_positions i = 0 for light_pos in light_positions: light_wp, tl_candid, tl_dist = self.calculate_distance_to_traffic_light( car_wp, light_pos) if (tl_dist < min_dist) and ( (car_wp % len(self.waypoints.waypoints)) < (light_wp % len(self.waypoints.waypoints))) and ( tl_dist < OBSERV_DIST): closest_light_wp = light_wp min_dist = tl_dist light = tl_candid i += 1 state = TrafficLight.UNKNOWN light_wp = -1 if light: state = self.get_light_state() light_wp = closest_light_wp else: light_wp = -1 state = TrafficLight.RED return light_wp, state """ # List of positions that correspond to the line to stop in front of for a given intersection stop_line_positions = self.config['stop_line_positions'] if(self.pose): car_position = self.get_closest_waypoint(self.pose.pose) #TODO find the closest visible traffic light (if one exists) diff=len(self.waypoints.waypoints) for i, light in enumerate(self.lights): #get stop line waypoint index line = stop_line_positions[i] temp_wp_idx = self.get_closest_waypoint(line[0],line[1]) #find closest stop line waypoint index d = temp_wp_idx- car_position_idx if 0 <= d < diff: diff = d closest_light = light line_wp_idx = temp_wp_idx if closest_light and line_wp_idx: state = self.get_light_state(closest_light) rospy.logdebug('closest light idx: {} \t state: {}'.format(line_wp_idx,state)) return light_wp_idx, state rospy.logwaren('No traffic light found') return -1, TrafficLight.UNKNOWN """ def calculate_distance_to_traffic_light(self, car_waypoint, light_position): tl_candid = self.create_tl(0.0, TrafficLight.UNKNOWN, light_position[0], light_position[1], 0.0) light_waypoint = self.get_closest_waypoint(tl_candid.pose.pose) tl_dist = self.calc_distance_coords_2D( self.waypoints.waypoints[car_waypoint].pose.pose.position.x, self.waypoints.waypoints[car_waypoint].pose.pose.position.y, self.waypoints.waypoints[light_waypoint].pose.pose.position.x, self.waypoints.waypoints[light_waypoint].pose.pose.position.y) return light_waypoint, tl_candid, tl_dist def calc_distance_coords_2D(self, x1, y1, x2, y2): return math.sqrt((x2 - x1)**2 + (y2 - y1)**2) def calc_distance_points_3D(self, a, b): return math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 + (a.z - b.z)**2) def create_tl(self, yaw, state, x, y, z): traffic_light = TrafficLight() traffic_light.header = Header() traffic_light.pose.header = Header() traffic_light.pose = PoseStamped() traffic_light.state = state traffic_light.pose.pose.position.x = x traffic_light.pose.pose.position.y = y traffic_light.pose.pose.position.z = z traffic_light.pose.header.stamp = rospy.Time.now() traffic_light.pose.header.frame_id = 'world' traffic_light.header.stamp = rospy.Time.now() traffic_light.header.frame_id = 'world' q = tf.transformations.quaternion_from_euler(0.0, 0.0, math.pi * yaw / 180.0) traffic_light.pose.pose.orientation = Quaternion(*q) return traffic_light