def __init__(self):
rospy.init_node('tl_detector')
self.current_pos = None
self.waypoints = None
self.waypoints_tree = None
self.camera_image = None
self.lights = []
self.wpi_stop_pos_dict = dict()
self.distance_to_stop = 0
self.publishing_rate = 10.0 #50.0
self.new_image = False
self.tl_sight_dist = 50. # meters before sending images to classifier.
self.is_simulator = True if rospy.get_param("~simulator") == 1 else False
rospy.loginfo("SIMULATOR:{0}".format(self.is_simulator))
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
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.
'''
# subscribers and publishers.
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
rospy.Subscriber('/image_color', Image, self.image_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
# Stop light config
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.bridge = CvBridge()
model_path = 'light_classification/frozen_inference_graph.pb'
if not self.is_simulator:
model_path = 'light_classification/real_frozen_graph.pb'
self.light_classifier = None
if USE_CLASSIFIER:
self.light_classifier = TLClassifier(model_path)
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_tl_wp = -1
self.state_count = 0
self.dbw_enabled = False
self.loop()
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.KDTree = None
self.camera_image = None
self.lights = []
self.stop_lines = None
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb, queue_size=1)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb, queue_size=1)
'''
/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.
'''
#[alexm]NOTE: we should rely on this topic's data except state of the light
#[alexm]NOTE: according to this: https://carnd.slack.com/messages/C6NVDVAQ3/convo/C6NVDVAQ3-1504625321.000063/
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb, queue_size=1)
rospy.Subscriber('/image_color', Image, self.image_cb, queue_size=1)
rospy.Subscriber('/next_wp', Int32, self.next_wp_cb, queue_size=1)
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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.light_classifier.init()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.next_wp = None
rospy.spin()
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.light_classifier = None
self.waypoints_2d = None
self.waypoints_tree = None
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
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.
'''
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb)
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.bridge = CvBridge()
dirname = os.path.dirname(__file__)
model_name = self.config['model_name']
model_path = os.path.join(dirname, "light_classification/models/{}".format(model_name))
label_path = os.path.join(dirname, 'light_classification/models/label_map.pbtxt')
self.light_classifier = TLClassifier(model_path, label_path)
self.listener = tf.TransformListener()
self.loop()
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.base_waypoints = None
self.camera_image = None
self.lights = []
self.has_image = False
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
model = load_model("./light_classification/model.h5")
self.light_classifier = TLClassifier(model)
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)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
self.bridge = CvBridge()
self.listener = tf.TransformListener()
rospy.spin()
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
# Parameters
self.use_classifier = rospy.get_param('~use_classifier', True)
camera_topic = rospy.get_param('~camera_topic', '/image_raw')
sub_curr_pose = rospy.Subscriber('/current_pose', PoseStamped,
self.pose_cb)
sub_base_waypts = 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.
'''
sub_traffic_lights = rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray,
self.traffic_cb)
sub_image_color = rospy.Subscriber(camera_topic,
Image,
self.image_cb,
queue_size=1)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
# List of positions that correspond to the line to stop in front of for a given intersection
self.stopline_positions = self.config['stop_line_positions']
self.stopline_wp_indices = []
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = None
self.state_count = 0
self.has_image = False
# Not using rospy.spin() in order to be able to tune ressources usage
self.loop()
def loop(self):
rate = rospy.Rate(5) #Hz
while not rospy.is_shutdown():
if self.pose is not None and self.waypoints is not None and self.has_image:
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at a fixed 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 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
rate.sleep()
def pose_cb(self, msg):
self.pose = msg.pose
def waypoints_cb(self, waypoints):
self.waypoints = waypoints.waypoints
self.map_stopline_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
def calc_distance(self, x1, y1, x2, y2):
"""Calculates the distance between two points
Args:
x1 (float): First point's x-coordinate
x2 (float): First point's y-coordinate
y1 (float): Second point's x-coordinate
y2 (float): Second point's y-coordinate
Returns:
float: Distance between the two provided points
"""
return math.sqrt((x1 - x2)**2 + (y1 - y2)**2)
def get_closest_waypoint_idx(self, x, y, waypoints_set=[]):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
x (float): point x-coordinate to match a waypoint to
y (float): point y-coordinate to match a waypoint to
waypoints_set (:obj:`list`, optional): List of waypoints
Returns:
int: index of the closest waypoint in self.waypoints
"""
shortest_distance = HIGHEST_NUM
closest_wp = -1
if len(waypoints_set) == 0 and self.waypoints is not None:
waypoints_set = self.waypoints
for idx, wp in enumerate(waypoints_set):
distance = self.calc_distance(x, y, wp.pose.pose.position.x,
wp.pose.pose.position.y)
if distance < shortest_distance:
shortest_distance = distance
closest_wp = idx
return closest_wp
def map_stopline_waypoints(self):
"""Generates a map of waypoints to stop lines
Returns:
None: Attaches the map to selt.stopline_wp_indices
"""
waypoint = 0
start = 0
waypoints = copy(self.waypoints)
for (sl_x, sl_y) in self.stopline_positions:
waypoints = waypoints[start:]
start = self.get_closest_waypoint_idx(sl_x, sl_y, waypoints)
waypoint += start
self.stopline_wp_indices.append(waypoint)
def is_waypoint_ahead(self, wp_x, wp_y):
"""Determines whether a waypoint is ahead of the car
Args:
wp_x (float): waypoint's global x-coordinate
wp_y (float): waypoint's global y-coordinate
Returns:
bool: Whether the waypoint is ahead of the car
"""
car_x = self.pose.position.x
car_y = self.pose.position.y
car_orientation = self.pose.orientation
euler = tf.transformations.euler_from_quaternion([
car_orientation.x, car_orientation.y, car_orientation.z,
car_orientation.w
])
car_yaw = euler[2]
return ((wp_x - car_x) * math.cos(car_yaw) +
(wp_y - car_y) * math.sin(car_yaw)) > 0
def get_closest_stopline_idx(self):
"""Determine the index of the closest traffic light stopline
Returns:
int: Index of closest traffic light stopline
"""
closest_stopline_idx = None
shortest_dist = HIGHEST_NUM
for idx, (sl_x, sl_y) in enumerate(self.stopline_positions):
if self.is_waypoint_ahead(sl_x, sl_y):
car_x, car_y = self.pose.position.x, self.pose.position.y
distance = self.calc_distance(car_x, car_y, sl_x, sl_y)
if distance < shortest_dist:
shortest_dist = distance
closest_stopline_idx = idx
return closest_stopline_idx
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
crop = cv_image[0:400, 0:800].copy()
if self.use_classifier is True:
#Get TL classification
return self.light_classifier.get_classification(crop)
else:
return light.state
def get_stopline_wp_idx(self, sl_x, sl_y):
"""Determines a stop line's waypoint index value
Args:
sl_x (float): a stop line's x-coordinate value
sl_y (float): a stop line's y-coordinate value
Returns:
int: A stop lines' waypoint index value
"""
stopline_wp_idx = None
shortest_distance = HIGHEST_NUM
for idx in self.stopline_wp_indices:
wp = self.waypoints[idx]
wp_x = wp.pose.pose.position.x
wp_y = wp.pose.pose.position.y
distance = self.calc_distance(sl_x, sl_y, wp_x, wp_y)
if distance < shortest_distance and self.is_waypoint_ahead(
wp_x, wp_y):
stopline_wp_idx = idx
shortest_distance = distance
return stopline_wp_idx
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
stopline_wp_idx = None
if self.pose is not None:
stopline_idx = self.get_closest_stopline_idx()
if stopline_idx is not None:
sl_x, sl_y = self.stopline_positions[stopline_idx]
stopline_wp_idx = self.get_stopline_wp_idx(sl_x, sl_y)
light = self.lights[stopline_idx]
if light is not None:
state = self.get_light_state(light)
return stopline_wp_idx, state
self.waypoints = None
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.tl_wps = []
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")
rospy.logwarn("The parameter string is : %s", config_string )
self.config = yaml.load(config_string)
self.upcoming_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
# debug: publisher of cross-correlation results
self.ccresult_pub = rospy.Publisher('/traffic_ccresult', Image, queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.count = 0
self.camera_car_position = []
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
if state == TrafficLight.UNKNOWN:
self.upcoming_light_pub.publish(Int32(-1))
return
self.last_wp = light_wp
self.upcoming_light_pub.publish(Int32( light_wp | (self.state << 16) ))
else:
self.upcoming_light_pub.publish(Int32(self.last_wp | (self.last_state << 16) ))
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
closest_index = -1
closest_dis = -1
if self.waypoints is not None:
wps = self.waypoints.waypoints
for i in range(len(wps)):
dis = (wps[i].pose.pose.position.x - pose.x) ** 2 + \
(wps[i].pose.pose.position.y - pose.y) ** 2
if (closest_dis == -1) or (closest_dis > dis):
closest_dis = dis
closest_index = i
return closest_index
# George: Here's my version of project_to_image_plane
# I am avoiding the TransformListener object as I am not sure about how
# to configure it without having doubts. The transform is easy to
# work-out directly from the pose vector and gives me control over the
# coordinate frame.
def project_to_image_plane(self, point_in_world):
"""Project point from 3D world coordinates to 2D camera image location
Args:
point_in_world (Point): 3D location of a point in the world
Returns:
x (int): x coordinate of target point in image
y (int): y coordinate of target point in image
"""
# Retreving camera intronsics
fx = self.config['camera_info']['focal_length_x']
fy = self.config['camera_info']['focal_length_y']
# image size
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
#TODO Use tranform and rotation to calculate 2D position of light in image
x = 0
y = 0
return (x,y)
def get_light_state(self, light):
"""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(not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
x, y = self.project_to_image_plane(light.pose.pose.position)
#get traffic light classification
return self.light_classifier.traffic_predict(cv_image)
def track_index_diff(self,index1, index2):
if (self.waypoints.waypoints is None):
return -1
N = len(self.waypoints.waypoints)
if index2 >= index1 :
return index2 - index1
else:
return N - index1 - 1 + index2
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)
"""
if (self.waypoints is None):
return (-1, TrafficLight.UNKNOWN)
# 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']
# now, for all stop_lines find nearest points (shoudl be done ONCE
if (len(self.tl_wps)==0):
for i, stop_line in enumerate(stop_line_positions):
tl_wp = self.get_closest_waypoint(Point(stop_line))
self.tl_wps.append( (tl_wp+5) % len(self.waypoints.waypoints) ) # +1 to give extra margin towards the traffic light
light = None
if(self.pose):
car_wp = self.get_closest_waypoint(self.pose.pose.position)
# Now find the smallest distance to a traffic light wp from the car wp:
minDist = self.track_index_diff(car_wp, self.tl_wps[0])
light_index = 0
for i in range(1, len(self.tl_wps)):
dist = self.track_index_diff(car_wp, self.tl_wps[i])
if dist > 150 / 0.63: #about 150 meters ON-TRACK
continue
if (dist < minDist):
minDist = dist
light_index = i
light = self.lights[light_index]
#print("Nearest Traffic light index : ", light_index)
if light:
# print(light_wp, self.count)
state = self.get_light_state(light)
return self.tl_wps[light_index], state
return -1, TrafficLight.UNKNOWN
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoints_2d = None
self.waypoint_tree = 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.stop_line_positions = self.config['stop_line_positions']
self.isSite = self.config['is_site']
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier(self.isSite)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.traffic_lights_2d = None
self.traffic_light_tree = None
self.light_dict = {0: 'Red', 1: 'Yellow', 2: 'Green', 4: 'Unknown'}
self.mapped_category = {
1: {
'id': 2,
'name': 'Green'
},
2: {
'id': 0,
'name': 'Red'
},
3: {
'id': 1,
'name': 'Yellow'
},
4: {
'id': 3,
'name': 'Unknown'
}
}
#rospy.spin()
self.loop()
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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
# LocateWaypointsAround service
self.locate_waypoints_around = rospy.ServiceProxy(
'/waypoint_locator/locate_waypoints_around', LocateWaypointsAround)
rospy.wait_for_service('/waypoint_locator/locate_waypoints_around')
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 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
"""
req = LocateWaypointsAroundRequest(pose)
resp = self.locate_waypoints_around(req)
return resp.nearest
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
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
# 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)
if light:
light_wp = self.look_for_traffic_light_ahead(
car_position, stop_line_positions)
if wp_idx > -1:
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
def look_for_traffic_light_ahead(self, wp_car_pose_idx,
stop_line_positions):
# fine to use brute-force searching since the number of
# traffic lights are quite small.
# looking for the closest light position
min_dist = 1e7
min_idx = -1
for idx, light in enumerate(self.lights):
light_pos = light.pose.position
dist = self.dist2d(pose.position, light_pos)
if dist < min_dist:
min_dist = dist
min_idx = idx
# the closest one doesn't mean the one ahead
# get the stop line position for the light and check
# if the vehicle has passed the nearest waypoint
# of the stop line. if yes, then we should head for the
# next traffic light.
#
# but in fact we don't have to seek the light ahead in this
# case, because the light detector will only detect the
# nearest light around the vehicle's current position, the
# next traffic light is still out of sight at this moment.
x, y = stop_line_positions[min_idx]
stop_line_pos = Pose(x, y, 0)
wp_stop_line_idx = self.get_closest_waypoint(stop_line_pos)
if wp_car_pose_idx > wp_stop_line_idx:
return -1
else:
return wp_stop_line_idx
def dist2d(self, a, b):
return math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2)
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoints_2d = None
self.waypoint_tree = None
self.camera_image = None
self.lights = []
self.has_image = False
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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.image_counter = 0
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
# setup k-d tree in order to find the closest path waypoint to the given position [get_closest_waypoint(position)]
# the code is copied from waypoints_cb(pose) in ros/src/waypoint_updater/waypoint_updater.py written by https://github.com/ysonggit
if not self.waypoints_2d:
self.waypoints_2d = [[waypoint.pose.pose.position.x, waypoint.pose.pose.position.y] for waypoint in waypoints.waypoints]
self.waypoint_tree = cKDTree(self.waypoints_2d)
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
"""
if self.image_counter < IMAGE_CB_THRESHOLD:
self.image_counter += 1
return
else:
self.image_counter = 0
self.has_image = True
self.camera_image = msg
light_wp, state = self.process_traffic_lights()
rospy.loginfo("Closest light waypoint= {0}, light state= {1}".format(light_wp, state))
'''
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 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, position):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
position [x, y]: position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
# cKDTree.query() returns (distance, index), but only index is needed
return self.waypoint_tree.query(position)[1]
def get_light_state(self, light):
"""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 not self.has_image:
# self.prev_light_loc = None
# return False
return TrafficLight.UNKNOWN
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
# Get classification
return self.light_classifier.get_classification(cv_image)
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)
"""
closest_light = None
closest_light_waypoint_idx = None
# 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_waypoint_idx = self.get_closest_waypoint([self.pose.pose.position.x, self.pose.pose.position.y])
# find the closest visible traffic light (if one exists)
min_stop_line_dist = len(self.waypoints.waypoints)
for idx, light in enumerate(self.lights):
current_stop_line_position = stop_line_positions[idx]
current_stop_line_waypoint_idx = self.get_closest_waypoint([current_stop_line_position[0], current_stop_line_position[1]])
# number of waypoints between selected stop_line and the car
current_stop_line_dist = current_stop_line_waypoint_idx - car_waypoint_idx
# check if stop line is in front of the car (current_stop_line_dist > 0) and is closest
if 0.0 <= current_stop_line_dist < min_stop_line_dist:
min_stop_line_dist = current_stop_line_dist
closest_light = light
closest_light_waypoint_idx = current_stop_line_waypoint_idx
if closest_light and closest_light_waypoint_idx:
state = self.get_light_state(closest_light)
return closest_light_waypoint_idx, state
self.waypoints = None # TODO check why self.waypoints are reset
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.current_pos = None
self.waypoints = None
self.waypoints_tree = None
self.camera_image = None
self.lights = []
self.wpi_stop_pos_dict = dict()
self.distance_to_stop = 0
self.publishing_rate = 10.0 #50.0
self.new_image = False
self.tl_sight_dist = 50. # meters before sending images to classifier.
self.is_simulator = True if rospy.get_param("~simulator") == 1 else False
rospy.loginfo("SIMULATOR:{0}".format(self.is_simulator))
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
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.
'''
# subscribers and publishers.
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
rospy.Subscriber('/image_color', Image, self.image_cb)
rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
# Stop light config
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.bridge = CvBridge()
model_path = 'light_classification/frozen_inference_graph.pb'
if not self.is_simulator:
model_path = 'light_classification/real_frozen_graph.pb'
self.light_classifier = None
if USE_CLASSIFIER:
self.light_classifier = TLClassifier(model_path)
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_tl_wp = -1
self.state_count = 0
self.dbw_enabled = False
self.loop()
def loop(self):
'''
Loop at sampling rate to publish detected traffic light indexs.
'''
rate = rospy.Rate(self.publishing_rate)
while not rospy.is_shutdown():
if not (self.new_image and self.current_pos and self.waypoints_tree
and self.dbw_enabled):
rate.sleep()
continue
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 use previous stable
state.
'''
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 else -1
self.last_tl_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_tl_wp))
self.state_count += 1
rate.sleep()
def pose_cb(self, msg):
self.current_pos = msg.pose.position
def waypoints_cb(self, msg):
'''
Callback for getting a list of waypoints.
'''
# Dont process further if we are getting similar waypoints.
if self.waypoints and (sorted(self.waypoints) ==
sorted(msg.waypoints)):
return
self.waypoints = msg.waypoints
waypoints_2d = [[wp.pose.pose.position.x, wp.pose.pose.position.y]
for wp in self.waypoints]
self.waypoints_tree = KDTree(waypoints_2d)
self.update_stop_light_waypoints()
rospy.loginfo("Number of Waypoints received: {0}".format(
len(self.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.new_image = True
self.camera_image = msg
def dbw_enabled_cb(self, msg):
'''
Returns if vehicle is in DBW mode.
'''
self.dbw_enabled = msg.data
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
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
closest_id = self.waypoints_tree.query([x, y], 1)[1]
return closest_id
def get_light_state(self, light):
"""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)
"""
try:
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
except CvBridgeError as e:
rospy.logwarn(e)
return TrafficLight.UNKNOWN, 0
traffic_class, score = self.light_classifier.get_classification(cv_image)
return traffic_class, score
def update_stop_light_waypoints(self):
'''
Map traffic lights to its closest waypoint index.
'''
sl_positions = self.config['stop_line_positions']
for stop_pos in sl_positions:
idx = self.get_closest_waypoint(stop_pos[0], stop_pos[1])
self.wpi_stop_pos_dict[idx] = stop_pos
rospy.loginfo("Stop line positions from config: {0}".
format(self.wpi_stop_pos_dict))
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closest to the upcoming stop line for a
traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
stop_pos = None
# Traffic light and waypoints are fixed; or updated less frequently.
# If * = Waypoints; # = Traffic light waypoints. Representation:
# * * * # * * # * * * * # * * * * # * * # * # ** * * * * # * *
next_stop_line_wpi = -1
car_wpi = self.get_closest_waypoint(self.current_pos.x,
self.current_pos.y)
# Get closest stopline waypoint to car position.
if car_wpi < max(self.wpi_stop_pos_dict.keys()):
next_stop_line_wpi = min([x for x in self.wpi_stop_pos_dict.keys()
if x >= car_wpi])
else: # Circular.
next_stop_line_wpi = min(self.wpi_stop_pos_dict.keys())
# Get the stop position corrosponding to the waypoint index.
stop_pos = self.wpi_stop_pos_dict.values()[
self.wpi_stop_pos_dict.keys().index(next_stop_line_wpi)]
dl = lambda x, y, x1, y1 : math.sqrt((x-x1)**2 + (y-y1)**2)
self.distance_to_stop = dl(self.current_pos.x, self.current_pos.y,
stop_pos[0], stop_pos[1])
# Process TL images only if the car is in delta range.
if self.distance_to_stop <= self.tl_sight_dist:
if USE_CLASSIFIER:
state, score = self.get_light_state(stop_pos)
if state == TrafficLight.RED:
rospy.loginfo("Traffic light:{0}; Red, score:{1}".format(
state, score))
return next_stop_line_wpi, state
elif (state == TrafficLight.YELLOW and
self.distance_to_stop >= MIN_STOPPING_DISTANCE):
# Treat as Red; stop tl.
state = TrafficLight.RED
rospy.loginfo("Traffic light:{0}; Yellow, score:{1}".format(
state, score))
return next_stop_line_wpi, state
else:
return -1, state
else:
for light in self.lights:
if dl(light.pose.pose.position.x, light.pose.pose.position.y,
stop_pos[0], stop_pos[1]) <= 25.0:
return next_stop_line_wpi, light.state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoints_2d = None
self.waypoints_tree = 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)
sub4 = 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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
self.waypoints_tree = KDTree(self.waypoints_2d)
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 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, x, y):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
x (double): Position x of the query point
y (double): Position y of the query point
Returns:
int: index of the closest waypoint in self.waypoints
"""
#TODO Implement
closest_idx = self.waypoints_tree.query([x, y], 1)[1]
return closest_idx
def get_light_state(self, light):
"""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)
"""
# Used for training only
# return light.state
if (not self.has_image):
return TrafficLight.UNKNOWN
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
# Get classification
return self.light_classifier.get_classification(cv_image)
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)
"""
closest_light = None
line_wp_idx = None
# 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_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
# 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_wp_idx
if d >= 0 and d < diff:
diff = d
closest_light = light
line_wp_idx = temp_wp_idx
if closest_light:
state = self.get_light_state(closest_light)
return line_wp_idx, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
_ = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
_ = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
_ = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
_ = rospy.Subscriber('/image_color',
Image,
self.image_cb,
queue_size=1)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.stop_line_positions = self.config['stop_line_positions']
#self.upcuming_red_light_publisher = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.has_image = False
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.work_with_tl_classifier = False
if self.work_with_tl_classifier:
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.light_classifier = TLClassifier()
else:
self.upcuming_red_light_publisher = rospy.Publisher(
'/traffic_waypoint', Int32, queue_size=1)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
self.loop()
rate.sleep()
def loop(self):
if self.pose and self.waypoints:
rospy.loginfo("Current pos %s, %s", self.pose.pose.position.x,
self.pose.pose.position.y)
if self.lights and not self.work_with_tl_classifier:
next_red_light_pose, next_red_light_state = self.get_next_red_light_pose(
)
if next_red_light_pose:
rospy.loginfo("Next light pos %s, %s",
next_red_light_pose.pose.pose.position.x,
next_red_light_pose.pose.pose.position.y)
xy = self.get_closest_stop_line_position(
next_red_light_pose)
rospy.loginfo("Stop line pos %s, %s", xy[0], xy[1])
index = self.get_closest_waypoint_xy(xy)
rospy.loginfo("publish = %s", index)
if next_red_light_state == TrafficLight.RED or next_red_light_state == TrafficLight.YELLOW:
self.upcuming_red_light_publisher.publish(Int32(index))
else:
self.upcuming_red_light_publisher.publish(Int32(-1))
def pose_cb(self, msg):
self.pose = msg
quaternion = (self.pose.pose.orientation.x,
self.pose.pose.orientation.y,
self.pose.pose.orientation.z,
self.pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
self.current_yaw = euler[2]
def waypoints_cb(self, waypoints):
if self.waypoints is None:
self.waypoints = copy.deepcopy(waypoints.waypoints)
def traffic_cb(self, msg):
self.lights = msg.lights
def get_closest_stop_line_position(self, light_pose):
"""
Find closest stop line position to the given position as a list of x, y coordinates. If no stop line position is provided return 0, 0
"""
assert len(self.stop_line_positions) > 0
curr_min_distance = 1.0E10
curr_closest_pos = [0, 0]
for pos in self.stop_line_positions:
x, y = pos[0] - light_pose.pose.pose.position.x, pos[
1] - light_pose.pose.pose.position.y
curr_dist = math.sqrt(x * x + y * y)
if curr_dist < curr_min_distance:
curr_min_distance = curr_dist
curr_closest_pos = pos
return curr_closest_pos
def get_stop_line(self, distance):
"""
Find closest stop line position to the given position as a list of x, y coordinates. If no stop line position is provided return 0, 0
"""
assert len(self.stop_line_positions) > 0
min_delta = 1.0E10
closest_line = [0, 0]
for pos in self.stop_line_positions:
x, y = pos[0] - self.pose.pose.position.x, pos[
1] - self.pose.pose.position.y
stop_line_dist = math.sqrt(x * x + y * y)
delta = distance - stop_line_dist
if delta > 0 and delta < min_delta:
min_delta = delta
closest_line = pos
return closest_line
def get_next_red_light_pose(self):
"""
Find position of next traffic light
"""
#red_lights = [l for l in self.lights if l.state == TrafficLight.RED]
# rospy.loginfo("get_next_red_light_pose")
# if (self.has_image):
# time0 = time.time()
# next_traffic_light_pose, next_traffic_light_state = self.get_light_state(None)
# time1 = time.time()
# rospy.loginfo("cost_time: %s ms, traffic light status: %s", 1000*(time1-time0), next_traffic_light_state)
# return next_traffic_light_pose, next_traffic_light_state
# else:
# return None, self.state
index = self.get_waypoint_ahead(self.pose.pose, self.lights)
index = index % len(self.lights)
next_traffic_light_pose = self.lights[index]
next_traffic_light_state = self.lights[index].state
return next_traffic_light_pose, next_traffic_light_state
def get_waypoint_ahead(self, pose, waypoints):
closest_dist = 100000
closest_idx = 0
for idx, wp in enumerate(waypoints):
dist = self.distance(pose.position, wp.pose.pose.position)
if dist < closest_dist:
closest_dist = dist
closest_idx = idx
p1 = pose.position
p2 = waypoints[closest_idx].pose.pose.position
heading = math.atan2((p2.y - p1.y), (p2.x - p1.x))
angle = abs(self.current_yaw - heading)
if angle > math.pi / 4:
closest_idx += 1
return closest_idx
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
"""
rospy.loginfo("get_image_message")
self.has_image = True
self.camera_image = msg
time0 = time.time()
light_wp, state = self.process_traffic_lights()
time1 = time.time()
rospy.loginfo("cost_time: %s ms, traffic light status: %s",
1000 * (time1 - time0), state)
'''
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 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
"""
closest_dist = 100000
closest_idx = -1
for idx, wp in enumerate(self.waypoints):
dist = self.distance(pose.position, wp.pose.pose.position)
if dist < closest_dist:
closest_dist = dist
closest_idx = idx
return closest_idx
def get_closest_waypoint_xy(self, xy):
closest_dist = 100000
closest_idx = -1
assert len(self.waypoints) > 0
for idx, wp in enumerate(self.waypoints):
x = xy[0] - wp.pose.pose.position.x
y = xy[1] - wp.pose.pose.position.y
dist = math.sqrt(x * x + y * y)
if dist < closest_dist:
closest_dist = dist
closest_idx = idx
return closest_idx
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
'''
for tl in self.lights:
if (distance(tl.pose.position, light.pose.position) < 0.1):
return tl.state
return TrafficLight.UNKNOWN
'''
def distance(self, p1, p2):
x, y, z = p1.x - p2.x, p1.y - p2.y, p1.z - p2.z
return math.sqrt(x * x + y * y + z * z)
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
# List of positions that correspond to the line to stop in front of for a given intersection
if (self.pose):
car_position = self.get_closest_waypoint(self.pose.pose)
#find the closest visible traffic light (if one exists)
dist, state = self.get_light_state(light)
if (dist == -1):
return -1, TrafficLight.UNKNOWN
else:
stop_line = self.get_stop_line(dist)
rospy.loginfo("Stop line pos %s, %s", stop_line[0], stop_line[1])
light_wp = self.get_closest_waypoint_xy(stop_line)
rospy.loginfo("publish = %s", light_wp)
return light_wp, state
'''
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.base_waypoints = None
self.camera_image = None
self.lights = []
self.has_image = False
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
model = load_model("./light_classification/model.h5")
self.light_classifier = TLClassifier(model)
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)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
self.bridge = CvBridge()
self.listener = tf.TransformListener()
rospy.spin()
def pose_cb(self, msg):
self.pose = msg.pose
def waypoints_cb(self, data): # is only called once
self.base_waypoints = data.waypoints
def traffic_cb(self, msg): # call-back for cheating, as long as we dont have a classifier
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 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
"""
dist = float('inf') # a very large number
nearest_idx = None
for idx,waypoint in enumerate(self.base_waypoints):
temp_dist = self.distance(waypoint, pose)
#rospy.logwarn(temp_dist)
if dist > temp_dist:
dist = temp_dist
nearest_idx = idx
return nearest_idx
def distance(self, waypoint, pose):
dl = lambda a, b: math.sqrt((a.x-b.x)**2 + (a.y-b.y)**2)
return dl(waypoint.pose.pose.position, pose.position)
def project_to_image_plane(self, point_in_world):
"""Project point from 3D world coordinates to 2D camera image location
Args:
point_in_world (Point): 3D location of a point in the world
Returns:
x (int): x coordinate of target point in image
y (int): y coordinate of target point in image
"""
fx = self.config['camera_info']['focal_length_x']
fy = self.config['camera_info']['focal_length_y']
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
# get transform between pose of camera and world frame
trans = None
try:
now = rospy.Time.now()
self.listener.waitForTransform("/base_link",
"/world", now, rospy.Duration(1.0))
(trans, rot) = self.listener.lookupTransform("/base_link",
"/world", now)
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logerr("Failed to find camera to map transform")
#TODO Use tranform and rotation to calculate 2D position of light in image
x = 0
y = 0
return (x, y)
def get_light_state(self, light):
"""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(not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#x, y = self.project_to_image_plane(light.pose.pose.position)
#Get classification
return self.light_classifier.get_classification(cv_image)
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)
"""
# check if all necessary information is available
if (self.pose == None or self.base_waypoints == None):
return -1, TrafficLight.UNKNOWN
light_positions = self.config['stop_line_positions']
# find the waypoint index that is closest to the car
car_wp_idx = self.get_closest_waypoint(self.pose)
# find the closest visible traffic light (if one exists)
light_wp_idx = None
light = None # TrafficLight object
tl_waypoint_indices = self.get_traffic_light_wp_index(light_positions)
for i, tl_wp_idx in enumerate(tl_waypoint_indices):
idx_diff = tl_wp_idx - car_wp_idx
# traffic light is ahead of the car within number of LOOKAHEAD_WPS
if idx_diff >= 0 and idx_diff <= LOOKAHEAD_WPS:
# minus LIGHTGAP so that the car stops near the stop line
light_wp_idx = tl_wp_idx - LIGHTGAP
light = self.lights[i]
#rospy.logwarn(light_wp_idx)
#rospy.logwarn(tl_waypoint_indices)
if light:
state = self.get_light_state(light)
#rospy.logwarn(state)
return light_wp_idx, state#light.state
else:
return -1, TrafficLight.UNKNOWN
def get_traffic_light_wp_index(self, light_positions):
indices = []
for pos in light_positions:
pose = Pose()
pose.position.x = pos[0]
pose.position.y = pos[1]
indices.append(self.get_closest_waypoint(pose))
return indices
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.KDTree = None
self.camera_image = None
self.lights = []
self.stop_lines = None
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb, queue_size=1)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb, queue_size=1)
'''
/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.
'''
#[alexm]NOTE: we should rely on this topic's data except state of the light
#[alexm]NOTE: according to this: https://carnd.slack.com/messages/C6NVDVAQ3/convo/C6NVDVAQ3-1504625321.000063/
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb, queue_size=1)
rospy.Subscriber('/image_color', Image, self.image_cb, queue_size=1)
rospy.Subscriber('/next_wp', Int32, self.next_wp_cb, queue_size=1)
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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.light_classifier.init()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.next_wp = None
rospy.spin()
def next_wp_cb(self, val):
self.next_wp = val.data
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, lane):
if self.waypoints != lane.waypoints:
data = []
for wp in lane.waypoints:
data.append((wp.pose.pose.position.x, wp.pose.pose.position.y))
self.KDTree = spatial.KDTree(data)
self.waypoints = lane.waypoints
self.stop_lines = None
def find_stop_line_position(self, light):
"""Finds stop line position from config corresponding to given light
Args:
msg (Image): image from car-mounted camera
"""
stop_line_positions = self.config['stop_line_positions']
min_distance = 100000
result = None
light_pos = light.pose.pose.position
for pos in stop_line_positions:
distance = self.euclidean_distance_2d(pos, light_pos)
if (distance< min_distance):
min_distance = distance
result = pos
return result
def traffic_cb(self, msg):
if not self.stop_lines and self.KDTree:
stop_lines = []
for light in msg.lights:
# find corresponding stop line position from config
stop_line_pos = self.find_stop_line_position(light)
# find corresponding waypoint indicex
closest_index = self.KDTree.query(np.array([stop_line_pos]))[1][0]
closest_wp = self.waypoints[closest_index]
if not self.is_ahead(closest_wp.pose.pose, stop_line_pos):
closest_index = max(closest_index - 1, 0)
# add index to list
stop_lines.append(closest_index)
# update lights and stop line waypoint indices
self.lights = msg.lights
self.stop_lines = stop_lines
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 in [TrafficLight.RED, 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 project_to_image_plane(self, stamped_world_point):
"""Project point from 3D world coordinates to 2D camera image location
Args:
point_in_world (Point): 3D location of a point in the world
Returns:
x (int): x coordinate of target point in image
y (int): y coordinate of target point in image
"""
fx = self.config['camera_info']['focal_length_x']
fy = self.config['camera_info']['focal_length_y']
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
# get transform between pose of camera and world frame
stamped_world_point.header.stamp = rospy.Time.now()
base_point = None
try:
now = rospy.Time.now()
self.listener.waitForTransform("/base_link",
"/world", now, rospy.Duration(1.0))
base_point = self.listener.transformPoint("/base_link", stamped_world_point);
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logerr("Failed to find camera to map transform")
if not base_point:
return (0, 0)
base_point = base_point.point
print (base_point)
cx = image_width/2
cy = image_height/2
# Ugly workaround wrong camera settings
if fx < 10.:
fx = 2344.
fy = 2552.
cy = image_height
base_point.z -= 1
cam_matrix = np.array([[fx, 0, cx],
[ 0, fy, cy],
[ 0, 0, 1]])
obj_points = np.array([[- base_point.y, - base_point.z, base_point.x]]).astype(np.float32)
result, _ = cv2.projectPoints(obj_points, (0,0,0), (0,0,0), cam_matrix, None)
#print(result)
cx = image_width/2
cy = image_height
x = int(result[0,0,0])
y = int(result[0,0,1])
print(x, y)
return (x, y)
def get_light_state(self, light):
"""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(not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#TODO use light location to zoom in on traffic light in image
# Projection wont work cuz of absent base_link->world transform on site
#tl_point = PointStamped()
#tl_point.header = light.pose.header
#tl_point.point = Point()
#tl_point.point.x = light.pose.pose.position.x
#tl_point.point.y = light.pose.pose.position.y
#tl_point.point.z = light.pose.pose.position.z
#x, y = self.project_to_image_plane(tl_point)
#Get classification
state = self.light_classifier.get_classification(cv_image)
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
light_wp = -1
if(self.waypoints and self.next_wp and self.stop_lines):
next_wp = self.waypoints[min(self.next_wp, len(self.waypoints)-1)]
target_velocity = next_wp.twist.twist.linear.x
search_distance = target_velocity * target_velocity / 2 / MAX_DECEL
min_distance = search_distance
for i in range(len(self.stop_lines)):
stop_line_wp_index = self.stop_lines[i]
if stop_line_wp_index >= self.next_wp:
stop_line_wp = self.waypoints[stop_line_wp_index]
distance = self.euclidean_distance_2d(next_wp.pose.pose.position, stop_line_wp.pose.pose.position)
if (distance < min_distance):
light_wp = stop_line_wp_index
light = self.lights[i]
# print('n_wp:{}; l_wp:{}'.format(self.next_wp, light_wp))
if light_wp > -1:
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
def is_ahead(self, origin_pose, test_position):
"""Determines if test position is ahead of origin_pose
Args:
origin_pose - geometry_msgs.msg.Pose instance
test_position - geometry_msgs.msg.Point instance, or tuple (x,y)
Returns:
bool: True iif test_position is ahead of origin_pose
"""
test_x = self.get_x(test_position)
test_y = self.get_y(test_position)
orig_posit = origin_pose.position
orig_orient = origin_pose.orientation
quaternion = (orig_orient.x, orig_orient.y, orig_orient.z, orig_orient.w)
_, _, yaw = tf.transformations.euler_from_quaternion(quaternion)
orig_x = ((test_x - orig_posit.x) * math.cos(yaw) \
+ (test_y - orig_posit.y) * math.sin(yaw))
return orig_x > 0.
def euclidean_distance_2d(self, position1, position2):
"""Finds distance between two position1 and position2
Args:
position1, position2 - geometry_msgs.msg.Point instance, or tuple (x,y)
Returns:
double: distance
"""
a_x = self.get_x(position1)
a_y = self.get_y(position1)
b_x = self.get_x(position2)
b_y = self.get_y(position2)
return math.sqrt((a_x - b_x)**2 + (a_y - b_y)**2)
def get_x(self, pos):
return pos.x if isinstance(pos, Point) else pos[0]
def get_y(self, pos):
return pos.y if isinstance(pos, Point) else pos[1]
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.light_classifier = None
self.waypoints_2d = None
self.waypoints_tree = None
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
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.
'''
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb)
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.bridge = CvBridge()
dirname = os.path.dirname(__file__)
model_name = self.config['model_name']
model_path = os.path.join(dirname, "light_classification/models/{}".format(model_name))
label_path = os.path.join(dirname, 'light_classification/models/label_map.pbtxt')
self.light_classifier = TLClassifier(model_path, label_path)
self.listener = tf.TransformListener()
self.loop()
def loop(self):
rate = rospy.Rate(50) # 50Hz
while not rospy.is_shutdown():
rate.sleep()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
if not self.waypoints_2d:
self.waypoints_2d = [[e.pose.pose.position.x, e.pose.pose.position.y] for e in waypoints.waypoints]
self.waypoints_tree = KDTree(self.waypoints_2d)
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 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, x, y):
"""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
"""
closest_idx = self.waypoints_tree.query([x, y], 1)[1]
return closest_idx
def get_light_state(self, light):
"""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(not self.has_image or not self.light_classifier):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
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)
"""
closest_light = None
line_wp_idx = None
# 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 and self.waypoints_tree):
car_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x, self.pose.pose.position.y)
# 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_wp_idx
if d >= 0 and d < diff:
diff = d
closest_light = light
line_wp_idx = temp_wp_idx
if closest_light:
state = self.get_light_state(closest_light)
return line_wp_idx, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.base_waypoints = None
self.waypoints_2d = None
self.waypoint_tree = None
self.camera_image = None
self.lights = []
self.bridge = CvBridge()
self.light_classifier = TLClassifier(rospy.get_param('~model_file'))
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.class_count = 0
self.process_count = 0
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=2)
rospy.Subscriber('/base_waypoints',
Lane,
self.waypoints_cb,
queue_size=8)
rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray,
self.traffic_cb,
queue_size=2)
rospy.Subscriber('/image_color', Image, self.image_cb, queue_size=1)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.base_waypoints = waypoints
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
self.waypoint_tree = KDTree(self.waypoints_2d)
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
:param msg: image from car-mounted camera
"""
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 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 process_traffic_lights(self):
"""
Finds closest visible traffic light, if one exists, and determines its location and color
:return:
int: index of waypoint closest to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
closest_light = None
line_wp_idx = -1
state = TrafficLight.UNKNOWN
# 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_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
diff = len(self.base_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_wp_idx
if 0 <= d < diff:
diff = d
closest_light = light
line_wp_idx = temp_wp_idx
# if we have found a closest light to monitor, then determine the stop line position of this light
if closest_light:
self.process_count += 1
state = self.get_light_state(closest_light)
if (self.process_count % LOGGING_THROTTLE_FACTOR) == 0:
rospy.logwarn("DETECT: line_wp_idx={}, state={}".format(
line_wp_idx, self.to_string(state)))
return line_wp_idx, state
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
:param x: x coord position to match a waypoint to
:param y: y coord position to match a waypoint to
:return: index of the closest waypoint in self.waypoints
"""
closest_idx = self.waypoint_tree.query([x, y], 1)[1]
return closest_idx
def get_light_state(self, light):
"""
Determines the current color of the traffic light
:param light: light to classify
:return: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
# For test mode, just return the light state
if TEST_MODE_ENABLED:
classification = light.state
else:
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
# Get classification
classification = self.light_classifier.get_classification(cv_image)
# Save image (throttled)
if SAVE_IMAGES and (self.process_count % LOGGING_THROTTLE_FACTOR
== 0):
save_file = "../../../imgs/{}-{:.0f}.jpeg".format(
self.to_string(classification), (time.time() * 100))
cv2.imwrite(save_file, cv_image)
return classification
def to_string(self, state):
out = "unknown"
if state == TrafficLight.GREEN:
out = "green"
elif state == TrafficLight.YELLOW:
out = "yellow"
elif state == TrafficLight.RED:
out = "red"
return out
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)
'''
/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")
model_filename = rospy.get_param(
"/traffic_light_model", "frozen_inference_graph_simulation_ssd")
self.config = yaml.load(config_string)
self.light_positions = self.config['stop_line_positions']
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.camera_image = None
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.state_count = 0
self.last_wp = -1
if DEBUG_SAVE_IMAGES:
self.image_counter = 0
self.save_dir = '/images/'
if not DEBUG_GROUND_TRUTH:
# TLClassifier now takes the "model_filename", read from parameter "/traffic_light_model", as model
self.light_classifier = TLClassifier(model_filename)
rospy.spin()
else:
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.process_traffic_lights()
rate.sleep()
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):
"""
Callback function for camera images
"""
self.camera_image = msg
if DEBUG_SAVE_IMAGES:
self.img_save(msg)
self.process_traffic_lights()
def img_save(self, img):
curr_dir = os.path.dirname(os.path.realpath(__file__))
img.encoding = "rgb8"
cv_image = CvBridge().imgmsg_to_cv2(img, "bgr8")
file_name = curr_dir + self.save_dir + '/img_' + '%06d' % self.image_counter + '.png'
rospy.logwarn("file_name = %s", file_name)
cv2.imwrite(file_name, cv_image)
self.image_counter += 1
rospy.logwarn("[TD] Image saved!")
"""
Creates a pose for the traffic light in the format required by get_closest_waypoint function instead of creating a new one
"""
def create_light_site(self, x, y, z, yaw, state):
light = TrafficLight()
light.header = Header()
light.header.stamp = rospy.Time.now()
light.header.frame_id = 'world'
# Create a Pose object to place inside the TrafficLight object
light.pose = PoseStamped()
light.pose.header = Header()
light.pose.header.stamp = rospy.Time.now()
light.pose.header.frame_id = 'world'
light.pose.pose.position.x = x
light.pose.pose.position.y = y
light.pose.pose.position.z = z
# For reference: https://answers.ros.org/question/69754/quaternion-transformations-in-python/
q = tf.transformations.quaternion_from_euler(0.0, 0.0,
math.pi * yaw / 180.0)
light.pose.pose.orientation = Quaternion(*q)
light.state = state
return light
"""
2D Euclidean distance
"""
def distance2d(self, x1, y1, x2, y2):
return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
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
"""
dist = float('inf')
dl = lambda a, b: math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 +
(a.z - b.z)**2)
wp = 0
for i in range(len(self.waypoints.waypoints)):
new_dist = dl(pose.position,
self.waypoints.waypoints[i].pose.pose.position)
if new_dist < dist:
dist = new_dist
wp = i
return wp
def get_light_state(self):
"""Determines the current color of the traffic light
"""
if self.camera_image is None:
rospy.logwarn('[Detector] No image')
return False
else:
self.camera_image.encoding = "rgb8"
#return TrafficLight.UNKNOWN
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
#cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
# tic = rospy.get_time()
state = self.light_classifier.get_classification(cv_image)
# tac = rospy.get_time()
# rospy.logwarn("Detection time: {:1.6f}s".format(tac-tic))
# If classification result is unknown, return last state
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. It then publishes
"""
light = None
# Proceed only if the first messages have been received
if hasattr(self, 'pose') and hasattr(self, 'waypoints'):
# Get position of car
car_wp = self.get_closest_waypoint(self.pose.pose)
# If using GT, make sure the data have been received
if DEBUG_GROUND_TRUTH:
if hasattr(self, 'lights'):
# Real state of traffic lights
light_positions_state = self.lights
else:
# If message has not been received yet
rospy.logwarn('Traffic_lights not received yet')
return
# Stop poritions for each traffic light
light_positions = self.light_positions
min_distance = float('inf')
for i, light_position in enumerate(light_positions):
if DEBUG_GROUND_TRUTH:
true_state = light_positions_state[i].state
light_candidate = self.create_light_site(
light_position[0], light_position[1], 0.0, 0.0,
true_state)
else:
light_candidate = self.create_light_site(
light_position[0], light_position[1], 0.0, 0.0,
TrafficLight.UNKNOWN)
light_wp = self.get_closest_waypoint(light_candidate.pose.pose)
# Check that's ahead of us and not behind and is in sight
light_distance = self.distance2d(
self.waypoints.waypoints[car_wp].pose.pose.position.x,
self.waypoints.waypoints[car_wp].pose.pose.position.y,
self.waypoints.waypoints[light_wp].pose.pose.position.x,
self.waypoints.waypoints[light_wp].pose.pose.position.y)
# Getting the closest traffic light which is ahead of us and within visible distance
if (light_wp % len(self.waypoints.waypoints)) > (
car_wp % len(self.waypoints.waypoints)) and (
light_distance < VISIBLE_DISTANCE) and (
light_distance < min_distance):
light = light_candidate
closest_light_wp = light_wp
min_distance = light_distance
if light:
if DEBUG_GROUND_TRUTH:
# Use ground truth provided by simulator
state = light.state
else:
# Nominal mode: light state comes from classifier
state = self.get_light_state()
rospy.logwarn(
'[TD] Traffic light id {} in sight, color state: {}'.
format(closest_light_wp, state))
light_wp = closest_light_wp
else:
light_wp = -1
state = TrafficLight.UNKNOWN
else: # if messages are not received yet (only for beginning of simulation), return position of first traffic light with RED status
# light_wp = 292 will raise a problem in second test
light_wp = -1
state = TrafficLight.RED
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 in [
TrafficLight.RED, 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
'''
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoints_2d = None
self.waypoints_tree = 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)
# traffic light states and waypoint
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
# classifier members
self.bridge = CvBridge()
self.light_classifier = TLClassifier(self.config['is_site'])
self.listener = tf.TransformListener()
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
rospy.logerr('waypoint_cb was called')
if not self.waypoints:
rospy.logerr('!!! waypoint_cb called but no waypoints present!?')
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
self.waypoints_tree = KDTree(self.waypoints_2d)
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, new_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 != new_state:
self.state_count = 0
self.state = new_state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp if new_state == TrafficLight.RED 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, x, y):
"""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: for now ignore if the point is in front or behind the given point
closest_idx = self.waypoints_tree.query([x, y], 1)[1]
return closest_idx
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
"""
# TODO: for now, I just use the plain signalled ground truth value as I want to
# create the classifier last
return light.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
light_wp = None
# 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 not self.waypoints:
rospy.logerr(
'!!! No waypoints present - waypoint_cb was not called!')
if self.pose and self.waypoints:
car_position = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
#TODO find the closest visible traffic light (if one exists)
# set the diff to the maximum possible difference --> waypoint count
wp_diff = len(self.waypoints.waypoints)
# enumerate all the lights and their positions to get the difference of waypoints from
# the closest waypoint to the TL position
for i, cur_light in enumerate(self.lights):
# get the corresponding stop line position for the trafficlight
stop_line = stop_line_positions[i]
# get the closest waypoint to the stop line position
stop_line_wp = self.get_closest_waypoint(
stop_line[0], stop_line[1])
# update the difference value if needed
sl_difference = stop_line_wp - car_position
if (sl_difference > 0) and (sl_difference < wp_diff):
wp_diff = sl_difference
light = cur_light
light_wp = stop_line_wp
if light and light_wp:
new_state = self.get_light_state(light)
return light_wp, new_state
return -1, TrafficLight.UNKNOWN
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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
rospy.logdebug("current pose: %s", self.pose)
def waypoints_cb(self, waypoints):
self.waypoints = waypoints.waypoints
rospy.logdebug("waypoints: %s", self.waypoints)
def traffic_cb(self, msg):
self.lights = msg.lights
rospy.logdebug("lights: %s", self.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 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 calculate_distance(self, x1, x2, y1, y2):
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2)
def get_closest_waypoint_idx(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
index = -1
dist_min = float("inf")
for i, wp in enumerate(self.waypoints):
dist = self.calculate_distance(pose.position.x, wp.pose.pose.positions.x, pose.position.y, wp.pose.pose.position.y)
if dist < dist_min:
dist_min = distance
index = i
return index
def get_light_state(self, light):
"""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(not self.has_image):
self.prev_light_loc = None
return TrafficLight.UNKNOWN
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
def is_ahead_of(self, pose, x, y):
"""Determines whether a wappoint is ahead of position
Args:
pose: base position
x (float): waypoint's global x-coordinate
y (float): waypoint's global y-coordinate
Returns:
bool: Whether the waypoint is ahead of this position
"""
x1 = pose.position.x
y1 = pose.position.y
orientation = pose.orientation
euler = tf.transformations.euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])
yaw = euler[2]
return ((x - x1) * math.cos(yaw) + (y - y1) * math.sin(yaw)) > 0
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
light_wp = -1
if not self.waypoints:
return -1, TrafficLight.UNKNOWN
if not self.lights:
return -1, TrafficLight.UNKNOWN
# 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']
car_position_idx = 0
if(self.pose):
car_position_idx = self.get_closest_waypoint_idx(self.pose.pose)
car_position = self.waypoints[car_position_idx]
#TODO find the closest visible traffic light (if one exists)
# Fined the closet visible traffic light based on current position
dist_min = float('inf')
closest_stop_line_idx = -1
for i, stop_line_pose in enumerate(stop_line_positions):
if self.is_ahead_of(car_position.pose.pose, stop_line_pose[0], stop_line_pose[1]):
dist = self.calculate_distance(car_position.pose.pose.position.x, stop_line_pose[0], car_position.pose.pose.position.y, stop_line_pose[1])
if dist < dist_min:
dist_min = dist
closest_stop_line_idx = i
if closest_stop_line_idx >= 0:
# Find wp index in waypoints which is closest to the traffic light
light = self.lights[closest_stop_line_idx]
dist_min = float('inf')
for i, wp in enumerate(self.waypoints):
if self.is_ahead_of(wp.pose.pose, stop_line_positions[closest_stop_line_idx][0], stop_line_positions[closest_stop_line_idx][1]):
dist = self.calculate_distance(wp.pose.pose.position.x, stop_line_positions[closest_stop_line_idx][0], wp.pose.pose.position.y, stop_line_positions[closest_stop_line_idx][1])
if dist < dist_min:
dist_min = dist
light_wp = i
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
#self.initialized = False
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.waypoints_2d = None
self.waypoint_tree = None
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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.last_state = TrafficLight.UNKNOWN
self.last_light_state = None
self.last_wp = -1
self.state_count = 0
self.has_image = False
self.state = TrafficLight.UNKNOWN
self.last_img_processed = 0
#self.initialized = True
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
rospy.loginfo("wpt_cb: invoked")
self.waypoints = waypoints
if not self.waypoints_2d: #not initialized yet
self.waypoints_2d = [[
wpt.pose.pose.position.x, wpt.pose.pose.position.y
] for wpt in waypoints.waypoints]
self.waypoint_tree = KDTree(self.waypoints_2d)
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
"""
if (self.waypoint_tree == None):
return
time_elapsed = timer() - self.last_img_processed
#Do not process the camera image unless 20 milliseconds have passed from last processing
if (time_elapsed < CAMERA_IMG_PROCESS_RATE):
return
#rospy.loginfo ("image_cb: time_elapsed {}".format(time_elapsed))
self.has_image = True
self.camera_image = msg
self.last_img_processed = timer()
light_wp, state = self.process_traffic_lights()
#rospy.loginfo ("image_cb: light_wp {0} \n state {1}".format (light_wp, state))
"""
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 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, x, y):
"""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
closest_idx = self.waypoint_tree.query([x, y], 1)[1]
# rospy.loginfo("tl_gcwp: x {}; y {} closest_idx {}".format(x, y, closest_idx))
return (closest_idx)
def get_light_state(self, light):
"""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)
"""
# for testing just use simulator light state
#if not (self.config['tl'] ['is_carla']):
# return light.state
#check if image is present
if (self.has_image == False):
self.prev_light_loc = None
return False
# check whether tl_classifier for simulator is working
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
result = self.light_classifier.get_classification(cv_image)
if (self.last_light_state != result):
rospy.loginfo("image classification {}".format(result))
self.last_light_state = result
return result
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)
"""
closest_light = None
line_wp_idx = None
# 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 and self.waypoint_tree:
car_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
#TODO find the closest visible traffic light (if one exists)
diff = len(self.waypoints.waypoints)
for i, light in enumerate(self.lights):
# get the stop light line way point index
line = stop_line_positions[i]
temp_wp_idx = self.get_closest_waypoint(line[0], line[1])
d = temp_wp_idx - car_wp_idx
if (d >= 0) and (d < diff):
diff = d
closest_light = light
line_wp_idx = temp_wp_idx
# do not process the camera image unless the traffic light <= 300 waypoints
if (closest_light) and (
(line_wp_idx - car_wp_idx) <= WAYPOINT_DIFFERENCE):
#rospy.loginfo ("tl_detector: car_wp_idx: {}, line_wp_index: {}".format(car_wp_idx, line_wp_idx))
state = self.get_light_state(closest_light)
return (line_wp_idx, state)
else:
#rospy.loginfo ("ptf: unknown state of traffic light")
#self.waypoints = None # check on this
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector', log_level=rospy.DEBUG)
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.stop_lines=[]
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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.logdebug("initialized....")
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
rospy.logdebug("waypoints received....")
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
start = timeit.default_timer()
light_wp, state = self.process_traffic_lights()
end = timeit.default_timer()
'''
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_coordinates_vector(self,position):
return np.asarray([position.x, position.y, position.z])
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
"""
if self.waypoints is not None:
node = self.get_coordinates_vector(pose.position)
waypoints = np.asarray([[waypoint.pose.pose.position.x, waypoint.pose.pose.position.y, waypoint.pose.pose.position.z] for waypoint in
self.waypoints.waypoints])
nearest_index = distance.cdist([node], waypoints).argmin()
else:
rospy.logdebug("no waypoints")
return nearest_index
def get_light_state(self, light):
"""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(not self.has_image):
self.prev_light_loc = None
rospy.logdebug("returning false")
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
#Get classification
return self.light_classifier.get_classification(cv_image)
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
light_wp = -1
state = TrafficLight.UNKNOWN
# 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']
closest_waypoint_fn = self.get_closest_stoplight_waypoint
closest_waypoints = [closest_waypoint_fn(stop_line_position) for stop_line_position in stop_line_positions]
self.stop_lines.extend(closest_waypoints)
# Find the nearest waypoint
#print("self pose ",self.pose)
min_idx_dist = 150
if(self.pose):
car_position = self.get_closest_waypoint(self.pose.pose)
for i, stop_line_idx in enumerate(self.stop_lines):
idx_dist = stop_line_idx - car_position
if 0 < idx_dist < min_idx_dist:
light = i
min_idx_dist = idx_dist
light_wp = stop_line_idx
break
else:
light=None
#light=1
#TODO find the closest visible traffic light (if one exists)
if light is not None:
state = self.get_light_state(light)
return light_wp, state
#self.waypoints = None
return -1, TrafficLight.UNKNOWN
def get_closest_stoplight_waypoint(self, stop_line_position):
pose = Pose()
pose.position.x = stop_line_position[0]
pose.position.y = stop_line_position[1]
pose.position.z = 0
closest_waypoint = self.get_closest_waypoint(pose)
return closest_waypoint
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.light_classifier = TLClassifier()
self.pose = None
self.waypoints = None
self.waypoints_num = None
self.current_waypoint_index = None
self.camera_image = None
self.lights = []
self.lights_dict = {}
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub2 = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
sub7 = rospy.Subscriber('/current_waypoint', Int32,
self.current_waypoint_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.
'''
self.traffic_lights_sub = rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray,
self.traffic_cb)
sub6 = rospy.Subscriber('/image_color',
Image,
self.image_cb,
queue_size=1)
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.bridge = CvBridge()
self.listener = tf.TransformListener()
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, msg):
self.waypoints = msg.waypoints
self.waypoints_num = len(self.waypoints)
def current_waypoint_cb(self, msg):
self.current_waypoint_index = msg.data
def traffic_cb(self, msg):
self.lights = self.get_sorted_lights_with_waypoints(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 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_sorted_lights_with_waypoints(self, lights):
delimiter = '#'
for light in lights:
key = str(light.pose.pose.position.x) + delimiter + str(light.pose.pose.position.y) \
+ delimiter + str(light.pose.pose.position.z)
if key in self.lights_dict:
light.closest_stop_waypoint = self.lights_dict[key]
else:
light.closest_stop_waypoint = self.get_closest_stop_line_waypoint(
light.pose.pose)
self.lights_dict[key] = light.closest_stop_waypoint
return sorted(lights, key=lambda l: l.closest_stop_waypoint)
def get_closest_stop_line_waypoint(self, pose):
""" the reason for all the extra processing is that we get stop line positions and traffic light positions
in different feeds, it would be much easier if we wouldn't have to do the match and get it a priori """
# find closest waypoint before the traffic light
closest_waypoint_index = self.get_closest_waypoint(pose, 0, True)
# find the closest stop line pose to waypoint
current_stop_line_pose = self.get_closest_stop_line_pose(
closest_waypoint_index)
# find closest waypoint to stop line going backwards from the waypoint closest to traffic light
return waypoint_helper.get_closest_waypoint_before_index(
self.waypoints, current_stop_line_pose, closest_waypoint_index,
self.waypoints_num, True)
def get_closest_stop_line_pose(self, closest_waypoint_index):
if closest_waypoint_index is 0:
return 0
light_stop_line_positions = self.config['stop_line_positions']
waypoint = self.waypoints[closest_waypoint_index]
min_distance = float('inf')
current_stop_line_position = waypoint.pose.pose.position
for stop_line in light_stop_line_positions:
stop_line_position = Point()
stop_line_position.x = stop_line[0]
stop_line_position.y = stop_line[1]
stop_line_position.z = 0
distance = waypoint_helper.direct_position_distance(
stop_line_position, waypoint.pose.pose.position)
if distance < min_distance:
min_distance = distance
current_stop_line_position = stop_line_position
current_stop_line_pose = Pose()
current_stop_line_pose.position = current_stop_line_position
return current_stop_line_pose
def get_closest_waypoint(self,
pose,
current_waypoint_index=0,
before=False):
"""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
current_waypoint_index (Int32): current waypoint index
before: (Boolean): should return waypoint before or ahead of the position
Returns:
int: index of the closest waypoint in self.waypoints
"""
if before:
return waypoint_helper.get_closest_waypoint_before_index(
self.waypoints, pose, current_waypoint_index,
self.waypoints_num)
else:
return waypoint_helper.get_closest_waypoint_ahead_index(
self.waypoints, pose, current_waypoint_index,
self.waypoints_num)
def get_light_state(self, light):
"""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 not self.has_image:
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
# Get classification
return self.light_classifier.get_classification(cv_image)
def distance_between_pos(self, pos1, pos2):
return math.sqrt((pos1.x - pos2.x)**2 + (pos1.z - pos2.z)**2 +
(pos1.y - pos2.y)**2)
def get_stop_line_positions(self):
stop_line_positions = []
for light_position in self.config['stop_line_positions']:
p = Waypoint()
p.pose.pose.position.x = light_position[0]
p.pose.pose.position.y = light_position[1]
p.pose.pose.position.z = 0.0
stop_line_positions.append(p)
return stop_line_positions
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)
"""
if not self.lights or not self.pose:
return -1, TrafficLight.UNKNOWN
closest_light_stop_waypoint, closest_light = self.get_nearby_traffic_light(
self.current_waypoint_index)
if closest_light:
state = self.get_light_state(closest_light)
# state = closest_light.state
rospy.loginfo("Traffic light state is: " + str(state))
if state == TrafficLight.RED:
return closest_light_stop_waypoint, state
else:
return -1, state
return -1, TrafficLight.UNKNOWN
def get_nearby_traffic_light(self, current_waypoint):
closest_light_waypoint = None
closest_light = None
# find the closest light ahead of the car
for i in range(len(self.lights)):
light = self.lights[i]
if light.closest_stop_waypoint > current_waypoint:
if not closest_light_waypoint or light.closest_stop_waypoint < closest_light_waypoint:
closest_light_waypoint = light.closest_stop_waypoint
closest_light = light
# if we found light after our current position and it's close to us, return it, otherwise return None
if closest_light_waypoint:
distance_to_traffic_light = waypoint_helper.direct_position_distance(\
self.waypoints[closest_light_waypoint].pose.pose.position, \
self.waypoints[current_waypoint].pose.pose.position)
if distance_to_traffic_light < MAX_TRAFFIC_LIGHT_DISTANCE_METERS:
return closest_light_waypoint, closest_light
return None, None
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.stop_line_array = []
self.light_array = []
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.lights_received = False
self.pos_computed = False
self.waypoints_len = -1
self.previous_wp = -1
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.bridge = CvBridge()
self.light_classifier = TLClassifier("ssd_mobilenet_coco")
self.listener = tf.TransformListener()
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
self.waypoints_len = len(waypoints.waypoints)
def traffic_cb(self, msg):
self.lights = msg.lights
self.lights_received = True
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 else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
if state == TrafficLight.RED:
rospy.loginfo("RED or YELLOW")
else:
rospy.loginfo("GREEN or UNKNOWN")
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
closest_point = 0
max_distance = 99999
dl = lambda a, b: math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 +
(a.z - b.z)**2)
for i in range(len(self.waypoints.waypoints)):
current_distance = dl(
pose.position, self.waypoints.waypoints[i].pose.pose.position)
if (current_distance < max_distance):
closest_point = i
max_distance = current_distance
return closest_point
def get_closest_waypoint_with_history(self, pose, previous_point):
closest_point = 0
max_distance = 99999
dl = lambda a, b: math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 +
(a.z - b.z)**2)
for i in range(previous_point, previous_point + 5):
i_wrapped = i % self.waypoints_len
current_distance = dl(
pose.position,
self.waypoints.waypoints[i_wrapped].pose.pose.position)
if (current_distance < max_distance):
closest_point = i_wrapped
max_distance = current_distance
return closest_point
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
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
"""
# List of positions that correspond to the line to stop in front of for a given intersection
"""
if (self.lights_received) and (self.waypoints) and (
not self.pos_computed):
stop_line_positions = self.config['stop_line_positions']
for i_light in range(len(self.lights)):
current_stop_line = Pose()
current_stop_line.position.x = stop_line_positions[i_light][0]
current_stop_line.position.y = stop_line_positions[i_light][1]
current_stop_line_position = self.get_closest_waypoint(
current_stop_line)
current_light_position = self.get_closest_waypoint(
self.lights[i_light].pose.pose)
self.stop_line_array.append(current_stop_line_position)
self.light_array.append(current_light_position)
self.pos_computed = True
rospy.loginfo("pos computed")
if self.pos_computed:
car_position = self.previous_wp
if (self.pose):
if (self.previous_wp == -1):
car_position = self.get_closest_waypoint(self.pose.pose)
rospy.loginfo("First previous_wp computed")
self.previous_wp = car_position
else:
car_position = self.get_closest_waypoint_with_history(
self.pose.pose, self.previous_wp)
self.previous_wp = car_position
#TODO find the closest visible traffic light (if one exists)
for i_light in range(len(self.lights)):
current_stop_line_position = self.stop_line_array[i_light]
current_light_position = self.light_array[i_light]
if (current_stop_line_position > car_position):
distance_to_light = self.distance(self.waypoints.waypoints,
car_position,
current_light_position)
if (distance_to_light < 60):
light = self.lights[i_light]
light_wp = current_stop_line_position
if light:
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
def distance(self, waypoints, wp1, wp2):
dist = 0
dl = lambda a, b: math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 +
(a.z - b.z)**2)
for i in range(wp1, wp2 + 1):
dist += dl(waypoints[wp1].pose.pose.position,
waypoints[i].pose.pose.position)
wp1 = i
return dist
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.unity_sim = rospy.get_param("unity_sim", False)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier(self.unity_sim)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
if not self.unity_sim:
STATE_COUNT_THRESHOLD = 10
self.poses = []
self.lights_list = []
rospy.spin()
def pose_cb(self, msg):
self.poses.append(msg)
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
def traffic_cb(self, msg):
if not self.unity_sim:
msg.header.stamp = rospy.Time.now(
) #hack for missing timestamp in real vehicle!!!!!
self.lights_list.append(msg)
def find_closest(self, time, input_list):
while len(input_list) > 1:
item = input_list.pop(0)
if (item.header.stamp > time):
return item
if input_list:
return input_list[0]
return None
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
"""
#sync other inputs
self.pose = self.find_closest(msg.header.stamp, self.poses)
sync_lights_msg = self.find_closest(msg.header.stamp, self.lights_list)
if sync_lights_msg:
self.lights = sync_lights_msg.lights
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.
'''
#print "state: ", state, " red: ", TrafficLight.RED
if self.state != state:
self.state_count = 0
self.state = state
if self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED 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
# DONE by Facheng Li.
min_distance = None
min_dist_ind = -1
if self.waypoints is not None:
for i, wp in enumerate(self.waypoints.waypoints):
distance = (wp.pose.pose.position.x - pose.position.x) ** 2 + \
(wp.pose.pose.position.y - pose.position.y) ** 2
if distance < min_distance or min_distance is None:
min_distance = distance
min_dist_ind = i
else:
print "No waypoints in TL Detector"
return min_dist_ind
def project_to_image_plane(self, point_in_world):
"""Project point from 3D world coordinates to 2D camera image location
Args:
point_in_world (Point): 3D location of a point in the world
Returns:
x (int): x coordinate of target point in image
y (int): y coordinate of target point in image
"""
#used equations and parameters from https://discussions.udacity.com/t/focal-length-wrong/358568/23
fx = self.config['camera_info']['focal_length_x']
fy = self.config['camera_info']['focal_length_y']
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
x_offset = 0
y_offset = 0
corner_offset = 0
if self.unity_sim:
x_offset = (image_width / 2) - 30
y_offset = image_height + 50
corner_offset = 1.5
else:
x_offset = 170
y_offset = 350
corner_offset = 1.5
##########################################################################################
# get transform between pose of camera and world frame
trans = None
if not self.pose:
return (0, 0, 0, 0)
try:
now = rospy.Time.now()
self.listener.waitForTransform("/base_link", "/world",
self.pose.header.stamp,
rospy.Duration(1.0))
(trans,
rot) = self.listener.lookupTransform("/base_link", "/world",
self.pose.header.stamp)
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logerr("Failed to find camera to map transform")
return (0, 0, 0, 0)
# TODO Use tranform and rotation to calculate 2D position of light in image
# DONE by Facheng Li
RT = np.mat(self.listener.fromTranslationRotation(trans, rot))
camMat = np.mat([[fx, 0, image_width / 2], [0, fy, image_height / 2],
[0, 0, 1]])
point_3d = np.mat([[point_in_world.x], [point_in_world.y],
[point_in_world.z], [1.0]])
point_3d_vehicle = (RT * point_3d)[:-1, :]
'''
point_2d = camMat * (RT * point_3d)[:-1, :]
x = int(point_2d[0, 0] / point_2d[2, 0])
y = int(point_2d[1, 0] / point_2d[2, 0])
'''
camera_height_offset = 1.1
camera_x = -point_3d_vehicle[1]
camera_y = -(point_3d_vehicle[2] - camera_height_offset)
camera_z = point_3d_vehicle[0]
# apply focal length and offsets
# top left
left_x = int((camera_x - corner_offset) * fx / camera_z) + x_offset
top_y = int((camera_y - corner_offset) * fy / camera_z) + y_offset
# bottom right
right_x = int((camera_x + corner_offset) * fx / camera_z) + x_offset
bottom_y = int((camera_y + corner_offset) * fy / camera_z) + y_offset
return (left_x, top_y, right_x, bottom_y)
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
x0, y0, x1, y1 = self.project_to_image_plane(light.pose.pose.position)
h, w, _ = cv_image.shape
if (x0 == x1 or y0 == y1 or x0 < 0 or x1 < 0 or y0 < 0 or y1 < 0
or x0 > w or x1 > w or y0 > h or y1 > h):
return TrafficLight.UNKNOWN
im_light = cv_image[y0:y1, x0:x1, :]
'''
#write image to file
base_dir = '/home/marv/data/tl/'
current_time_str = str(int(time.time()*1000))
if (light.state == TrafficLight.RED):
img_save_path = base_dir + 'red/udacity_img' + current_time_str + '.jpg'
elif (light.state == TrafficLight.GREEN):
img_save_path = base_dir + 'green/udacity_img' + current_time_str + '.jpg'
elif (light.state == TrafficLight.YELLOW):
img_save_path = base_dir + 'yellow/udacity_img' + current_time_str + '.jpg'
else:
img_save_path = base_dir + 'unknown/udacity_img' + current_time_str + '.jpg'
if DEBUG:
cv2.imwrite(img_save_path,im_light)
'''
# Get classification
tl_class = self.light_classifier.get_classification(im_light)
return tl_class
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
light_wp = None
stop_pose = Pose()
visible = False
# 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)
if DEBUG:
pass
#print "Car position is: ", car_position
# TODO find the closest visible traffic light (if one exists)
for light_i in self.lights:
light_pos_i = np.array([
light_i.pose.pose.position.x, light_i.pose.pose.position.y
])
min_dist = 1000000
for stop_pos_j in stop_line_positions:
stop_pos_j = np.array(stop_pos_j)
dist = np.sum((stop_pos_j - light_pos_i)**2)
if dist < min_dist:
min_dist = dist
stop_pose.position.x = stop_pos_j[0]
stop_pose.position.y = stop_pos_j[1]
light_wp_i = self.get_closest_waypoint(stop_pose)
if light_wp_i >= car_position and light_wp_i != -1:
if light_wp is None or light_wp_i < light_wp:
light_wp = light_wp_i
light = light_i
# if light_wp is close car_position, then light will be visible
if light_wp is not None and light_wp - car_position < 100:
visible = True
if light is not None and visible:
#determine image latency
image_time = self.camera_image.header.stamp
light_time = light.header.stamp
#print ("image latency is: ", (light_time-image_time).to_sec())
#get state
state = self.get_light_state(light)
print "Inferred Traffic Light state is: ", state
#KR testing
#state = light.state
#print("Correct TL State is: ", light.state)
return light_wp, state
#Why are we setting self.waypoints to None?
#self.waypoints = None
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoints_2d = None
self.waypoint_tree = 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.stop_line_positions = self.config['stop_line_positions']
self.isSite = self.config['is_site']
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier(self.isSite)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.traffic_lights_2d = None
self.traffic_light_tree = None
self.light_dict = {0: 'Red', 1: 'Yellow', 2: 'Green', 4: 'Unknown'}
self.mapped_category = {
1: {
'id': 2,
'name': 'Green'
},
2: {
'id': 0,
'name': 'Red'
},
3: {
'id': 1,
'name': 'Yellow'
},
4: {
'id': 3,
'name': 'Unknown'
}
}
#rospy.spin()
self.loop()
def loop(self):
rate = rospy.Rate(1)
while not rospy.is_shutdown():
rate.sleep()
def distance(self, ax, ay, bx, by, az=0, bz=0):
dist = math.sqrt((ax - bx)**2 + (ay - by)**2 + (az - bz)**2)
return dist
#Function to check closest traffic light
def get_closest_traffic_light_idx(self):
#X and Y coordinate from Current Position of vehicle
x = self.pose.pose.position.x
y = self.pose.pose.position.y
#Query the tree to fond the closest waypoint to the vehicle position
closest_idx = self.traffic_light_tree.query([x, y], 1)[1]
#Check if the vehicle is ahead or behind
closest_cord = self.traffic_lights_2d[closest_idx]
previous_cord = self.traffic_lights_2d[closest_idx -
1] # One paypoint behind -1
#Convert to array to create a hyperplane
closest_vector = np.array(closest_cord)
previous_vector = np.array(previous_cord)
position_vector = np.array([x, y])
dot_product = np.dot(closest_vector - previous_vector,
position_vector - closest_vector)
if dot_product > 0:
closest_idx = (closest_idx + 1) % len(self.traffic_lights_2d)
return closest_idx
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
self.waypoint_tree = KDTree(self.waypoints_2d)
def traffic_cb(self, msg):
self.lights = msg.lights
if not self.traffic_lights_2d: # If not initialized
#Convert to a list with 2D coordinates X and Y only
self.traffic_lights_2d = [[
light.pose.pose.position.x, light.pose.pose.position.y
] for light in self.lights]
self.traffic_light_tree = KDTree(self.traffic_lights_2d)
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
#Collect Training Data
if COLLECTING_TRAINING_DATA:
traffic_light_id = self.get_closest_traffic_light_idx()
x1 = self.pose.pose.position.x
y1 = self.pose.pose.position.y
x2 = self.lights[traffic_light_id].pose.pose.position.x
y2 = self.lights[traffic_light_id].pose.pose.position.y
dist = self.distance(x1, y1, x2, y2)
light_color = self.light_dict[self.lights[traffic_light_id].state]
global num
if dist < 50:
save_image = self.bridge.imgmsg_to_cv2(self.camera_image,
"bgr8")
save_name = out_path + light_color + '%04d.png' % num
cv2.imwrite(save_name, save_image)
num = num + 1
global count
#Training Data End
light_wp, state = self.process_traffic_lights()
count = 0
'''
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 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, current_x, current_y):
"""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
"""
closest_idx = self.waypoint_tree.query([current_x, current_y], k=1)[1]
coords = self.waypoints_2d[closest_idx]
prev_coords = self.waypoints_2d[closest_idx - 1]
cl_vector = np.array(coords)
prev_vector = np.array(prev_coords)
pos_vector = np.array([current_x, current_y])
val = np.dot(cl_vector - prev_vector, pos_vector - cl_vector)
if val > 0:
closest_idx = (closest_idx + 1) % len(self.waypoints_2d)
return closest_idx
def get_light_state(self, light):
"""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 USING_INTERNAL_LIGHT_STATE:
state = self.lights[light].state
return state
if (not self.has_image):
self.prev_light_loc = None
state = self.lights[light].state
return state
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
classified_state = self.light_classifier.get_classification(cv_image)
return self.mapped_category[classified_state]['id']
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)
"""
# List of positions that correspond to the line to stop in front of for a given intersection
if (self.pose):
x = self.pose.pose.position.x
y = self.pose.pose.position.y
car_position = self.get_closest_waypoint(x, y)
#TODO find the closest visible traffic light (if one exists)
traffic_light_id = self.get_closest_traffic_light_idx()
x1 = self.pose.pose.position.x
y1 = self.pose.pose.position.y
x2 = self.lights[traffic_light_id].pose.pose.position.x
y2 = self.lights[traffic_light_id].pose.pose.position.y
dist = self.distance(x1, y1, x2, y2)
if dist < 200:
stop_line_pose = self.stop_line_positions[traffic_light_id]
closest_wp_idx = self.get_closest_waypoint(stop_line_pose[0],
stop_line_pose[1])
state = self.get_light_state(traffic_light_id)
return closest_wp_idx, state
#self.waypoints = None
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.camera_image = None
self.lights = []
self.base_waypoints = None
self.waypoint_tree = None
self.waypoints_2d = None
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
# initialise flag as classifier is not running
self.classifier_running = False
# variable to hold states return from image classifier
# 1 = Green, 2 = Red, 3 = Yellow, 4 = Unknown
self.classified_light_state = 4
# wait until subscribers are ready
rospy.wait_for_message('/current_pose', PoseStamped)
rospy.wait_for_message('/base_waypoints', Lane)
# subscribe to get current localised position of vehicle
sub1 = rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=1)
# subscriber to get base waypoints of track
sub2 = rospy.Subscriber('/base_waypoints',
Lane,
self.waypoints_cb,
queue_size=1)
# get position of all traffic lights in map world
sub3 = rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray,
self.traffic_cb,
queue_size=1)
# get image to detect and classify traffic light states
sub6 = rospy.Subscriber('/image_color',
Image,
self.image_cb,
queue_size=1)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
# create a publish topic for traffic lights
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
# flag to indicate classifier is running
self.classifier_ready = rospy.Publisher('/classifier_status',
Bool,
queue_size=1)
# create a terminate thread
self.image_ready = threading.Event()
# create a lock for thread
self.thread_image_lock = threading.Lock()
# create a thread that will run classification
inception_thread = threading.Thread(target=self.get_classification)
# start thread
inception_thread.start()
# initiate main program loop
self.run_main()
rospy.spin()
# -----------------------------------------------------------------------------------------------------
def get_classification(self):
while not rospy.is_shutdown():
# check to see if image is available for processing
if not self.image_ready.wait(1):
continue
# lock the image for retrieval
self.thread_image_lock.acquire()
# get image
image = self.camera_image
# release lock
self.thread_image_lock.release()
# prepare image
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
image_width = self.camera_image.width
image_height = self.camera_image.height
# load image into numpy arrray
np_image = np.array(cv_image).reshape(image_height, image_width,
3).astype(np.uint8)
# update latest light_state by get classification from net model
self.classified_light_state = self.light_classifier.get_classification(
np_image)
# set flag to say classifer is running
self.classifier_running = True
# clear flag for image processing
self.image_ready.clear()
# if classifer is not running set flag
self.classifier_running = False
# -----------------------------------------------------------------------------------------------------
def run_main(self):
# set loop to 10Hz - traffic light positions can be updated faster tan images
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# wait until all states are ready
if self.pose and self.base_waypoints and self.waypoint_tree:
# publish light message when not using classifier
light_wp, state = self.process_traffic_lights()
self.classifier_ready.publish(self.classifier_running)
# if new state is different to previous state - light has changed
if self.state != state:
# reset state counter
self.state_count = 0
# set state
self.state = state
# if witnessed 3 same state - steady condition so go confidence
elif self.state_count >= STATE_COUNT_THRESHOLD:
# update traffic light state for action
self.last_state = self.state
# set light waypint as stop line if light state is RED or YELLOW
light_wp = light_wp if state == TrafficLight.RED else -1
self.last_wp = light_wp
# publish message with latest traffic light state
self.upcoming_red_light_pub.publish(Int32(light_wp))
# if state is same and threshol is not yet reached - keep publishing previous light state
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
# increment state counter as same state witnessed again
self.state_count += 1
rate.sleep()
# -----------------------------------------------------------------------------------------------------
# callback for current_pose msg
def pose_cb(self, msg):
self.pose = msg
# -----------------------------------------------------------------------------------------------------
# callback for base_waypoints msg - load base map to memory
def waypoints_cb(self, waypoints):
# store waypoints in the object for continued use
# latched subscriber - base waypoints never change so should only be stored once
self.base_waypoints = waypoints
# if waypoints_2d is empty, then populate with received track points
if not self.waypoints_2d:
# convert waypoints to 2D coordinates for each waypoint using list comprehension
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
# populate the waypoint tree variable with the points passed from
self.waypoint_tree = KDTree(np.array(self.waypoints_2d))
# -----------------------------------------------------------------------------------------------------
# callback to store traffic light positions
def traffic_cb(self, msg):
# get details from simulator about stop lines positions - for uing simulator state info
self.lights = msg.lights
# -----------------------------------------------------------------------------------------------------
# Callback which runs classifier if traffic light position is in range of vehicle
def image_cb(self, msg):
# lock the image variable to update with image received
self.thread_image_lock.acquire()
# pass image from msg into variable
self.camera_image = msg
# release lock
self.thread_image_lock.release()
# set flag to say image is ready
self.image_ready.set()
# -----------------------------------------------------------------------------------------------------
# function to find closest waypoint index for traffic light
def get_closest_waypoint(self, x, y):
# load passed in co-ordinates into numpy array
target_coordinate = np.array([x, y])
# find closest waypoint index from waypoint tree
closest_idx = self.waypoint_tree.query(target_coordinate, 1)[1]
# return waypoint index
return closest_idx
# -----------------------------------------------------------------------------------------------------
def process_traffic_lights(self):
# Finds closest visible traffic light, if one exists, and determines its location and colour
# initialise closest light to None
closest_light = None
# initialise line waypoint index to None
line_wp_idx = None
# List of positions that correspond to the line to stop in front of for a given intersection
# get stop line positions from config file
stop_line_positions = self.config['stop_line_positions']
# if pose of ego is known
if (self.pose and self.base_waypoints and self.waypoint_tree):
# find nearest waypoint index from current car_position
car_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
# find the closest visible traffic light (if one exists)
# iterate through the whole list of traffic lights
diff = len(self.base_waypoints.waypoints)
# update waypoint information in lights
for i, light in enumerate(self.lights):
# get stop line waypoint index corresponding to this light loop
line = stop_line_positions[i]
# get closest waypoint feeding in x and y value of stop line
temp_wp_idx = self.get_closest_waypoint(line[0], line[1])
# find closest stop line waypoint index - store number of indicies between car position and ligh position
d = temp_wp_idx - car_wp_idx
# if number of indicies is >0 - so in front of car and not exceeding number of points to search
if d >= 0 and d < diff:
# set closest light, stop line
# metres to stop line - greater than 10 looks like car is passed
diff = d
# xyz positon coordinates of closest light and zw orientation
closest_light = light
# waypoint index of traffic light stop line
line_wp_idx = temp_wp_idx
if closest_light:
# grab state of traffic light after getting closest light
# call get latest light status to update state
state = self.get_latest_light_state()
# return light position index and state
return line_wp_idx, state
# if no traffic light detected or unknown state
return -1, TrafficLight.UNKNOWN
# -----------------------------------------------------------------------------------------------------
def get_latest_light_state(self):
# get the latest light state into variable
light_status = self.classified_light_state
# decide on stop or go
if (light_status == TrafficLight.RED) or (light_status
== TrafficLight.YELLOW):
return TrafficLight.RED
elif (light_status == TrafficLight.GREEN):
return TrafficLight.GREEN
return TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector', log_level=rospy.DEBUG)
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.current_closest_wp_index = None
self.wp_direction = None
self.min_wp_index = 0
self.max_wp_index = 10901
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.bridge = CvBridge()
# self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.previous_state = TrafficLight.UNKNOWN # xg: code review this.
self.last_wp = -1
self.state_count = 0
self.light_classifier = TLClassifier(model_path, PATH_TO_LABELS)
img_full_np = self.light_classifier.load_image_into_numpy_array(
np.zeros((800, 600, 3)))
self.light_classifier.get_classification(img_full_np)
self.base_timer = time.time()
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()
# print "light_wp: " , light_wp, " state: " , state
'''
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 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
"""
min_dist = np.inf
min_index = -1
if self.waypoints is not None:
for index, wp in enumerate(self.waypoints.waypoints):
x2 = np.power(wp.pose.pose.position.x - pose.position.x, 2)
y2 = np.power(wp.pose.pose.position.y - pose.position.y, 2)
dist = np.sqrt(x2 + y2)
if dist < min_dist:
min_dist = dist
min_index = index
return min_index
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
# testing the detection result:
# based on the test, gpu performance for one iamges is about 1 seconds,
# add some buffer here to avoid the images flood to the classfication
if time.time() > self.base_timer + 0.2:
state, d_time = self.light_classifier.get_classification(cv_image)
self.previous_state = state
rospy.logdebug("Detection Result: %s, time: %s" % (state, d_time))
self.base_timer = time.time()
return state
else:
return self.previous_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
state = 4 # unknown
# 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_wp_index = self.get_closest_waypoint(self.pose.pose)
if self.current_closest_wp_index is not None:
if car_wp_index == self.min_wp_index and self.current_closest_wp_index == self.maz_wp_index:
self.wp_direction = "increasing"
elif car_wp_index == self.max_wp_index and self.current_closest_wp_index == self.min_wp_index:
self.wp_direction = "decreasing"
elif car_wp_index > self.current_closest_wp_index:
self.wp_direction = "increasing"
elif car_wp_index < self.current_closest_wp_index:
self.wp_direction = "decreasing"
self.current_closest_wp_index = car_wp_index
#TODO find the closest visible traffic light (if one exists)
min_dist = np.inf
min_index = -1
line_wp_index = -1
for index, slp in enumerate(stop_line_positions):
slp_pose = Pose()
slp_pose.position.x = slp[0]
slp_pose.position.y = slp[1]
x2 = np.power(slp[0] - self.pose.pose.position.x, 2)
y2 = np.power(slp[1] - self.pose.pose.position.y, 2)
dist = np.sqrt(x2 + y2)
if dist < min_dist:
line_wp_index = self.get_closest_waypoint(slp_pose)
if ((line_wp_index >= car_wp_index
and self.wp_direction == "increasing") or
(line_wp_index <= car_wp_index
and self.wp_direction == "decreasing")) and dist < 120:
min_dist = dist
min_index = index
light = self.lights[min_index]
if light:
# This line should be uncommented once the "get_light_state" is ready
state = self.get_light_state(light)
# state = light.state
return line_wp_index, state
return -1, TrafficLight.UNKNOWN
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.camera_image = None
self.lights = []
self.base_waypoints = None
self.waypoint_tree = None
self.waypoints_2d = None
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
# initialise flag as classifier is not running
self.classifier_running = False
# variable to hold states return from image classifier
# 1 = Green, 2 = Red, 3 = Yellow, 4 = Unknown
self.classified_light_state = 4
# wait until subscribers are ready
rospy.wait_for_message('/current_pose', PoseStamped)
rospy.wait_for_message('/base_waypoints', Lane)
# subscribe to get current localised position of vehicle
sub1 = rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=1)
# subscriber to get base waypoints of track
sub2 = rospy.Subscriber('/base_waypoints',
Lane,
self.waypoints_cb,
queue_size=1)
# get position of all traffic lights in map world
sub3 = rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray,
self.traffic_cb,
queue_size=1)
# get image to detect and classify traffic light states
sub6 = rospy.Subscriber('/image_color',
Image,
self.image_cb,
queue_size=1)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
# create a publish topic for traffic lights
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
# flag to indicate classifier is running
self.classifier_ready = rospy.Publisher('/classifier_status',
Bool,
queue_size=1)
# create a terminate thread
self.image_ready = threading.Event()
# create a lock for thread
self.thread_image_lock = threading.Lock()
# create a thread that will run classification
inception_thread = threading.Thread(target=self.get_classification)
# start thread
inception_thread.start()
# initiate main program loop
self.run_main()
rospy.spin()
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.camera_image = None
self.lights = []
self.car_position = -1
self.waypoints_2d = None
self.stop_line_idxs = []
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.light_classifier = TLClassifier(self.config['is_site'])
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.has_image = False
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)
rospy.Subscriber('/vehicle/car_position', Int32, self.car_position_cb)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
waypoint_tree = KDTree(self.waypoints_2d)
stop_line_positions = self.config['stop_line_positions']
for i, stop_line in enumerate(stop_line_positions):
# Get stop line waypoint index
idx = waypoint_tree.query([stop_line[0], stop_line[1]], 1)[1]
self.stop_line_idxs.append(idx)
self.stop_line_idxs.sort()
if self.config['is_site']:
self.stop_line_idxs[0] -= 3
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()
# cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
# cv2.imwrite("./images/" + '{:04}'.format(msg.header.seq) + ".jpg", cv_image)
'''
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 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 car_position_cb(self, msg):
self.car_position = msg.data
if len(self.stop_line_idxs) == 0:
return
if self.car_position > self.stop_line_idxs[0]:
if not self.config['is_site']:
self.stop_line_idxs = self.stop_line_idxs[1:]
def get_light_state(self):
"""Determines the current color of the traffic light
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
# return light.state
if not self.has_image:
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
#Get classification
return self.light_classifier.get_classification(cv_image)
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)
"""
check_light_state = False
light_wp = None
if self.pose and len(self.stop_line_idxs) > 0:
# find the closest visible traffic light (if one exists)
closest_stop_line_index = self.stop_line_idxs[0]
d = closest_stop_line_index - self.car_position
if 0 <= d < TL_DETCTION_LOOKAHEAD_WPS:
check_light_state = True
light_wp = closest_stop_line_index
if check_light_state:
state = self.get_light_state()
return light_wp, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector', log_level=rospy.INFO)
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
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.
'''
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
rospy.Subscriber('/image_color', Image, self.image_cb)
# same as present in site_traffic_light_config and sim_trafficlight_config
config_string = rospy.get_param("/traffic_light_config")
rospy.logwarn(NODE_NAME + "traffic_light_config: %s", config_string)
self.config = yaml.load(config_string)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
if USE_CV_CLASSIFIER:
self.light_classifier = TLClassifier()
else:
curr_dir = os.path.dirname(os.path.realpath(__file__))
self.debug("Curr directory is " + curr_dir)
labels_path = curr_dir + "/light_classification/models/label_map.pbtxt"
IS_SIMULATOR = rospy.get_param('~is_simulator')
if IS_SIMULATOR:
self.debug("Running in simulator")
model_path = curr_dir + "/light_classification/models/sim/frozen_inference_graph.pb"
else:
self.debug("Running on site")
model_path = curr_dir + "/light_classification/models/real/frozen_inference_graph.pb"
self.light_classifier = TLClassifier(model_path, labels_path)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rate = rospy.Rate(10)
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 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, waypoints):
"""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
"""
# DONE implement
closest_dist = MAX_DISTANCE
closest_waypoint = 0
for i in range(len(waypoints)):
dist = self.euclidean_distance(pose.position.x, pose.position.y,
waypoints[i].pose.pose.position.x,
waypoints[i].pose.pose.position.y)
if dist < closest_dist:
closest_dist = dist
closest_waypoint = i
return closest_waypoint
@staticmethod
def euclidean_distance(x1, y1, x2, y2):
dx = x2 - x1
dy = y2 - y1
return math.sqrt(dx * dx + dy * dy)
@staticmethod
def euclidean_distance_between_pose(pose1, pose2):
return TLDetector.euclidean_distance(pose1.position.x,
pose1.position.y,
pose2.position.x,
pose2.position.y)
# https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
@staticmethod
def get_geometrically_close_point(waypoints, pose):
closest_waypoint_distance = MAX_DISTANCE
closest_waypoint_index = -1
for i in range(0, len(waypoints)):
pose1 = waypoints[i].pose.pose
waypoint_distance = TLDetector.euclidean_distance_between_pose(
pose1, pose)
if waypoint_distance <= closest_waypoint_distance:
closest_waypoint_distance = waypoint_distance
closest_waypoint_index = i
return closest_waypoint_index
@staticmethod
def get_pose_from_line(x, y):
pose = PoseStamped()
pose.pose.position.x = x
pose.pose.position.y = y
return pose
def debug(self, message):
rospy.logdebug(NODE_NAME + message)
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
# Get classification
return self.light_classifier.get_classification(cv_image)
def get_closest_tl(self):
return TLDetector.get_geometrically_close_point(
self.lights, self.pose.pose)
def get_closest_light(self, waypoints):
closest_light_idx = -1
closest_wp_idx = self.get_closest_waypoint(self.pose.pose,
self.waypoint.waypoint)
closest_tl_idx = self.get_closest_tl()
# check if closest TL is not already passed, otherwise we need to pick the next light
closest_tl = self.lights[closest_tl_idx]
closest_wp = self.waypoints[closest_wp_idx]
diff_wpx = closest_wp.pose.pose.position.x - self.pose.pose.position.x
diff_wpy = closest_wp.pose.pose.position.y - self.pose.pose.position.y
diff_tlx = closest_tl.pose.pose.position.x - self.pose.pose.position.x
diff_tly = closest_tl.pose.pose.position.y - self.pose.pose.position.y
dot_product = diff_wpx * diff_tlx + diff_wpy * diff_tly
# dot product is less means light is behind car
if dot_product < 0:
if closest_light_idx is not len(self.lights) - 1:
closest_light_idx = closest_light_idx + 1
return closest_light_idx
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
min_dist = float("inf")
# 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,
self.waypoints.waypoints)
k = -1
for i in range(len(stop_line_positions)):
current_light = self.get_pose_from_line(
stop_line_positions[i][0], stop_line_positions[i][1])
light_waypoint = self.get_closest_waypoint(
current_light.pose, self.waypoints.waypoints)
car_dist = self.euclidean_distance(
self.waypoints.waypoints[car_position].pose.pose.position.
x, self.waypoints.waypoints[car_position].pose.pose.
position.y, self.waypoints.waypoints[light_waypoint].pose.
pose.position.x, self.waypoints.waypoints[light_waypoint].
pose.pose.position.x)
if car_dist < min_dist and (
light_waypoint - car_position >
0) and (light_waypoint - car_position < 90): # 125
light = current_light
light_wp = light_waypoint
k = i
if light:
# state = self.lights[k].state
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.light = None
self.last_pose = None
self.last_light_wp = None
sub1 = rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=QUEUE_SIZE)
sub2 = rospy.Subscriber('/base_waypoints',
Lane,
self.waypoints_cb,
queue_size=QUEUE_SIZE)
'''
/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,
queue_size=QUEUE_SIZE)
sub6 = rospy.Subscriber('/image_color',
Image,
self.image_cb,
queue_size=QUEUE_SIZE)
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.bridge = CvBridge()
self.on_sim = True if rospy.get_param('~on_sim') == 1 else False
self.light_classifier = TLClassifier(self.on_sim)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = 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 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
pos_x = pose.position.x
pos_y = pose.position.y
# iterate over waypoints to find the next one (i.e. closest in front)
min_dist = LARGE
min_idx = 0
for idx, wp in enumerate(self.waypoints):
wp_x = wp.pose.pose.position.x
wp_y = wp.pose.pose.position.y
if euc_dist(wp_x, wp_y, pos_x, pos_y) < min_dist:
min_dist = euc_dist(wp_x, wp_y, pos_x, pos_y)
min_idx = idx
return min_idx
def get_closest_light(self, car_pose, light_waypoints):
"""Identifies the closest position between the given car position and traffic light waypoints
"""
min_idx = LARGE
for pos_x, pos_y in light_waypoints:
light_pose = Pose()
light_pose.position.x = pos_x
light_pose.position.y = pos_y
light_wp = self.get_closest_waypoint(light_pose)
# Check if the wp (idx) is ahead of car position
# and the wp is closer than the min_idx
if (light_wp >= car_pose and light_wp < min_idx):
min_idx = light_wp
return min_idx
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
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)
"""
# img_time = self.camera_image.header.stamp
# img_age = rospy.Time.now() - img_time
# rospy.loginfo('(%s), (%s)', rospy.Time.now().secs, self.camera_image.header.stamp.secs)
#if (img_age.secs > 0.2):
# pass
# rospy.loginfo('Image age (%s) is greater than threhsold (%s)', img_age.secs, 0.2)
# return -1, TrafficLight.UNKNOWN
# 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']
# Don't update when car is not moving
if (self.pose and self.waypoints):
if (self.last_pose and self.pose.pose != self.last_pose.pose):
car_wp = self.get_closest_waypoint(self.pose.pose)
light_wp = self.get_closest_light(car_wp, stop_line_positions)
# rospy.loginfo("Car position: (%s) Closest waypoint idx: (%s) Closest light waypoint idx: (%s)",
# self.pose.pose.position, car_wp, light_wp)
# If the next traffic light is 100 waypoints ahead of current car position
self.light = light_wp - car_wp <= LOOKAHEAD_WPS
else:
light_wp = self.last_light_wp
self.last_pose = self.pose
self.last_light_wp = light_wp
#TODO find the closest visible traffic light (if one exists)
if self.light:
state = self.get_light_state(self.light)
#rospy.loginfo("CLASSIFIER Light state near waypoint (%s) is (%s)", light_wp, state)
return light_wp, state
# self.waypoints = None
return None, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.car_pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.traffic_positions = tf_helper.get_given_traffic_lights()
self.last_traffic_light_state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.last_reported_traffic_light_id = None
self.last_reported_traffic_light_time = None
self.traffic_lights = None
self.image = None
'''
/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.
'''
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.bridge = CvBridge()
self.experiment_environment = rospy.get_param('/experiment_environment', "site")
self.light_classifier = TLClassifier(self.experiment_environment)
# self.light_classifier = TLClassifierCV()
self.listener = tf.TransformListener()
rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb, queue_size=1)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb, queue_size=1)
rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray, self.traffic_cb, queue_size=1)
rospy.Subscriber('/image_color', Image, self.image_cb, queue_size=1)
self.upcoming_stop_light_pub = rospy.Publisher(
'/upcoming_stop_light_position', geometry_msgs.msg.Point, queue_size=1)
rospy.spin()
def pose_cb(self, msg):
self.car_pose = msg.pose
# For debugging(Ground Truth data)
# arguments = [self.traffic_lights, self.car_pose, self.waypoints, self.image]
arguments = [self.traffic_positions, self.car_pose, self.waypoints, self.image]
are_arguments_available = all([x is not None for x in arguments])
if are_arguments_available:
# Get closest traffic light
traffic_light = tf_helper.get_closest_traffic_light_ahead_of_car(
self.traffic_positions.lights, self.car_pose.position, self.waypoints)
# These values seem so be wrong - Udacity keeps on putting in config different values that what camera
# actually publishes.
# image_width = self.config["camera_info"]["image_width"]
# image_height = self.config["camera_info"]["image_height"]
# Therefore simply check image size
self.camera_image = self.image
self.camera_image.encoding = "rgb8"
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
traffic_light_state = self.light_classifier.get_classification(cv_image)
# lights_map = {0: "Red", 1: "Yellow", 2: "Green"}
# rospy.logwarn("Detected light: {}".format(lights_map.get(traffic_light_state, "Other")))
if traffic_light_state == TrafficLight.RED or traffic_light == TrafficLight.YELLOW:
self.upcoming_stop_light_pub.publish(traffic_light.pose.pose.position)
def waypoints_cb(self, lane):
self.waypoints = lane.waypoints
def traffic_cb(self, msg):
self.traffic_lights = msg.lights
def image_cb(self, msg):
self.image = msg
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.waypoints_2d = None
self.waypoint_tree = None
self.process_ctr = True
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.bridge = CvBridge()
self.light_classifier = TLClassifier(self.config["is_site"])
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
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)
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
self.waypoint_tree = KDTree(self.waypoints_2d)
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
#additional logic to skip every alternate images
if self.process_ctr < 2:
self.process_ctr += 1
return
else:
self.process_ctr = 0
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
if state == TrafficLight.RED or state == TrafficLight.UNKNOWN:
#When a red light is known or a unclassified light is obtained
light_wp = light_wp
else:
#Green light case
light_wp = -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_state_x, pose_state_y):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose_state_x (Pose): position to match a waypoint to in X
pose_state_y: position to match a waypoint to in Y
Returns:
int: index of the closest waypoint in self.waypoints
"""
x = pose_state_x
y = pose_state_y
#finde the closest waypoint's idx
closest_idx = self.waypoint_tree.query([x, y], 1)[1]
# check if closest is ahead or behind car
closest_waypoint = self.waypoints_2d[closest_idx]
pre_closest_waypoint = self.waypoints_2d[closest_idx - 1]
cl_vect = np.array(closest_waypoint)
pre_vect = np.array(pre_closest_waypoint)
pos_vect = np.array([x, y])
if np.dot(cl_vect - pre_vect, pos_vect - cl_vect) > 0:
closest_idx = (closest_idx + 1) % len(self.waypoints_2d)
return closest_idx
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
#Get classification
return self.light_classifier.get_classification(cv_image)
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
light_wp = None
min_dist = None
car_position_wp = None
stop_line_pos_for_light = None
nearest_wp_to_stopline = None
wp_delta_stopline_car_position = None
# 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_wp = self.get_closest_waypoint(
self.pose.pose.position.x, self.pose.pose.position.y)
#find the closest visible traffic light (if one exists)
min_dist = len(self.waypoints.waypoints)
for i in range(len(self.lights)):
stop_line_pos_for_light = stop_line_positions[i]
nearest_wp_to_stopline = self.get_closest_waypoint(
stop_line_pos_for_light[0], stop_line_pos_for_light[1])
wp_delta_stopline_car_position = nearest_wp_to_stopline - car_position_wp
if wp_delta_stopline_car_position >= 0 and wp_delta_stopline_car_position < min_dist:
min_dist = wp_delta_stopline_car_position
light = self.lights[i]
light_wp = nearest_wp_to_stopline
#Skip inference if traffic light is Cutoff distance away from current pose
if light and (min_dist <= STOP_LINE_CURRENT_POSE_DIST_CUTOFF):
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
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)
self.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.
'''
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
sub6 = rospy.Subscriber('/image_color',
Image,
self.image_cb,
queue_size=1)
rospy.spin()
###########################################################################################################################
def pose_cb(self, msg):
self.pose = msg.pose
###########################################################################################################################
def waypoints_cb(self, waypoints):
self.waypoints = waypoints.waypoints
self.waypoints_array = np.asarray([(w.pose.pose.position.x,
w.pose.pose.position.y)
for w in waypoints.waypoints])
#rospy.loginfo('waypoints {} = {}'.format(self.waypoints_array.shape, self.waypoints_array))
if rospy.get_param('/unregister_base_waypoints', False):
self.sub2.unregister()
rospy.loginfo('base_waypoints subscriber unregistered')
###########################################################################################################################
def traffic_cb(self, msg):
self.lights = msg.lights
###########################################################################################################################
def image_cb(self, msg):
global ProcessingTimeSum, ProcessingIterations
"""Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
"""
if MEASURE_PERFORMANCE:
startTime = time.time()
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 >= rospy.get_param('~state_count_threshold', 3):
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED 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
if MEASURE_PERFORMANCE:
endTime = time.time()
duration = endTime - startTime
ProcessingTimeSum += duration
ProcessingIterations += 1
rospy.loginfo("Processing time of image_cb(): " + str(duration) +
" average: " +
str(ProcessingTimeSum / ProcessingIterations))
###########################################################################################################################
def get_closest_waypoint(self, position_x, position_y):
"""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
"""
index = -1
#Return if way points are empty
if self.waypoints is None:
return index
position = np.asarray([position_x, position_y])
dist_squared = np.sum((self.waypoints_array - position)**2, axis=1)
index = np.argmin(dist_squared)
#rospy.loginfo('tl.detector.get_closest_waypoint({}) found at = {}, distance = {}, time = {}'.format(
# position, index, np.sqrt(dist_squared[index]), time.time() - t))
return index
###########################################################################################################################
def euclidianDistance(self, x1, y1, x2, y2):
return math.sqrt((x1 - x2)**2 + (y1 - y2)**2)
###########################################################################################################################
def project_to_image_plane(self, point_in_world_x, point_in_world_y,
point_in_world_z):
"""Project point from 3D world coordinates to 2D camera image location
Args:
point_in_world (Point): 3D location of a point in the world
Returns:
x (int): x coordinate of target point in image
y (int): y coordinate of target point in image
"""
fx = self.config['camera_info']['focal_length_x']
fy = self.config['camera_info']['focal_length_y']
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
# get transform between pose of camera and world frame
trans = None
rot = None
try:
now = rospy.Time.now()
self.listener.waitForTransform("/base_link", "/world", now,
rospy.Duration(1.0))
(trans,
rot) = self.listener.lookupTransform("/base_link", "/world", now)
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logerr("Failed to find camera to map transform")
if trans == None or rot == None:
return False, -1, -1
#Use tranform and rotation to calculate 2D position of light in image
point_in_world_z = 1 # Does not work with correct z value
world_point = np.array(
[point_in_world_x, point_in_world_y,
point_in_world_z]).reshape(1, 3, 1)
camera_mat = np.matrix([[fx, 0, image_width / 2],
[0, fy, image_height / 2], [0, 0, 1]])
distCoeff = None
rot_vec, _ = cv2.Rodrigues(self.QuaterniontoRotationMatrix(
rot)) # 4x1 -> quaternion to rotation matrix at z-axis
ret, _ = cv2.projectPoints(world_point, rot_vec,
np.array(trans).reshape(3, 1), camera_mat,
distCoeff)
#Unpack values and return
ret = ret.reshape(2, )
#For some reason u & v are swapped
u = int(round(ret[1]))
v = int(round(ret[0]))
traffic_light_visible = False
if u >= 0 and u < image_width and v >= 0 and v <= image_height:
traffic_light_visible = True
return traffic_light_visible, u, v
###########################################################################################################################
def QuaterniontoRotationMatrix(self, q):
'''Calculates the Rotation Matrix from Quaternion
s is the real part
x, y, z are the complex elements'''
#https://www.uni-koblenz.de/~cg/veranst/ws0001/sem/Bartz.pdf Chap. 1.2.6
x, y, z, s = q
#Precalculate repeatedly used values
x2 = x**2
xy = x * y
xz = x * z
y2 = y**2
yz = y * z
z2 = z**2
sz = s * z
sy = s * y
sx = s * x
#Calculate rotation matrix
R11 = 1 - 2.0 * (y2 + z2)
R12 = 2.0 * (xy - sz)
R13 = 2.0 * (xz + sy)
R21 = 2.0 * (xy + sz)
R22 = 1 - 2.0 * (x2 + z2)
R23 = 2.0 * (yz - sx)
R31 = 2.0 * (xz - sy)
R32 = 2.0 * (yz + sx)
R33 = 1 - 2.0 * (x2 + y2)
return np.matrix([[R11, R12, R13], [R21, R22, R23], [R31, R32, R33]])
###########################################################################################################################
def get_light_state(self, light_pos_x, light_pos_y, light_pos_z):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
visible, x, y = self.project_to_image_plane(light_pos_x, light_pos_y,
light_pos_z)
#Show car image
if DISPLAY_CAMERA:
image_tmp = np.copy(cv_image)
#Draw a circle
cv2.circle(image_tmp, (x, y), 20, (255, 0, 0), thickness=2)
cv2.imshow('image', image_tmp)
cv2.waitKey(1)
state = TrafficLight.UNKNOWN
if visible:
#TODO use light location to zoom in on traffic light in image
#Get classification
state = self.light_classifier.get_classification(cv_image)
#Return state
return state
###########################################################################################################################
def get_nearest_traffic_light(self, waypoint_start_index):
traffic_light = None
traffic_light_positions = self.config['light_positions']
#traffic_light_positions = self.config['manual_light_positions']
last_index = sys.maxsize
#TODO: Only one complete circle, no minimum distance considered, yet
for i in range(0, len(traffic_light_positions)):
index = self.get_closest_waypoint(
float(traffic_light_positions[i][0]),
float(traffic_light_positions[i][1]))
if index > waypoint_start_index and index < last_index:
last_index = index
traffic_light = traffic_light_positions[i]
return traffic_light, last_index
###########################################################################################################################
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 traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
if (self.pose):
#find the closest visible traffic light (if one exists)
car_position = self.get_closest_waypoint(self.pose.position.x,
self.pose.position.y)
light_pos = None
if car_position > 0:
light_pos, light_waypoint = self.get_nearest_traffic_light(
car_position)
if light_pos:
#rospy.loginfo("Next traffic light ahead from waypoint " + str(car_position) +
# " is at position " + str(light_pos) + " at waypoint " + str(light_waypoint))
state = TrafficLight.UNKNOWN
if rospy.get_param('~use_classifier', False):
state = self.get_light_state(
light_pos[0], light_pos[1],
light_pos[2] if len(light_pos) >= 3 else 0.)
else:
for light in self.lights:
''' If position of the light from the yaml file and one roperted via
/vehicle/traffic_lights differs only within 30 m consider them as same '''
if self.euclidianDistance(
light.pose.pose.position.x,
light.pose.pose.position.y, light_pos[0],
light_pos[1]) < 30:
#state = self.get_light_state(light.pose.pose.position.x, light.pose.pose.position.y, light.pose.pose.position.z)
state = light.state
return light_waypoint, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
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.bridge = CvBridge()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.waypoints_2d = None
self.waypoints_tree = None
self.image_count = 0
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)
self.light_classifier = TLClassifier({v: k for (k, v) in LIGHT_LABELS.items()})
# Wait with subscribing for imge_color until classifier is ready
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
# this strikes me as an exceedingly ugly thing to do in a constructor ...
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
self.waypoints_2d = [(waypoint.pose.pose.position.x, waypoint.pose.pose.position.y) for waypoint in
waypoints.waypoints]
self.waypoints_tree = KDTree(self.waypoints_2d)
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.camera_image = msg
light_wp, state = self.process_traffic_lights()
# rospy.logwarn("Closest light wp: {0} and light state: {1}".format(light_wp, state))
'''
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 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, x, y):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
x (float): x position
y (float): y position
Returns:
int: index of the closest waypoint in self.waypoints
"""
return self.waypoints_tree.query((x, y), 1)[1]
def get_light_state(self, light):
"""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(not self.camera_image):
return self.last_state
if IMAGE_HANDLING_MODE == "classify":
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
return self.light_classifier.get_classification(cv_image)
# otherwise return the light state from the simulator
return light.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)
"""
closest_light = None
line_wp_idx = None
# 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_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x, self.pose.pose.position.y)
# 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_wp_idx
if 0 <= d < diff:
diff = d
closest_light = light
line_wp_idx = temp_wp_idx
self.image_count += 1
if IMAGE_HANDLING_MODE == "collect" and self.image_count % 20 == 0 and self.camera_image:
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
if diff < 150:
label = LIGHT_LABELS[light.state]
else:
label = LIGHT_LABELS[TrafficLight.UNKNOWN]
timestamp = time.time()
filename = '{IMAGE_DIR}/{label}/{timestamp}.jpg'.format(
IMAGE_DIR=IMAGE_DIR,
label=label,
timestamp=timestamp)
cv2.imwrite(filename, cv_image)
if closest_light:
state = self.get_light_state(closest_light)
return line_wp_idx, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.waypoints_2d = None
self.waypoints_tree = None
self.last_img_process_time = 0 # initialize before image_cb
self.light_ahead = None
#/current_pose can be used used to determine the vehicle's location.
#/base_waypoints provides the complete list of waypoints for the course.
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.
'''
#data sent from simulator - message has RGY and location, good for testing
#should this topic not be used when the NN classifier is used and tested for real vehicle
# udacity simulator gives position and state
# Carla - real world testing, only position is given by this topic, need to get state from NN
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
#image_raw - translate to new color scheme will lose data
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
#/vehicle/traffic_lights provides the (x, y, z) coordinates of all traffic lights.
#/image_color which provides an image stream from the car's camera. These images are used to determine the color of upcoming traffic lights.
#The permanent (x, y) coordinates for each traffic light's stop line are provided by the config dictionary
# traffic_light_config is parameter in ros/launch/site.launch and ros/launch/styx.launch
#ros/launch/styx.launch - ros/src/tl_detector/sim_traffic_light_config.yaml - is_site: true
#ros/launch/site.launch - ros/src/tl_detector/site_traffic_light_config.yaml - is_site: false
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
#self.is_site = self.config["is_site"]
#print("is_site %d" % self.is_site)
#The node should publish the index of the waypoint for nearest upcoming red light's stop line to a single topic:
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
#publish if there is an upcoming traffic signal, so that twist_controller can reduce throttle
#self.upcoming_traffic_light_pub = rospy.Publisher('/traffic_light_ahead', Int32, queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
if not self.waypoints_2d: ##?? is this required?
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
self.waypoints_tree = KDTree(self.waypoints_2d) #??
def traffic_cb(self, msg):
self.lights = msg.lights
#code to publish the results of process_traffic_lights is written for you already in the image_cb method.
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
"""
if (self.waypoints_tree == None):
return
#''' #start TEST 2
if (self.is_traffic_light_ahead()
): # read image, run classifier, publish to waypoint updater
rospy.logwarn(
"tl_detector.image_cb: YES light within {0} waypoints".format(
WAYPOINT_DIFFERENCE))
#self.upcoming_traffic_light_pub.publish(Int32(1))
pass
else: #dont read image, and dont call classifier, return from here
#rospy.logwarn("tl_detector.image_cb: NO light within {0} waypoints".format(WAYPOINT_DIFFERENCE))
#rospy.logwarn("tl_detector.image_cb: NO light within 300 waypoints self.last_wp: {0}".format(self.last_wp))
self.upcoming_red_light_pub.publish(Int32(-1))
#self.upcoming_traffic_light_pub.publish(Int32(-1))
# publishing self.last_wp will make car stuck when TL are too close
# wait for some time before publishing -1 as waypoint updater may not pick up this signal
# it needs some time to start decelerating
return
#''' #end TEST 2
#''' #start TEST 1
#time_elapsed = timer() - self.last_img_process_time
##rospy.loginfo ("image_cb: time_elapsed {}".format(time_elapsed))
##Do not process the camera image unless 20 milliseconds have passed from last processing
#if (time_elapsed < CAMERA_IMG_PROCESS_INT):
# return;
#instead of above CAMERA_IMG_PROCESS_INT logic, use the flag from process_traffic lights which is more accurate
#that will reduce the time here but can mess up near the light, need to decelerate at that stage
#''' #end TEST 1
self.has_image = True
self.camera_image = msg
#rospy.logwarn("Got Image... ")
#rospy.logwarn("self.pose is not None: {0} ".format(self.pose is not None))
#rospy.logwarn("self.waypoints is not None: {0} ".format(self.waypoints is not None))
#rospy.logwarn("self.camera_image is not None: {0} ".format(self.camera_image is not None))
self.last_img_process_time = timer()
light_wp, state = self.process_traffic_lights(
) #whenever we get an image this cb is called
rospy.logwarn("tl_detector - light_wp: {0} ".format(light_wp))
rospy.logwarn("tl_detector - RED(state=0) state: {0}".format(state))
#rospy.logwarn("tl_detector - self.state: {0}".format(self.state))
#rospy.logwarn("tl_detector - self.state_count: {0}".format(self.state_count))
'''
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.
'''
#stored prev state of light, for every image, determine if its state has changed and count
#can also update code if light is yellow
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD: #threshold = 3
#classifier may be noisy, make sure light is going to stay before taking action
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED else -1
#for publishing location of light, only RED state is important
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
#rospy.logwarn("tl_detector - publish RED light_wp: {0}".format(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp)) #
#rospy.logwarn("tl_detector - publish self.last_wp: {0}".format(self.last_wp))
self.state_count += 1
def get_closest_waypoint(self, x, y): #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
closest_idx = self.waypoints_tree.query(
[x, y], 1)[1] #KDTree to get closest index
return closest_idx
#Use the camera image data to classify the color of the traffic light.
#train a deep learning classifier to classify the entire image as containing either a red light, yellow light, green light, or no light.
#One resource that's available to you is the traffic light's position in 3D space via the vehicle/traffic_lights topic.
def get_light_state(self, light):
"""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)
"""
#when running in the simulator during testing, the light state is available, no classifier is needed
#return light.state
#'''
if (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
#'''
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)
"""
closest_light = None
line_wp_idx = None
#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 and self.waypoints_tree):
car_wp_idx = self.get_closest_waypoint(
self.pose.pose.position.x,
self.pose.pose.position.y) #using KDTree
#TODO find the closest visible traffic light (if one exists)
#Need to get index of closet traffic line, from the light info
diff = len(self.waypoints.waypoints) #num of waypoints
for i, light in enumerate(
self.lights
): #iterate through traffic lights - 8 intersections, not using KDTree for small list
#Get stop line way point index
line = stop_line_positions[i] #Get the line of traffic light
temp_wp_idx = self.get_closest_waypoint(line[0], line[1])
#Find closest stop line way point index
d = temp_wp_idx - car_wp_idx # num of indices between closest traffic line wp and car position
#rospy.logwarn("tl_detector - process_traffic_lights d: {0}".format(d))
#rospy.logwarn("tl_detector - process_traffic_lights diff: {0}".format(diff))
if d > 0 and d < diff: #waypoint is ahead of car and < num of waypoints
diff = d
closest_light = light
line_wp_idx = temp_wp_idx
#at this point we have closest light and index of the waypoint closest to the light
# do not process the camera image unless the traffic light <= 300 waypoints
#if closest_light and ((line_wp_idx - car_wp_idx) <= WAYPOINT_DIFFERENCE):
if closest_light:
state = self.get_light_state(closest_light)
return line_wp_idx, state
return -1, TrafficLight.UNKNOWN #For UNKNOWN, keep car going can still test dbw system, reality need to stop the car
#''' #start new code to reduce processing time by reading the image only when the car approaches the light
def is_traffic_light_ahead(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)
"""
closest_light = None
line_wp_idx = None
self.light_ahead = False
#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 and self.waypoints_tree):
car_wp_idx = self.get_closest_waypoint(
self.pose.pose.position.x,
self.pose.pose.position.y) #using KDTree
#TODO find the closest visible traffic light (if one exists)
#Need to get index of closet traffic line, from the light info
diff = len(self.waypoints.waypoints) #num of waypoints
for i, light in enumerate(
self.lights
): #iterate through traffic lights - 8 intersections, not using KDTree for small list
#Get stop line way point index
line = stop_line_positions[i] #Get the line of traffic light
temp_wp_idx = self.get_closest_waypoint(line[0], line[1])
#Find closest stop line way point index
d = temp_wp_idx - car_wp_idx # num of indices between closest traffic line wp and car position
if d > 0 and d < diff: #waypoint is ahead of car and < num of waypoints
diff = d
closest_light = light
line_wp_idx = temp_wp_idx
#at this point we have closest light and index of the waypoint closest to the light
# do not process the camera image unless the traffic light <= 300 waypoints
if closest_light and (
(line_wp_idx - car_wp_idx) <= WAYPOINT_DIFFERENCE):
self.light_ahead = True
#rospy.logwarn("tl_detector light ahead line_wp_idx: {0}".format(line_wp_idx))
#rospy.logwarn("tl_detector light ahead car_wp_idx : {0}".format(car_wp_idx))
return self.light_ahead
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.last_car_position = []
self.last_light_pos_wp = []
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)
camera_string = rospy.get_param("/camera_topic")
sub6 = rospy.Subscriber(camera_string, 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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
stop_line_positions = self.config['stop_line_positions']
print(stop_line_positions)
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = 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 light_wp == -1:
self.state_count = 0
return
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
self.last_wp = light_wp
if state == TrafficLight.GREEN:
light_wp = len(self.waypoints) + 1
self.upcoming_red_light_pub.publish(Int32(light_wp))
self.state_count += 1
def distance(self, pos1, pos2):
x = pos1.x - pos2.x
y = pos1.y - pos2.y
#z = pos1.z - pos2.z
return math.sqrt(x * x + y * y)
def distance_light(self, pos1, pos2):
x = pos1[0] - pos2.x
y = pos1[1] - pos2.y
#z = pos1.z - pos2.z
return math.sqrt(x * x + y * y)
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
closest_waypoint_dist = 100000
closest_waypoint_ind = -1
#Use loop to find closest one, based on https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
for i in range(0, len(self.waypoints)):
waypoint_distance = self.distance(
self.waypoints[i].pose.pose.position, pose.position)
if waypoint_distance <= closest_waypoint_dist:
closest_waypoint_dist = waypoint_distance
closest_waypoint_ind = i
return closest_waypoint_ind
def get_closest_waypoint_light(self, way_point, light_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
"""
closest_waypoint_dist = 100000
closest_waypoint_ind = -1
#Use loop to find closest one, based on https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
for i, point in enumerate(way_point):
waypoint_distance = self.distance_light(light_pose,
point.pose.pose.position)
if waypoint_distance <= closest_waypoint_dist:
closest_waypoint_dist = waypoint_distance
closest_waypoint_ind = i
return closest_waypoint_ind
def get_light_state(self, light):
"""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 (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
#x, y = self.project_to_image_plane(light.pose.pose.position)
#TODO use light location to zoom in on traffic light in image
#Get classification
return self.light_classifier.get_classification(cv_image)
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
# 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']
light_waypoint_pos = []
if self.waypoints is None:
return -1, TrafficLight.UNKNOWN
for i in range(len(stop_line_positions)):
light_pos = self.get_closest_waypoint_light(
self.waypoints, stop_line_positions[i])
light_waypoint_pos.append(light_pos)
print("light pos", light_pos)
self.last_light_pos_wp = light_waypoint_pos
if (self.pose):
car_position = self.get_closest_waypoint(self.pose.pose)
if car_position is not None:
self.last_car_position = car_position
else:
return -1, TrafficLight.UNKNOWN
if self.last_car_position > max(self.last_light_pos_wp):
light_num_wp = min(self.last_light_pos_wp)
else:
light_delta = self.last_light_pos_wp[:]
light_delta[:] = [x - self.last_car_position for x in light_delta]
light_num_wp = min(
i for i in light_delta if i >= 0) + self.last_car_position
light_ind = self.last_light_pos_wp.index(light_num_wp)
light = stop_line_positions[light_ind]
print("Last car pos ", self.last_car_position)
light_distance = self.distance_light(
light, self.waypoints[self.last_car_position].pose.pose.position)
print("Distance to light --- ", light_distance)
search_for_light_distance = 5
if light:
if light_distance >= search_for_light_distance:
return -1, TrafficLight.UNKNOWN
else:
state = self.get_light_state(light)
return light_num_wp, state
return -1, TrafficLight.UNKNOWN
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.waypoints_2d = None
self.waypoints_tree = None
self.last_img_process_time = 0 # initialize before image_cb
self.light_ahead = None
#/current_pose can be used used to determine the vehicle's location.
#/base_waypoints provides the complete list of waypoints for the course.
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.
'''
#data sent from simulator - message has RGY and location, good for testing
#should this topic not be used when the NN classifier is used and tested for real vehicle
# udacity simulator gives position and state
# Carla - real world testing, only position is given by this topic, need to get state from NN
sub3 = rospy.Subscriber('/vehicle/traffic_lights', TrafficLightArray,
self.traffic_cb)
#image_raw - translate to new color scheme will lose data
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
#/vehicle/traffic_lights provides the (x, y, z) coordinates of all traffic lights.
#/image_color which provides an image stream from the car's camera. These images are used to determine the color of upcoming traffic lights.
#The permanent (x, y) coordinates for each traffic light's stop line are provided by the config dictionary
# traffic_light_config is parameter in ros/launch/site.launch and ros/launch/styx.launch
#ros/launch/styx.launch - ros/src/tl_detector/sim_traffic_light_config.yaml - is_site: true
#ros/launch/site.launch - ros/src/tl_detector/site_traffic_light_config.yaml - is_site: false
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
#self.is_site = self.config["is_site"]
#print("is_site %d" % self.is_site)
#The node should publish the index of the waypoint for nearest upcoming red light's stop line to a single topic:
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
#publish if there is an upcoming traffic signal, so that twist_controller can reduce throttle
#self.upcoming_traffic_light_pub = rospy.Publisher('/traffic_light_ahead', Int32, queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.spin()
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.bridge = CvBridge()
graph = self.config['model']['graph']
labels = self.config['model']['labels']
self.light_classifier = TLClassifier(graph, labels)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, msg):
self.waypoints = msg.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.GREEN 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, x,y):
"""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
if self.waypoints is None:
return
# Find index of closest waypoint
closestwp = float("inf")
targetIndex = 0
for index, waypoint in enumerate(self.waypoints):
dist = self.distanceCalculation(x,waypoint.pose.pose.position.x,y,waypoint.pose.pose.position.y)
if dist < closestwp:
closestwp = dist
targetIndex = index
return targetIndex
# Calculate distance
def distanceCalculation(self,a1,a2,b1,b2):
return math.sqrt((a1-a2)**2 + (b1-b2)**2)
def get_light_state(self, light):
"""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(not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
#Get classification
lightState = self.light_classifier.get_classification(cv_image)
return lightState
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)
"""
if self.waypoints is None:
return -1, TrafficLight.UNKNOWN
light = None
light_wp = None
# 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.position.x, self.pose.pose.position.y)
#TODO Find closest traffic light
closestwp = float('inf')
for lightIndex, stopLine in enumerate(stop_line_positions):
lineIndex = self.get_closest_waypoint(stopLine[0], stopLine[1])
if (abs(lineIndex-car_position) < Trajectory) and (abs(lineIndex-car_position) < closestwp) and (lineIndex > car_position):
light = self.lights[lightIndex]
light_wp = lineIndex
if light:
state = self.get_light_state(light)
rospy.loginfo("Herby Light Status: %s", lightState[state])
return light_wp, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
# Variables
self.pose = None
self.base_waypoints = None
self.waypoints_2d = None
self.waypoint_tree = None
self.camera_image = None
self.camera_image_is_raw = False
self.has_image_color = False
self.lights = []
self.waypoints_2d = None
self.waypoint_tree = None
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.log_count = 0
# load the x,y coordinates of each traffic light from config
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
running_on_site = self.config['is_site']
self.light_classifier = TLClassifier(running_on_site)
# all initializations should happen before the subscriptions, otherwise the callbacks are
# hitting uninitialized / missing members
# Subscribers
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_color', Image, self.image_cb_color)
sub7 = rospy.Subscriber('/image_raw', Image, self.image_cb_raw)
# Publishers
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.base_waypoints = waypoints
if not self.waypoints_2d:
self.waypoints_2d = [[
waypoint.pose.pose.position.x, waypoint.pose.pose.position.y
] for waypoint in waypoints.waypoints]
# Build a KD Tree to speed up searching for waypoints in the future
self.waypoint_tree = spatial.KDTree(self.waypoints_2d)
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
"""
# Checks for image from camera
self.has_image = True
self.camera_image = msg
#rospy.loginfo("image raw %d seq %d", self.camera_image_is_raw, self.camera_image.header.seq)
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.
'''
# Check state of light
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 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 image_cb_color(self, msg):
self.has_image_color = True
self.camera_image_is_raw = False
self.image_cb(msg)
def image_cb_raw(self, msg):
if self.has_image_color == False:
self.camera_image_is_raw = True
self.image_cb(msg)
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
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.base_waypoints
"""
# Find the closest waypoint
if self.waypoint_tree is None:
return -1
closest_idx = self.waypoint_tree.query([x, y], 1)[1]
return closest_idx
def get_light_state(self, light):
"""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)
"""
# For testing, just return the light state from simulator
if (SIMULATED_LIGHTS):
return light.state
if (not self.has_image):
self.prev_light_loc = None
return TrafficLight.UNKNOWN
cv_image = None
if (self.camera_image_is_raw == False):
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
else:
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image,
"bayer_grbg8")
cv_image = cv2.cvtColor(
cv_image, cv2.COLOR_BayerGB2BGR
) # cv2 name GB is from "bayer_grbg"[-1:-2]
#rospy.loginfo("raw image seq %d size %d,%d", self.camera_image.header.seq, cv_image.shape[0], cv_image.shape[1])
# Get classification
result = self.light_classifier.get_classification(cv_image)
# Write annotated image for testing
write_annotated_image = False
if (self.camera_image_is_raw == True
and write_annotated_image == True):
color = (0, 0, 0)
if result == TrafficLight.GREEN:
color = (0, 255, 0)
elif result == TrafficLight.YELLOW:
color = (255, 255, 0)
elif result == TrafficLight.RED:
color = (255, 0, 0)
dbgimg = cv2.circle(cv_image, (20, 20), 10, color, -1)
cv2.imwrite('/home/student/shared/ros_image/raw_%04d.png' %
self.camera_image.header.seq,
dbgimg) #, cv2.cvtColor(dbgimg, cv2.COLOR_RGB2BGR))
return result
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)
"""
closest_light = None
stop_line_wp_idx = -1
state = TrafficLight.UNKNOWN
# 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.position.x,
self.pose.pose.position.y)
if car_position != -1:
# Find the closest visible traffic light (if one exists)
diff = len(self.base_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
if d >= 0 and d < diff:
diff = d
closest_light = light
stop_line_wp_idx = temp_wp_idx
if closest_light:
self.log_count += 1
state = self.get_light_state(closest_light)
if (self.log_count % LOGGING_RATE) == 0:
rospy.logwarn(
"DETECT: stop_line_wp_idx={}, state={}, car_position={}".
format(stop_line_wp_idx, self.state_to_string(state),
car_position))
#self.base_waypoints = None
return stop_line_wp_idx, state
def state_to_string(self, state):
out = "unknown"
if state == TrafficLight.GREEN:
out = "green"
elif state == TrafficLight.YELLOW:
out = "yellow"
elif state == TrafficLight.RED:
out = "red"
return out
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None # current car pose
self.waypoints = None # all waypoints
self.camera_image = None
self.lights = [] # All Traffic lights info
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_raw', Image, self.image_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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.waypoints_2d = None
self.waypoint_tree = None
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
if not self.waypoints_2d:
self.waypoints_2d = [[
wp.pose.pose.position.x, wp.pose.pose.position.y
] for wp in waypoints.waypoints]
self.waypoint_tree = KDTree(self.waypoints_2d)
if (len(self.waypoints_2d) == 0):
rospy.loginfo("Empty Waypoints")
else:
rospy.loginfo("Received 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
"""
#rospy.loginfo("image_cb")
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.
'''
#rospy.loginfo("image_cb : light_wp %s",light_wp);
#rospy.loginfo("image_cb : state %s", state);
if self.state != state:
self.state_count = 0
self.state = state
elif (self.state_count >= STATE_COUNT_THRESHOLD) and (self.last_state
!= self.state):
if (state == TrafficLight.RED):
rospy.loginfo("Red Light Decteded!!")
else:
rospy.loginfo("Non Red Light Decteded!!")
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED else -1
self.last_wp = light_wp
self.publish_stop_line_waypoint(Int32(light_wp))
else:
self.publish_stop_line_waypoint(Int32(self.last_wp))
self.state_count += 1
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
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
#DONE implement
closest_waypoint_idx = self.waypoint_tree.query([x, y], 1)[1]
return closest_waypoint_idx
def get_light_state(self, light):
"""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)
"""
#rospy.loginfo("get_light_state")
if not (self.config['tl']['is_carla']):
#rospy.loginfo("get_light_state : Simulator detected")
return light.state
else:
if (not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
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)
"""
#rospy.loginfo("process_traffic_lights")
closest_light = None
closest_line_wp_idx = None
# 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_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
#DONE : find the closest visible traffic light (if one exists)
closest_line_dist = len(self.waypoints.waypoints)
for i, light in enumerate(self.lights):
cur_stop_line_pos = stop_line_positions[i]
cur_stop_line_wp_idx = self.get_closest_waypoint(
cur_stop_line_pos[0], cur_stop_line_pos[1])
# initializing closest light distance
cur_line_dist = cur_stop_line_wp_idx - car_wp_idx
# Checking If stop line is in front of the car (dist>0)
#rospy.logwarn(" tl_detector : i: %s cur_stop_line_pos: %s ,cur_stop_line_wp_idx : %s ,closest_line_dist : %s ,cur_line_dist: %s", i,cur_stop_line_pos,
# cur_stop_line_wp_idx, closest_line_dist, cur_line_dist);
if 0 <= cur_line_dist < closest_line_dist:
closest_line_dist = cur_line_dist
closest_light = light
closest_line_wp_idx = cur_stop_line_wp_idx
if closest_light:
state = self.get_light_state(light)
return closest_line_wp_idx, state
rospy.loginfo("process_traffic_lights : No nearby light detected")
#rospy.loginfo("process_traffic_lights : current pose of the car %s",self.pose);
return -1, TrafficLight.UNKNOWN
def publish_stop_line_waypoint(self, stop_line_wp):
self.upcoming_red_light_pub.publish(stop_line_wp)
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.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
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 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
return 0
def get_light_state(self, light):
"""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(not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
return self.light_classifier.get_classification(cv_image)
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
# 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)
if light:
state = self.get_light_state(light)
return light_wp, state
self.waypoints = None
return -1, TrafficLight.UNKNOWN
