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")
print('Light State: ', light.state)
# 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.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')
self.last_busy = 0
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.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
# Generate a list of stop line points from given configuration
stop_line_positions = self.config['stop_line_positions']
self.stop_line_points = [
self.make_point(p[0], p[1], 0) for p in stop_line_positions
]
self.light_classifier.get_classification(np.zeros((800, 600, 3)))
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 make_point(self, x, y, z):
point = Point()
point.x = x
point.y = y
point.z = z
return point
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
"""
if len(self.lights) < 1:
print("Got image, ignoring because no lights are known.")
return False
if self.waypoints is None:
print("Got image, ignoring because no waypoints are known.")
return False
# Skip frames received within 70ms of the previous so we do not saturate the pipeline
# images take ~50-100ms to process
if time.time() - self.last_busy < 70 / 1000:
print("Busy, skipping frame.")
return False
else:
print("Processing image")
self.last_busy = 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 >= 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:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
if self.waypoints is None:
return 0
waypoints = [
waypoint.pose.pose.position for waypoint in self.waypoints
]
index, _ = self.get_closest_point(position, waypoints)
return index
def get_closest_point(self, needle, list_of_points):
"""Identifies the closest point in a list 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 point in list_of_points
pose: closest point
"""
# Brute-force solution is fine here since N is small
minimum_distance = 9999999
closest_point = 0
i = 0
for point in list_of_points:
dist = self.distance_between_points(point, needle)
if dist < minimum_distance:
minimum_distance = dist
closest_point = i
i = i + 1
return closest_point, list_of_points[closest_point]
def get_closest_traffic_signal(self, point):
"""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
"""
light_poses = [light.pose.pose.position for light in self.lights]
index, _ = self.get_closest_point(point, light_poses)
return index
def distance_between_points(self, point1, point2):
"""Returns the Euclidean distance between two points"""
return math.sqrt(
math.pow(point1.x - point2.x, 2) +
math.pow(point1.y - point2.y, 2) +
math.pow(point1.z - point2.z, 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")
# Classify using the TF light classifier if not available (from simulator)
if light.state != TrafficLight.UNKNOWN and light.state != 3:
print("Using provided state: " + str(light.state))
return light.state
else:
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.pose):
car_position = self.get_closest_waypoint(self.pose.pose.position)
closest_light_index = self.get_closest_traffic_signal(
self.waypoints[car_position].pose.pose.position)
light = self.lights[closest_light_index]
if light:
state = self.get_light_state(light)
_, stopline = self.get_closest_point(light.pose.pose.position,
self.stop_line_points)
stopline_waypoint_index = self.get_closest_waypoint(stopline)
return stopline_waypoint_index, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoint_tree = None
self.waypoints_2d = None
self.camera_image = None
self.lights = []
self.closest_light = None
self.camera_info = None
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)
sub7 = rospy.Subscriber('/camera_info', CameraInfo, self.camera_cb)
# get camera info
#calib_yaml = rospy.get_param("/grasshopper_calibration_yaml")
#self.camera_info = yaml_to_CameraInfo(calib_yaml)
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.TLDetector_pub = rospy.Publisher('/traffic_light_detector',
Image,
queue_size=1)
self.most_confident_detection = rospy.Publisher(
'/traffic_light_most_confident', 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.image_counter = 0
self.detection_graph = None
self.category_index = None
self.tensor_dict = {}
self.image_tensor = None
self.sess = None
self.loadModel()
self.detections = None
self.hasDetections = False
rate = rospy.Rate(5) # Drop rate to 5 Hz to handle latency
rospy.spin()
#Leading pretrained model
def loadModel(self):
rospy.loginfo("Tensorflow version: %s", tensorflow.__version__)
MODEL_NAME = 'ssd_mobilenet_v1_coco_2017_11_17'
PATH_TO_FROZEN_GRAPH = MODEL_NAME + '/frozen_inference_graph.pb'
PATH_TO_LABELS = os.path.join('data', 'mscoco_label_map.pbtxt')
self.detection_graph = tensorflow.Graph()
with self.detection_graph.as_default():
od_graph_def = tensorflow.GraphDef()
with tensorflow.gfile.GFile(PATH_TO_FROZEN_GRAPH, 'rb') as fid:
serialized_graph = fid.read()
od_graph_def.ParseFromString(serialized_graph)
tensorflow.import_graph_def(od_graph_def, name='')
#category_index = label_map_util.create_category_index_from_labelmap(PATH_TO_LABELS, use_display_name=True)
self.category_index = {1: "traffic lights"}
rospy.loginfo("Detection graph loaded")
rospy.loginfo("Initializing Sesson")
with self.detection_graph.as_default():
self.sess = tensorflow.Session()
# Get handles to input and output tensors
ops = tensorflow.get_default_graph().get_operations()
all_tensor_names = {
output.name
for op in ops for output in op.outputs
}
self.tensor_dict = {}
for key in [
'num_detections', 'detection_boxes', 'detection_scores',
'detection_classes', 'detection_masks'
]:
tensor_name = key + ':0'
if tensor_name in all_tensor_names:
self.tensor_dict[key] = tensorflow.get_default_graph(
).get_tensor_by_name(tensor_name)
if 'detection_masks' in self.tensor_dict:
# The following processing is only for single image
detection_boxes = tensorflow.squeeze(
self.tensor_dict['detection_boxes'], [0])
detection_masks = tensorflow.squeeze(
self.tensor_dict['detection_masks'], [0])
# Reframe is required to translate mask from box coordinates to image coordinates and fit the image size.
real_num_detection = tensorflow.cast(
self.tensor_dict['num_detections'][0], tensorflow.int32)
detection_boxes = tensorflow.slice(detection_boxes, [0, 0],
[real_num_detection, -1])
detection_masks = tensorflow.slice(
detection_masks, [0, 0, 0], [real_num_detection, -1, -1])
detection_masks_reframed = utils_ops.reframe_box_masks_to_image_masks(
detection_masks, detection_boxes, image.shape[1],
image.shape[2])
detection_masks_reframed = tensorflow.cast(
tf.greater(detection_masks_reframed, 0.5),
tensorflow.uint8)
# Follow the convention by adding back the batch dimension
self.tensor_dict['detection_masks'] = tensorflow.expand_dims(
detection_masks_reframed, 0)
self.image_tensor = tensorflow.get_default_graph(
).get_tensor_by_name('image_tensor:0')
rospy.loginfo("Sesson Initialized")
def getBBox(self):
#rospy.loginfo("Getting bboxes")
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image,
desired_encoding="passthrough")
image_np = np.asarray(cv_image)
image = np.expand_dims(image_np, axis=0)
# Run inference
output_dict = self.sess.run(self.tensor_dict,
feed_dict={self.image_tensor: image})
# all outputs are float32 numpy arrays, so convert types as appropriate
output_dict['num_detections'] = int(output_dict['num_detections'][0])
output_dict['detection_classes'] = output_dict['detection_classes'][
0].astype(np.uint8)
output_dict['detection_boxes'] = output_dict['detection_boxes'][0]
output_dict['detection_scores'] = output_dict['detection_scores'][0]
if 'detection_masks' in output_dict:
output_dict['detection_masks'] = output_dict['detection_masks'][0]
detections = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
# Visualize detected bounding boxes.
num_detections = int(output_dict['num_detections'])
rows = detections.shape[0]
cols = detections.shape[1]
for i in range(num_detections):
bbox = [float(v) for v in output_dict['detection_boxes'][i]]
score = output_dict['detection_scores'][i]
if score > 0.3:
x = bbox[1] * cols
y = bbox[0] * rows
right = bbox[3] * cols
bottom = bbox[2] * rows
cv2.rectangle(detections, (int(x), int(y)),
(int(right), int(bottom)), (125, 255, 51),
thickness=2)
image_message = self.bridge.cv2_to_imgmsg(detections,
encoding="passthrough")
self.TLDetector_pub.publish(image_message)
return output_dict
def camera_cb(self, msg):
#print("Got camera info")
self.camera_info = msg
def pose_cb(self, msg):
self.pose = msg
if TESTING_WITHOUT_IMG:
#Remove -- Temporary code - sub for image processing
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
# Remove till here
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
#rospy.loginfo(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
"""
# Only check every 3rd image (reduce latency)
self.image_counter += 1
if self.image_counter % 5 != 0:
return
# End speed-up
self.has_image = True
self.hasDetections = False
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.waypoint_tree.query([x, y], 1)[1]
return closest_idx
def get_sim_light_state(self, light_idx):
"""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)
"""
# Dummy code to be removed later
#rospy.loginfo('Light state: %s', self.lights[light_idx].state)
if (self.lights):
#rospy.loginfo(self.lights[light_idx].state)
return self.lights[light_idx].state
else:
rospy.loginfo('Lights uninit')
return TrafficLight.GREEN
def get_tl_coords_in_image(self, coords_in_world):
"""Get transform from (X,Y,Z) world coords to (x,y) camera coords.
See https://github.com/udacity/CarND-Capstone/issues/24
Args:
coords_in_world : TrafficLight coordinates
"""
self.listener.waitForTransform("/world", "/base_link", rospy.Time(),
rospy.Duration(1.0))
try:
now = rospy.Time.now()
self.listener.waitForTransform("/world", "/base_link", now,
rospy.Duration(1.0))
(trans,
rot) = self.listener.lookupTransform("/world", "/base_link", now)
#print("Got map transform")
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.log_err("Couldn't find camera to map transform.")
#print("Can't get map transform")
# do 3D rotation and translation of light coords from world to car frame
x_world = coords_in_world.x
y_world = coords_in_world.y
z_world = coords_in_world.z
e = tf.transformations.euler_from_quaternion(rot)
cos_yaw = math.cos(e[2])
sin_yaw = math.sin(e[2])
x_car = x_world * cos_yaw - y_world * sin_yaw + trans[0]
y_car = x_world * sin_yaw + y_world * cos_yaw + trans[1]
z_car = z_world + trans[2]
# use camera projection matrix to translate world coords to camera pixel coords
# http://docs.ros.org/melodic/api/sensor_msgs/html/msg/CameraInfo.html
uvw = np.dot(self.camera_info.P, [x_car, y_car, z_car, 1])
camera_x = uvw[0] / uvw[2]
camera_y = uvw[1] / uvw[2]
#focal_length = 2300
#half_image_width = 400
#half_image_height = 300
#x_offset = -30
#y_offset = 340
#half_image_width = 400
#half_image_height = 300
return (camera_x, camera_y)
def get_light_state(self, tl_idx):
"""Determines the current color of the traffic light
Args:
tl_idx (int): index of 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 = cv_image.copy()
num_detections = int(self.detections['num_detections'])
rows = cv_image.shape[0]
cols = cv_image.shape[1]
index = np.argmax(self.detections['detection_scores'])
#rospy.loginfo('most confident detection = ', index)
#rospy.loginfo('max score = ', self.detections['detection_scores'][index])
for i in range(num_detections):
bbox = [float(v) for v in self.detections['detection_boxes'][i]]
score = self.detections['detection_scores'][i]
if score > 0.3:
x = int(bbox[1] * cols)
y = int(bbox[0] * rows)
right = int(bbox[3] * cols)
bottom = int(bbox[2] * rows)
crop_cv_image = cv_image[y:bottom, x:right].copy()
image_message = self.bridge.cv2_to_imgmsg(
crop_cv_image, encoding="passthrough")
self.most_confident_detection.publish(image_message)
return self.light_classifier.get_classification(crop_cv_image)
return TrafficLight.UNKNOWN
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its color
and the best waypoint for stopping.
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)
"""
line = None
max_visible_dist = 100 # units
line_wp_idx = -1
tl_idx = -1
# 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 the closest visible traffic light (if one exists)
diff = len(self.waypoints.waypoints)
for i, temp_closest_stop_position in enumerate(stop_line_positions):
line = stop_line_positions[i]
wp_idx = self.get_closest_waypoint(line[0], line[1])
d = wp_idx - car_position
if d >= 0 and d < diff and d < max_visible_dist:
diff = d
line_wp_idx = wp_idx
tl_idx = i
line = temp_closest_stop_position
# if light and self.closest_light != light:
# rospy.loginfo('TL coming up: %d', diff)
# rospy.loginfo('Light details(%d): %s', tl_idx, light)
# self.closest_light = light
# state = self.get_sim_light_state(tl_idx)
# return line_wp_idx, state
# elif not light and self.closest_light:
# rospy.loginfo('Free.. speed up')
# self.closest_light = None
# return -1, TrafficLight.UNKNOWN
if tl_idx >= 0:
if TESTING_WITHOUT_IMG:
state = self.get_sim_light_state(tl_idx)
return line_wp_idx, state
else:
# Fix this for image processing.
self.detections = self.getBBox()
self.hasDetections = True
state = self.get_light_state(tl_idx)
#print("Light state: {0}".format(state))
return line_wp_idx, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
'''
/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.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.tree = None
self.lights = []
self.img = None
self.has_image = False
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_time = 0
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)
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.last_wp = -1
self.state_count = 0
#rospy.spin()
self.loop()
def loop(self):
rate = rospy.Rate(5) # 5Hz
while not rospy.is_shutdown():
if self.has_image and self.pose:
light_wp, state = self.process_traffic_lights()
if light_wp:
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
rate.sleep()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints.waypoints
self.tree = KDTree([[p.pose.pose.position.x, p.pose.pose.position.y] for p in 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
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
if self.tree is not None:
x, y = pose.position.x, pose.position.y
return self.tree.query([x, y])[1]
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)
"""
#Get classification
img = np.frombuffer(self.camera_image.data, np.uint8).reshape((self.camera_image.height, self.camera_image.width, 3))
img = cv2.resize(img, None, fx = 0.5, fy = 0.5, interpolation = cv2.INTER_AREA)
#img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
#cv2.imwrite('/mnt/c/temp/img.png', img)
self.img = img.reshape((1, self.camera_image.height // 2, self.camera_image.width // 2, 3))
return self.light_classifier.get_classification(self.img)
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 + 1, wp2 + 1):
dist += dl(waypoints[wp1].pose.pose.position, waypoints[i].pose.pose.position)
wp1 = i
return dist
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']
car_position = self.get_closest_waypoint(self.pose.pose)
#TODO find the closest visible traffic light (if one exists)
if not train:
for stop in stop_line_positions:
stop_pose = Pose()
stop_pose.position.x, stop_pose.position.y = stop
light_wp = self.get_closest_waypoint(stop_pose)
if light_wp > car_position and self.distance(self.waypoints, car_position, light_wp) < 70:
light = light_wp
break
else:
for l in self.lights:
light_wp = self.get_closest_waypoint(l.pose.pose)
#rospy.loginfo('Light:{}, Car:{}, max:{}'.format(light_wp, car_position, self.waypoints))
if light_wp > car_position and self.distance(self.waypoints, car_position, light_wp) < 100:
light = light_wp
break
if light:
if not train:
state = self.get_light_state(light)
else:
state = l.state
return light, 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.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.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
# next lines from waypoint_updater for similar KDtree
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
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.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)
"""
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
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 stopline waypoint index
line = stop_line_positions[i]
temp_wp_idx = self.get_closest_waypoint(line[0], line[1])
#Find the closest stopline 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 = closest_light.state
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 = []
'''
/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.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.light_classifier = TLClassifier(
'./tf_files/retrained_graph.pb', './tf_files/retrained_labels.txt')
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.has_waypoints = False
self.has_pose = False
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)
self.save_images_min_dist = rospy.get_param('~save_images_min_dist')
rospy.loginfo('save img min dist: {}'.format(
self.save_images_min_dist))
self.save_images = rospy.get_param('~save_images')
self.use_classifier = rospy.get_param('~use_classifier')
self.max_tl_dist = rospy.get_param('~max_tl_distance')
# Always use classifier
self.use_classifier = True
if self.use_classifier:
rospy.loginfo('using traffic light classifier')
self.has_image = False
self.start()
def pose_cb(self, msg):
self.has_pose = True
self.pose = msg
def waypoints_cb(self, waypoints):
if self.has_waypoints:
return
self.waypoints = waypoints.waypoints
data = np.zeros((len(self.waypoints), 2), dtype=np.float32)
for idx, wp in enumerate(self.waypoints):
xy = (wp.pose.pose.position.x, wp.pose.pose.position.y)
data[idx, :] = xy
self.kdtree = scipy.spatial.KDTree(data)
self.has_waypoints = True
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 publish(self):
if not (self.has_pose and self.has_waypoints):
return
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("tl_detector - publishing state: %s light_wp %s", state,
# light_wp)
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_xy(self, pt):
d, kdwp = self.kdtree.query(pt)
return kdwp
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
"""
d, kdwp = self.kdtree.query((pose.position.x, pose.position.y))
return kdwp
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
rgb = self.bridge.imgmsg_to_cv2(self.camera_image)
if self.use_classifier:
light_state, hsv, thresh_red = self.light_classifier.get_classification(
rgb)
# Save images for visual debugging
# plt.imsave("./data/out-{}-{}.{}.hsv.jpg".format(
# light_state, light.state, self.camera_image.header.seq), hsv)
# plt.imsave("./data/out-{}-{}.{}.thresh_red.jpg".format(
# light_state, light.state, self.camera_image.header.seq),
# thresh_red)
rospy.loginfo('inference: {} / ground truth {}'.format(
light_state, light.state))
else:
rospy.loginfo("use classifier is ", self.use_classifier)
light_state = light.state
if self.save_images:
dist_to_light = self.calculate_distance(self.pose.pose.position.x,
light.pose.pose.position.x,
self.pose.pose.position.y,
light.pose.pose.position.y)
if dist_to_light <= self.save_images_min_dist:
bgr = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR)
cv2.imwrite(
"./data/test/test-{}-{}.{}.jpg".format(
light_state, light.state,
self.camera_image.header.seq), bgr)
return light_state
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
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]
#rospy.loginfo("tl_detector - pose %s %s", self.pose.pose.position, car_position.pose.pose.position)
#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 dist_min < self.max_tl_dist and closest_stop_line_idx >= 0:
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')
wp = self.get_closest_waypoint_xy(
stop_line_positions[closest_stop_line_idx])
if wp > car_position_idx:
light_wp = wp
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
def start(self):
rospy.loginfo("Traffic Light Detector - Starting")
rate = rospy.Rate(LOOP_RATE)
while not rospy.is_shutdown():
if self.has_image:
self.publish()
rate.sleep()
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoint_tree = None
self.waypoints_2d = None
self.camera_image = None
self.lights = []
rospy.loginfo('Creating CvBridge')
self.bridge = CvBridge()
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)
# might want to use image_raw instead for more data
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)
rospy.loginfo(self.config)
if self.config['is_site']:
rospy.loginfo('Loading Site Traffic Light Graph')
graph = self.load_graph(
'./saved_models/carla_traffic_light_classifier.pb')
else:
rospy.loginfo('Loading Simulator Traffic Light Graph')
graph = self.load_graph(
'./saved_models/traffic_light_classifier.pb')
self.light_classifier = TLClassifier(graph)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.last_car_wp = 0
self.state_count = 0
self.img_idx = 0
# model = keras.models.load_model('/capstone/ros/src/tl_detector/traffic_light_classifier.h5')
rospy.spin()
def load_graph(self, frozen_graph_filename):
# Source: https://blog.metaflow.fr/tensorflow-how-to-freeze-a-model-and-serve-it-with-a-python-api-d4f3596b3adc
# We load the protobuf file from the disk and parse it to retrieve the
# unserialized graph_def
with tensorflow.gfile.GFile(frozen_graph_filename, "rb") as f:
graph_def = tensorflow.GraphDef()
graph_def.ParseFromString(f.read())
# Then, we import the graph_def into a new Graph and returns it
with tensorflow.Graph().as_default() as graph:
# The name var will prefix every op/nodes in your graph
# Since we load everything in a new graph, this is not needed
tensorflow.import_graph_def(graph_def, name="prefix")
return graph
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
light_wp, state = self.process_traffic_lights()
# rospy.logwarn("Closest light wp: {0}\nAnd 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
# Stop on red or yellow light just to be safe
light_wp = light_wp if state == TrafficLight.RED or state == TrafficLight.YELLOW else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
def get_closest_waypoint(self, x, y):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
x (Pose position x): x position to match a waypoint to
y (Pose position y): y 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]
return closest_idx
def get_light_state(self, light, line_wp_idx, car_wp_idx):
"""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)
"""
state = TrafficLight.UNKNOWN
if not self.has_image:
self.prev_light_loc = None
return False
# rospy.logwarn("Next line wp: {}, Car Wp: {}".format(line_wp_idx, car_wp_idx))
if line_wp_idx - car_wp_idx < 50 and self.bridge:
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
state = self.light_classifier.get_classification(cv_image)
# ## Code below used to capture training image dataset.
# if self.last_car_wp == 0:
# self.last_car_wp = car_wp_idx
# elif self.last_car_wp != car_wp_idx:
# # rospy.logwarn("car_wp_idx: {}".format(car_wp_idx))
# self.last_car_wp = car_wp_idx
# if line_wp_idx - car_wp_idx < 100:
# cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
# cv2.imwrite('dataset/{}-{}-{}.png'.format(self.img_idx, line_wp_idx, light.state), cv_image)
# rospy.loginfo('Writing image to dataset/{}-{}-{}.png'.format(self.img_idx, line_wp_idx, light.state))
# self.img_idx += 1
# #Get classification
# for testing, just return the light state
rospy.loginfo("State: {}".format(state))
return state
# return light.state
# 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, line_wp_idx,
car_wp_idx)
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 = []
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.pose = None
self.waypoints_2d = None
self.waypoint_tree = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.process_count = 0
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
print("Traffic light classifier created")
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.last_processed_wp = -1
self.last_processed_state = TrafficLight.UNKNOWN
self.currentFrame = 1
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)
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)
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.has_image = True
self.camera_image = msg
if (self.currentFrame % PROCESS_FRAME_RATE == 1):
light_wp, state = self.process_traffic_lights()
self.last_processed_wp = light_wp
self.last_processed_state = state
else:
light_wp = self.last_processed_wp
state = self.last_processed_state
self.currentFrame += 1
rospy.loginfo("TL Detector ----> image processed: waypoint= " + str(light_wp) + ", state=" + str(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 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 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 self.pose:
current_wp=self.get_closest_waypoint(self.pose.pose.position.x, self.pose.pose.position.y)
else:
current_wp=-1
# if we have found a closest light to monitor, then determine the stop line position of this light
if closest_light and line_wp_idx:
state = self.get_light_state(closest_light)
if (self.process_count % LOGGING_THROTTLE_FACTOR) == 0:
rospy.logwarn("DETECT: line_wp_idx={}, current_wp={}, state={}".format(line_wp_idx, current_wp, self.to_string(state)))
rospy.logdebug('Closest light idx: {} \t state: {}'.format(line_wp_idx, 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)
"""
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)
print(classification)
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"
elif state == TrafficLight.UNKNOWN:
out = "unknown"
return out
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.prev_pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.stop_lines = {}
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
self.is_site = rospy.get_param("/launch") == "site"
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.light_classifier = TLClassifier()
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.prev_pose = self.pose
self.pose = msg
def car_moved(self):
if self.prev_pose is None and self.pose is not None:
return True
if self.prev_pose.pose.position.x != self.pose.pose.position.x:
return True
if self.prev_pose.pose.position.y != self.pose.pose.position.y:
return True
return False
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
# List of positions that correspond to the line to stop in front of for a given intersection
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
stop_line_positions = self.config['stop_line_positions']
self.stop_lines = {}
for stop_line in stop_line_positions:
stop_line_pose = Pose()
stop_line_pose.position.x = stop_line[0]
stop_line_pose.position.y = stop_line[1]
line_position = self.get_closest_waypoint(stop_line_pose)
self.stop_lines[line_position] = stop_line_pose
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
if EXTRACT_SITE_IMAGES:
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
filename = os.path.abspath(
"light_classification/site_training_data/4-{}.jpg".format(
rospy.Time.now()))
cv2.imwrite(filename, cv_image)
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 distance_to_waypoint(self, pose1, pose2):
veh_x = pose1.x
veh_y = pose1.y
wp_x = pose2.x
wp_y = pose2.y
dx = veh_x - wp_x
dy = veh_y - wp_y
return math.sqrt(dx * dx + dy * dy)
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
"""
nearest = None
min_dist = float("inf")
for index, waypoint in enumerate(self.waypoints.waypoints):
dist = self.distance_to_waypoint(pose.position,
waypoint.pose.pose.position)
if dist < min_dist:
min_dist = dist
nearest = index
return nearest
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']
cx = image_width / 2
cy = image_height / 2
# get transform between pose of camera and world frame
trans = None
try:
now = rospy.Time.now()
self.listener.waitForTransform("/world", "/base_link", now,
rospy.Duration(1.0))
tl_point = PointStamped()
tl_point.header.frame_id = "/world"
tl_point.header.stamp = now
tl_point.point.x = point_in_world.x
tl_point.point.y = point_in_world.y
tl_point.point.z = point_in_world.z
tl_point = self.listener.transformPoint("/base_link", tl_point)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException) as e:
rospy.logerr(
"Failed to find camera to map transform: {}".format(e))
return None
objectPoints = np.array([
tl_point.point.y / tl_point.point.x,
tl_point.point.z / tl_point.point.x, 1.0
])
################################################################################
# Manually tune focal length and camera coordinate for simulator
# for more details about this issue, please reference
# https://discussions.udacity.com/t/focal-length-wrong/358568/22
if fx < 10:
fx = -2580
fy = -2730
cx = 360
cy = 700
objectPoints[2]
################################################################################
# TODO This can be a class member
cameraMatrix = np.array([[fx, 0, 0], [0, fy, 0], [0, 0, 1]])
imagePoints = cameraMatrix.dot(objectPoints)
x = int(round(imagePoints[0]) + cx)
y = int(round(imagePoints[1]) + cy)
if 10 > x > image_width - 10 or 10 > y > image_height - 10:
return None
# rospy.loginfo("objectpoints: x: {}, y: {}, z: {}".format(tl_point.point.x, tl_point.point.y, tl_point.point.z))
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 light:
return TrafficLight.UNKNOWN
if not self.has_image:
self.prev_light_loc = None
return False
crop_size = self.light_classifier.crop_size
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
if self.is_site:
# remove hood and upper unused sections
cropped = cv_image[280:750, :, :]
cropped = cv2.resize(cropped, (crop_size, crop_size))
else:
xy = self.project_to_image_plane(light.pose.pose.position)
if xy is None:
cropped = cv2.resize(cv_image, (crop_size, crop_size))
else:
x, y = xy
rospy.loginfo("light image loc: {}, {}".format(x, y))
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
half_crop = int(crop_size / 2)
if x > image_width - half_crop:
x_min = image_width - crop_size
x_max = image_width
elif x < half_crop:
x_min = 0
x_max = crop_size
else:
x_min = max(0, x - half_crop)
x_max = min(x_min + crop_size, image_width)
if y > image_height - half_crop:
y_min = image_height - crop_size
y_max = image_height
elif y < half_crop:
y_min = 0
y_max = crop_size
else:
y_min = max(0, y - half_crop)
y_max = min(y_min + crop_size, image_height)
cropped = cv_image[y_min:y_max, x_min:x_max]
pred = self.light_classifier.get_classification(cropped)
rospy.loginfo("Truth: {}, Pred: {}".format(light.state, pred))
if light.state != TrafficLight.UNKNOWN and self.car_moved(
) and COLLECT_TRAINING_DATA:
# Ground truth is known, use it to create training data
filename = os.path.abspath(
"light_classification/training_data/{}-{}.jpg".format(
light.state, rospy.Time.now()))
rospy.loginfo("Car moved, saving new image: {}".format(filename))
cv2.imwrite(filename, cropped)
return pred
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.waypoints or not self.pose:
return -1, TrafficLight.UNKNOWN
light = None
# car_fwd indicates whether car's moving the same direction as the waypoint index increase
car_fwd = False
closest_wp_index = None
waypoints = self.waypoints.waypoints
wp_length = len(waypoints)
if self.pose:
car_orientation = self.pose.pose.orientation
quaternion = (car_orientation.x, car_orientation.y,
car_orientation.z, car_orientation.w)
_, _, yaw = tf.transformations.euler_from_quaternion(quaternion)
car_pose = self.pose.pose
closest_wp_index = self.get_closest_waypoint(car_pose)
wp1 = waypoints[closest_wp_index]
wp2 = waypoints[(closest_wp_index + 1) % wp_length]
fwd_angle = math.atan2(
wp2.pose.pose.position.y - wp1.pose.pose.position.y,
wp2.pose.pose.position.x - wp1.pose.pose.position.x)
if -math.pi / 2.0 < yaw - fwd_angle < math.pi / 2:
car_fwd = True
light, light_wp = None, None
for i in range(wp_length):
inc = 1 if car_fwd else -1
index = (closest_wp_index + i * inc) % wp_length
if index in self.stop_lines:
light_wp = index
stop_line_pose = self.stop_lines[index]
min_dist = float("inf")
# Find associated light
for light_candidate in self.lights:
dist = self.distance_to_waypoint(
light_candidate.pose.pose.position,
stop_line_pose.position)
if dist < min_dist:
light = light_candidate
min_dist = dist
break
if light:
state = TrafficLight.UNKNOWN
if abs(light_wp - closest_wp_index) < 50:
state = self.get_light_state(light)
return light_wp, state
self.waypoints = None
return -1, TrafficLight.UNKNOWN
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.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
#rospy.loginfo('llight waypoints %i',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_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
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)
#rospy.loginfo("wp_updater tl_detector: car wp %i", car_position)
if car_position > 0:
light_pos, light_waypoint = self.get_nearest_traffic_light(
car_position)
#rospy.loginfo("wp_updater tl_detector: light wp %i (%i)", light_waypoint, len(self.waypoints))
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.waypoints_2d = None
self.waypoint_tree = 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.has_image = False
self.process_count = 0
self.light_classifier = TLClassifier(rospy.get_param('~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)
config_string = rospy.get_param(
"/traffic_light_config"
) # Simulator_mode parameter (1== ON, 0==OFF)
self.config = yaml.load(config_string)
# Publish the index of the waypoint where we have to stop
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.loop()
#rospy.spin()
def loop(self):
# Set loop rate at 10Hz
rate = rospy.Rate(10)
# Run until node is shutted down
while not rospy.is_shutdown():
if self.pose and self.waypoints and self.camera_image:
# Process current image from camera
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.logwarn("self state :{} state:{} state_count:{}".format(self.state, state, self.state_count))
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
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
# Setup the Kd Tree which has log(n) complexity
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)
#rospy.logwarn("Updating TL detector 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 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]
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 test mode, just return the light state
if DEBUG_CODE:
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)
return classification
def process_traffic_lights(self):
closest_light = None
line_wp_idx = None
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): # 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 stop line waypoint index
line = stop_line_positions[i]
temp_wp_idx = self.get_closest_waypoint(line[0], line[1])
# Find the 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:
#self.process_count += 1
state = self.get_light_state(closest_light)
#if (self.process_count % 5) == 0:
# rospy.logwarn("DETECT: line_wp_idx={}, state={}".format(line_wp_idx, self.to_string(state)))
return line_wp_idx, state
self.waypoints = None
return -1, TrafficLight.UNKNOWN
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 AsyncPipeline:
"""
Incapsulates traffic light detector and classifier. Performs detection/classification in a dedicated thread.
After thread is started some iterations of classification is spent on graphic card warm up, after which a
message is published to /traffic_classifier_ready topic.
Methods and variables that can be accessed from different threads have "sh" prefix (shared)
"""
def __init__(self):
rospy.loginfo("Initializing traffic light classifier...")
self.object_detector = ObjectDetector()
self.classifier = TLClassifier()
rospy.loginfo("Traffic light classifier is initialized.")
# this top indicates whether we are ready for detection/classification.
self.traffic_classifier_ready_pub = rospy.Publisher(
"/traffic_classifier_ready", Bool, queue_size=1, latch=True)
# locks for getting/setting image and state values
self.image_lock = threading.Lock()
self.state_lock = threading.Lock()
# fields that are shared between threads
# should be accessed only via getters/setters which are using locks
# last camera image
self.sh_image = None
# last acknowledged traffic light state (classified enough times in a row, to make sure it is not noise)
self.sh_acknowledged_state = None
# result of last classification (may differ from acknowledged state)
self.sh_last_classified_state = None
# time of last classification
self.sh_last_classification_time = 0
def start_thread(self):
"""
Starts main classification thread.
:return:
"""
thread = threading.Thread(target=self.classification_thread_fn)
thread.start()
def sh_set_image(self, image):
"""
Sets camera image. Thread safe.
:param image: camera image message
"""
self.image_lock.acquire()
self.sh_image = image
self.image_lock.release()
def sh_get_image(self):
"""
Gets camera image thread safe.
:return: camera image.
"""
self.image_lock.acquire()
image = self.sh_image
self.image_lock.release()
return image
def sh_get_state_info(self):
"""
Returns results of classification. Thread safe.
:return: acknowledged classification result, last classification result, last classification time
"""
self.state_lock.acquire()
acknowledged_state = self.sh_acknowledged_state
last_classified_state = self.sh_last_classified_state
last_classification_time = self.sh_last_classification_time
self.state_lock.release()
return acknowledged_state, last_classified_state, last_classification_time
def sh_set_classification_info(self, last_classified_state,
last_classification_time):
"""
Sets last classification information. Thread safe.
:param last_classified_state: classification result
:param last_classification_time: classification time
"""
self.state_lock.acquire()
self.sh_last_classified_state = last_classified_state
self.sh_last_classification_time = last_classification_time
self.state_lock.release()
def sh_set_state_info(self, acknowledged_state, last_classification_time):
"""
Sets state information
:param acknowledged_state: acknowledged classification state
:param last_classification_time: last classification time
"""
self.state_lock.acquire()
self.sh_acknowledged_state = acknowledged_state
self.sh_last_classified_state = acknowledged_state
self.sh_last_classification_time = last_classification_time
self.state_lock.release()
def classification_thread_fn(self):
"""
Performs classification in a dedicated thread.
"""
# last image that classification was done on
last_image = None
# last classification result
last_state = None
# number of times last classification has the same result in a row
last_state_count = 0
# overall count of classifications
overall_count = 0
while not rospy.is_shutdown():
# 1. Get camera image
image = self.sh_get_image()
# 2. In case image was changed
if last_image != image:
# 3. In case warming up is done send message that the classifier is ready
if overall_count == WARMUP_NUM:
self.traffic_classifier_ready_pub.publish(Bool(True))
rospy.loginfo("Warming up is finished")
elif overall_count < WARMUP_NUM:
rospy.loginfo("Warming up")
# 4. Do classification
start_time = time.time()
state = self.get_light_state(image, overall_count)
classification_time = time.time() - start_time
if last_state != state: # in case state is changed, remember it and reset the counter
last_state_count = 0
last_state = state
self.sh_set_classification_info(state, classification_time)
elif last_state_count >= STATE_COUNT_THRESHOLD: # in case state persists long enough change it
self.sh_set_state_info(last_state, classification_time)
else: # otherwise just save last classificaton info for debugging
self.sh_set_classification_info(state, classification_time)
last_state_count += 1 # increment how many times in a row we got the same state
overall_count += 1 # increment how many times we done classification
last_image = image # memorize last image
def get_light_state(self, image, counter):
"""
Performs detection and then classification.
:param image: ROS image
:return: TrafficLight.state
"""
# 1. do detection
image_np = np.fromstring(image.data, dtype=np.uint8).reshape(
(image.height, image.width, 3))
# site images are in bgr, convert if needed
if image.encoding == "bgr8":
image_np = np.flip(image_np, 2)
boxes = self.object_detector.detect_traffic_lights(image_np)
# 2. create list of detected subimages
patches = []
for top, left, bottom, right in boxes:
patches.append(image_np[top:bottom, left:right])
# 3. do classification
predicted_state, classifications = self.classifier.get_classification(
patches)
#self.draw_boxes(image_np, boxes, classifications, counter)
return predicted_state
def draw_boxes(self, image, boxes, classifications, counter, thickness=4):
"""Draw bounding boxes on the image"""
colors = {
0: (255, 0, 0),
1: (255, 255, 0),
2: (0, 255, 0),
3: (255, 255, 255)
}
image = Image.fromarray(image)
draw = ImageDraw.Draw(image)
for i in range(len(boxes)):
bot, left, top, right = boxes[i, ...]
class_id = int(classifications[i])
color = colors[class_id]
draw.line([(left, top), (left, bot), (right, bot), (right, top),
(left, top)],
width=thickness,
fill=color)
image.save("./tmp/image_{:04d}.png".format(counter))
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose_stamped = None
self.waypoints_stamped = None
self.camera_image = None
self.lights = None
self.has_image = False
self.light_classifier = TLClassifier()
self.prev_light_loc = None
self.waypoint_tree = None
self.waypoints_2d = None
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.lights_wp = []
self.stoplines_wp = []
self.simulated_detection = rospy.get_param('~simulated_detection', 1)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
# Classifier Setup
rospy.loginfo("Loading TLClassifier model...")
self.light_classifier = TLClassifier()
model = load_model(self.config['tl']['tl_classification_model'])
resize_width = self.config['tl']['classifier_resize_width']
resize_height = self.config['tl']['classifier_resize_height']
self.light_classifier.setup_classifier(model, resize_width,
resize_height)
self.invalid_class_number = 3
# Detector setup
rospy.loginfo("Loading TLDetector model...")
custom_objects = {
'dice_coef_loss': dice_coef_loss,
'dice_coef': dice_coef
}
self.detector_model = load_model(
self.config['tl']['tl_detection_model'],
custom_objects=custom_objects)
self.detector_model._make_predict_function()
self.resize_width = self.config['tl']['detector_resize_width']
self.resize_height = self.config['tl']['detector_resize_height']
self.resize_height_ratio = self.config['camera_info'][
'image_height'] / float(self.resize_height)
self.resize_width_ratio = self.config['camera_info'][
'image_width'] / float(self.resize_width)
self.middle_col = self.resize_width / 2
self.is_carla = self.config['tl']['is_carla']
self.projection_threshold = self.config['tl']['projection_threshold']
self.projection_min = self.config['tl']['projection_min']
self.color_mode = self.config['tl']['color_mode']
rospy.loginfo("[TL_DETECTOR] Loaded models from disk")
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,
queue_size=1)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint',
Int32,
queue_size=1)
self.bridge = CvBridge()
rospy.spin()
## Callback functions
##==============================================================================
## Current Car pose
def pose_cb(self, msg):
self.pose_stamped = msg
## base waypoints.
def waypoints_cb(self, waypoints):
self.waypoints_stamped = 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)
for i in range(len(self.waypoints_stamped.waypoints)):
self.waypoints_stamped.waypoints[
i].pose.header.frame_id = self.waypoints_stamped.header.frame_id
self.calculate_traffic_light_waypoints()
## useful for simulater and cala, the color state will not be available on Cala
def traffic_cb(self, msg):
if self.simulated_detection > 0:
self.lights = msg.lights
self.calculate_traffic_light_waypoints()
light_wp, state = self.process_traffic_lights()
self.publish_upcoming_red_light(light_wp, state)
else:
if self.lights is not None:
return
self.lights = msg.lights
self.calculate_traffic_light_waypoints()
## Camera image
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.
'''
self.publish_upcoming_red_light(light_wp, state)
## methods
##==============================================================================
def publish_upcoming_red_light(self, light_wp, state):
"""Publishes the index of the waypoint closest to the red light's
stop line to /traffic_waypoint
Args:
light_wp: waypoint of the closest traffic light
state: state of the closest traffic light
"""
'''
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
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.pose_stamped is None or len(self.stoplines_wp) == 0:
rospy.loginfo("[TL_DETECTOR] No TL is detected. None")
return -1, TrafficLight.UNKNOWN
# find the closest visible traffic light (if one exists)
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_stamped):
car_wp_idx = self.get_closest_waypoint(
self.pose_stamped.pose.position.x,
self.pose_stamped.pose.position.y)
#TODO find the closest visible traffic light (if one exists)
diff = len(self.waypoints_stamped.waypoints)
for i, light in enumerate(
self.lights
): # the traffic light's information (position, number:8)
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
if closest_light:
state = self.get_light_state(closest_light)
return line_wp_idx, state
rospy.logwarn("[TL_DETECTOR] No TL is detected. None")
return -1, TrafficLight.UNKNOWN
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)
"""
labels = list(enumerate(['Red', 'Yellow', 'Green', 'None', 'None']))
if self.simulated_detection > 0:
if self.lights is None or light >= len(self.lights):
rospy.loginfo(
"[TL_DETECTOR] simulated_detection: No TL is detected. None"
)
return TrafficLight.UNKNOWN
state = self.lights[light].state
rospy.loginfo(
"[TL_DETECTOR] simulated_detection: Nearest TL-state is: %s",
labels[state][1])
return state
if not self.has_image:
self.prev_light_loc = None
rospy.loginfo(
"[TL_DETECTOR] has_image is None: No TL is detected. None")
return TrafficLight.UNKNOWN
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image,
self.color_mode)
tl_image = self.detect_traffic_light(cv_image)
if tl_image is not None:
state = self.light_classifier.get_classification(tl_image)
state = state if (
state != self.invalid_class_number) else TrafficLight.UNKNOWN
rospy.loginfo("[TL_DETECTOR] Nearest TL-state is: %s",
labels[state][1])
return state
else:
rospy.loginfo(
"[TL_DETECTOR] tl_image is None: No TL is detected. None")
return TrafficLight.UNKNOWN
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]
return closest_idx
def extract_image(self, pred_image_mask, image):
# rospy.loginfo("[TL_DETECTOR] Detecting TL...extract_image()")
if np.max(pred_image_mask) < self.projection_min:
return None
row_projection = np.sum(pred_image_mask, axis=1)
row_index = np.argmax(row_projection)
if np.max(row_projection) < self.projection_threshold:
return None
zero_row_indexes = np.argwhere(
row_projection <= self.projection_threshold)
top_part = zero_row_indexes[zero_row_indexes < row_index]
top = np.max(top_part) if top_part.size > 0 else 0
bottom_part = zero_row_indexes[zero_row_indexes > row_index]
bottom = np.min(
bottom_part) if bottom_part.size > 0 else self.resize_height
roi = pred_image_mask[top:bottom, :]
column_projection = np.sum(roi, axis=0)
if np.max(column_projection) < self.projection_min:
return None
non_zero_column_index = np.argwhere(
column_projection > self.projection_min)
index_of_column_index = np.argmin(
np.abs(non_zero_column_index - self.middle_col))
column_index = non_zero_column_index[index_of_column_index][0]
zero_column_indexes = np.argwhere(column_projection == 0)
left_side = zero_column_indexes[zero_column_indexes < column_index]
left = np.max(left_side) if left_side.size > 0 else 0
right_side = zero_column_indexes[zero_column_indexes > column_index]
right = np.min(
right_side) if right_side.size > 0 else self.resize_width
return image[
int(top * self.resize_height_ratio):int(bottom *
self.resize_height_ratio),
int(left * self.resize_width_ratio):int(right *
self.resize_width_ratio)]
def detect_traffic_light(self, cv_image):
# rospy.loginfo("[TL_DETECTOR] Detecting TL...detect_traffic_light()")
resize_image = cv2.resize(cv_image,
(self.resize_width, self.resize_height))
resize_image = cv2.cvtColor(resize_image, cv2.COLOR_RGB2GRAY)
resize_image = resize_image[..., np.newaxis]
if self.is_carla:
mean = np.mean(resize_image) # mean for data centering
std = np.std(resize_image) # std for data normalization
resize_image -= mean
resize_image /= std
image_mask = self.detector_model.predict(resize_image[None, :, :, :],
batch_size=1)[0]
image_mask = (image_mask[:, :, 0] * 255).astype(np.uint8)
return self.extract_image(image_mask, cv_image)
def distance2(self, pose1, pose2):
"""Calculate the square of the Eucleadian distance bentween the two poses given
Args:
pose1: given Pose
pose2: given Pose
Returns:
float: square of the Eucleadian distance bentween the two poses given
"""
dist2 = (pose1.position.x - pose2.position.x)**2 + (
pose1.position.y - pose2.position.y)**2
return dist2
def get_closest_stopline_pose(self, pose):
"""Finds closest stopline to the given Pose
Args:
pose: given Pose
Returns:
Pose: a Pose object whose position is that of the closest stopline
"""
stop_line_positions = self.config['stop_line_positions']
dist_min = sys.maxsize
stop_line_min = None
for stop_line_position in stop_line_positions:
stop_line_pose = Pose()
stop_line_pose.position.x = stop_line_position[0]
stop_line_pose.position.y = stop_line_position[1]
stop_line_pose.position.z = 0.0
dist = self.distance2(pose, stop_line_pose)
if dist < dist_min:
dist_min = dist
stop_line_min = stop_line_pose
return stop_line_min
def calculate_traffic_light_waypoints(self):
"""Populate traffic light waypoints and stopline waypoints arrays if they are not already populated
self.lights_wp contains the closest waypoints to corresponding trafic lights in self.lights
self.stoplines_wp contains the waypoints of stoplines corrsponding to trafic lights in self.lights
"""
if self.waypoints_stamped is not None and self.lights is not None and len(
self.lights_wp) == 0:
for i in range(len(self.lights)):
stopline = self.get_closest_stopline_pose(
self.lights[i].pose.pose)
self.stoplines_wp.append(
self.get_closest_waypoint(stopline.position.x,
stopline.position.y))
self.lights_wp.append(
self.get_closest_waypoint(
self.lights[i].pose.pose.position.x,
self.lights[i].pose.pose.position.y))
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.pose = None
self.waypoints = None
self.camera_image = None
self.waypoint_tree = None
self.waypoints_2d = None
self.lights = []
self.imgCnt = 0
sub1 = rospy.Subscriber('/current_pose', PoseStamped,
self.pose_cb) # from vehicle
sub2 = rospy.Subscriber(
'/base_waypoints', Lane, self.waypoints_cb
) #rom waypoint loader (only gets sent once on startup)
'''
/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 statewill 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
) # from simulator (continuous data about upcomming lights.
sub6 = rospy.Subscriber('/image_color', Image,
self.image_cb) # from vehicle
config_string = rospy.get_param(
"/traffic_light_config"
) #stopline positions and camera info AND real vs. sim
self.config = yaml.load(config_string)
self.upcoming_red_light_pub = rospy.Publisher(
'/traffic_waypoint', Int32, queue_size=1) # to waypoint updater
self.bridge = CvBridge()
# Read parameter to deterine if we're running in the sim or on Carla and pass to classifier constructor.
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
rospy.spin()
def pose_cb(self, msg):
#print("got pose")
self.pose = msg
def waypoints_cb(self, waypoints):
# Only gets called once
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):
if self.imgCnt == IMAGE_INTERVAL:
self.imgCnt = 0
#rospy.logwarn("image_cb")
"""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
else:
self.imgCnt += 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
"""
if self.waypoint_tree is not None:
closest_wp = self.waypoint_tree.query([x, y], 1)[1]
return closest_wp
else:
return None
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
#rospy.logwarn("getting light state")
#rospy.logwarn(light.state)
#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, "rgb8")
#Get classification
return self.light_classifier.get_classification(cv_image)
def process_traffic_lights(self):
#rospy.logwarn("processing traffic lights")
"""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)
"""
closest_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_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)
wp_len = len(self.waypoints.waypoints)
for i, light in enumerate(self.lights):
# Get stop line waypoint index
line = stop_line_positions[i]
light_wp_idx = self.get_closest_waypoint(line[0], line[1])
# Find the closest stop line waypoint index
wp_delta = light_wp_idx - car_wp_idx
#print(wp_delta)
if wp_delta >= 0 and wp_delta < wp_len:
closest_light = light
light_wp = light_wp_idx
break
if closest_light:
if wp_delta < 150:
state = self.get_light_state(closest_light)
else:
state = TrafficLight.UNKNOWN
#rospy.logwarn("state: {}".format(state))
return light_wp, state
self.waypoints = None
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.base_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
#self.speed_limit = float(self.kmph2mps(rospy.get_param('waypoint_loader/velocity')))
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, msg):
self.base_waypoints = msg.waypoints
#rospy.loginfo('base_waypoints[100] = %d', self.base_waypoints[100].pose.pose.position.x)
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, car_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_dist = 999999
closest_index = -1
# Find waypoint closest to vehicle position
for i in range(len(self.base_waypoints)):
dist = self.get_dist(self.base_waypoints[i].pose.pose.position, car_pose.position)
if dist < closest_dist:
closest_index = i
closest_dist = dist
return closest_index
def buffer_dist(self, current_velocity, decel):
v = current_velocity.twist.linear.x
return 0.5 * v * v / decel
def get_closest_waypoint_light(self, 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
"""
#TODO implement
min_dist = 999999
min_index = -1
# Find waypoint closest to vehicle position
for i in range(len(self.base_waypoints)):
dist = self.get_dist_light(self.base_waypoints[i].pose.pose.position, light_pose)
if dist < min_dist:
min_index = i
min_dist = dist
return min_index
def get_dist(self, pos1, pos2):
return math.sqrt((pos1.x - pos2.x)**2 + (pos1.y - pos2.y)**2 + (pos1.z - pos2.z)**2)
def get_dist_light(self, waypoint_pose, light_pose):
return math.sqrt((waypoint_pose.x - light_pose[0])**2 + (waypoint_pose.y - light_pose[1])**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, "bgr8")
light_image = cv2.resize(cv_image, (IMG_SIZE, IMG_SIZE), interpolation = cv2.INTER_CUBIC)
'''
bbox = self.project_to_image_plane(light)
if bbox is None:
return TrafficLight.UNKNOW
x1, y1, x2, y2 = bbox
if x1 is not None and abs(y2-y1) > 70 and abs(x2-x1) > 70:
light_roi = cv_image[y1:y2, x1:x2]
light_image = cv2.resize(light_roi, (IMG_SIZE, IMG_SIZE), interpolation = cv2.INTER_CUBIC)
'''
#Get classification
return self.light_classifier.get_classification(light_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']
#rospy.loginfo("stop_line_positions: x= %d, y= %d", stop_line_positions[0][0],stop_line_positions[0][1])
light_waypoint_pos = []
if self.base_waypoints is None:
return -1, TrafficLight.UNKNOWN
for i in range(len(stop_line_positions)):
light_pos = self.get_closest_waypoint_light(stop_line_positions[i])
light_waypoint_pos.append(light_pos)
#rospy.loginfo("light pos = %i", light_waypoint_pos[1])
#rospy.loginfo("light_waypoint_pos = %d", self.base_waypoints[light_waypoint_pos[0]].pose.pose.position.x)
self.last_tl_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_tl_pos_wp):
light_num_wp = min(self.last_tl_pos_wp)
else:
light_delta = self.last_tl_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_index = self.last_tl_pos_wp.index(light_num_wp)
light = stop_line_positions[light_index]
#light_distance = self.distance_light(light, point.pose.pose.position)
light_distance = self.get_dist_light(self.base_waypoints[self.last_car_position].pose.pose.position, light)
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)
rospy.loginfo('Traffic light index = %i, state = %d', light_num_wp, state)
return light_num_wp, state
self.base_waypoints = None
return -1, TrafficLight.UNKNOWN
def process_traffic_lights_2(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']
tl_waypoint_pos = []
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 project_with_fov(self, d, x, y):
# Camera characteristics
fov_x = self.config['camera_info']['focal_length_x']
fov_y = self.config['camera_info']['focal_length_y']
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
img_x = 0.5*image_width - 2574*x/d
img_y = image_height - 2740*y/d
img_x = self.clamp(img_x, 0, image_width)
img_y = self.clamp(img_y, 0, image_height)
return int(img_x), int(img_y)
def project_to_image_plane(self, light):
"""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']
# get transform between pose of camera and world frame
# trans = None
# rot = None
base_light = None
try:
now = rospy.Time.now()
self.listener.waitForTransform("/base_link",
light.header.frame_id, now, rospy.Duration(1.0))
base_light = self.listener.transformPose("base_link", light.pose)
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logerr("Failed to find camera to map transform")
return None
# Find bounding box of traffic light in image
if base_light is not None:
# Simulator uses FOV
if fx < 100:
# x, y = self.project_with_fov(base_light)
d = base_light.pose.position.x
x = base_light.pose.position.y + 0.5
y = base_light.pose.position.z - 1.75
ux, uy = self.project_with_fov(d, x + 0.5*LIGHT_WIDTH, y + 0.5*LIGHT_HEIGHT)
lx, ly = self.project_with_fov(d, x - 0.5*LIGHT_WIDTH, y - 0.5*LIGHT_HEIGHT)
return ux, uy, lx, ly
# Real car uses focal length
else:
rospy.loginfo('Real car detected... Process image using focal length!')
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, 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.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
rospy.loginfo("TLDetector is ready -> notify waypoint_updater")
#notify updater about detector readiness, use max negative integer for this
self.upcoming_red_light_pub.publish((-1234))
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):
global ProcessingTimeSum, ProcessingIterations
"""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 to call classification:
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.YELLOW:
light_wp = light_wp
else:
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):
"""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 we don't have any waypoints return None
if self.waypoints is None:
return
# Get current position
x = pose.position.x
y = pose.position.y
# define minimum distance variable
minimum_distance = None
minumum_location = None
# Search through all the waypoints to get the closes waypoint
for i,waypoint in enumerate(self.waypoints):
waypoint_x = waypoint.pose.pose.position.x
waypoint_y = waypoint.pose.pose.position.y
dist_to_waypoint = self.euclidianDistance(waypoint_x, waypoint_y, x, y)
# Initialize minimum distance at first value, or get new minimum distance
if minimum_distance is None:
minimum_location = i
minimum_distance = dist_to_waypoint
elif dist_to_waypoint <= minimum_distance:
minimum_location = i
minimum_distance = dist_to_waypoint
return minimum_location
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 get_distance(self, wp1_idx, wp2_idx):
"""Determines the distance between two way point's index
Args:
wp1_idx (waypoint index): waypoint index 1
wp2_idx (waypoint index): waypoint index 2
Returns:
int: distance in meters between two waypoint index
"""
if(wp1_idx < 0 or wp1_idx > len(self.waypoints) or
wp2_idx < 0 or wp2_idx > len(self.waypoints)):
return -1
wp1 = self.waypoints[wp1_idx].pose.pose.position
wp2 = self.waypoints[wp2_idx].pose.pose.position
return self.euclidianDistance(wp1.x, wp1.y, wp2.x, wp2.y)
def euclidianDistance(self, x1, y1, x2, y2):
return math.sqrt((x1 -x2)**2 + (y1 - y2)**2)
def find_closest_traffic_light(self, car_position):
"""Determines closest traffic to the car position
Args:
car_position: actual car position
Returns:
ligt: position of the traffic light
light_waypoint: closest waypoint to the traffic light
"""
light = None
closest_light = -1
# List of positions that correspond to the line to stop in front of for a given intersection
stop_light_positions = self.config['stop_line_positions']
# Loop through all the stop light positions
for i,stop_light_position in enumerate(stop_light_positions):
# Initialize a Pose
light_pose = Pose()
light_pose.position.x = stop_light_position[0]
light_pose.position.y = stop_light_position[1]
# Initialize a Waypoint, which is closest to the traffic light
light_waypoint = self.get_closest_waypoint(light_pose)
# Find the closest waypoint with a traffic light (or closest traffic light)
if light_waypoint >= car_position:
if light is None:
closest_light = light_waypoint
light = light_pose
elif light_waypoint < closest_light:
closest_light = light_waypoint
light = light_pose
return light, closest_light
def get_traffic_light_ground_truth(self, traffic_light):
"""
Finds ground truth from the traffic light
Args:
traffic_light: traffic light position
Returns:
light.state: traffic light state
"""
tl_x = traffic_light.position.x
tl_y = traffic_light.position.y
state = TrafficLight.UNKNOWN
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
'''
l_x = light.pose.pose.position.x
l_y = light.pose.pose.position.y
if self.euclidianDistance(l_x, l_y, tl_x, tl_y) < TL_DETECTION_RANGE:
return light.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 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)
"""
if (self.has_image == False):
return -1, TrafficLight.UNKNOWN
light_pos = None
closest_light = -1
car_position = -1
distance_to_tl = -1
if(self.pose):
car_position = self.get_closest_waypoint(self.pose.pose)
light_pos, closest_light = self.find_closest_traffic_light(car_position)
if car_position and light_pos:
waypoint_num_to_light = abs(car_position - closest_light)
distance_to_tl = self.get_distance(car_position, closest_light)
if light_pos:
state = TrafficLight.UNKNOWN
if USE_CLASSIFIER:
if (distance_to_tl < TL_DETECTION_RANGE):
state = self.get_light_state(light_pos)
#rospy.loginfo("-dist to closest TL- " + str(distance_to_tl))
else:
state = self.get_traffic_light_ground_truth(light_pos)
return closest_light, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoints2D = None
self.waypointsTree = 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
self.wrong_image_count = 0
self.has_image = False
# rospy.spin()
self.loop()
def loop(self):
rate = rospy.Rate(50)
while not rospy.is_shutdown():
self.checkLightAndPublish()
rate.sleep()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, msg):
self.waypoints = msg.waypoints
self.waypoints2D = [[wp.pose.pose.position.x, wp.pose.pose.position.y]
for wp in msg.waypoints]
self.waypointsTree = KDTree(self.waypoints2D)
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 checkLightAndPublish(self):
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
"""
return self.waypointsTree.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.has_image):
self.prev_light_loc = None
return light.state
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
classification = self.light_classifier.get_classification(cv_image)
if classification == TrafficLight.RED and light.state != TrafficLight.RED:
self.wrong_image_count += 1
cv2.imwrite(
"light_classification/wrong/shouldntbered" +
str(self.wrong_image_count) + ".png", cv_image)
return classification
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
closestLight = None
if (self.pose):
car_waypoint = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
else:
return -1, TrafficLight.UNKNOWN
# find the closest visible traffic light (if one exists)
closestLight, waypointToStopAt = self.get_closest_light(car_waypoint)
if closestLight:
state = self.get_light_state(closestLight)
return waypointToStopAt, state
return -1, TrafficLight.UNKNOWN
def get_closest_light(self, current_waypoint):
smallestDistance = len(self.waypoints)
closestLight = None
waypointToStopAt = 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']
for i, light in enumerate(self.lights):
line = stop_line_positions[i]
lineWaypoint = self.get_closest_waypoint(line[0], line[1])
distance = lineWaypoint - current_waypoint
if distance < smallestDistance and distance >= 0:
smallestDistance = distance
closestLight = light
waypointToStopAt = lineWaypoint
return closestLight, waypointToStopAt
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 = []
self.light_classifier = None
self.disable_camera = False #True
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.has_image = False
self.drop_count = 0
self.drop_every_num_frames = 2
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
"""
if self.disable_camera == True:
return
self.drop_count += 1
if self.drop_count == self.drop_every_num_frames:
rospy.loginfo("Got new image!!!!")
self.has_image = True
self.camera_image = msg
light_wp, state = self.process_traffic_lights()
rospy.loginfo("light_wp = %s, state = %s", 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
self.drop_count = 0
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
"""
idx = -1
dis_closest = -1
if self.waypoints is not None:
wps = self.waypoints.waypoints
for i in range(len(wps)):
wp = wps[i]
dist = (wp.pose.pose.position.x-pose.x)**2 + \
(wp.pose.pose.position.y-pose.y)**2
if (dis_closest == -1) or (dis_closest > dist):
dis_closest = dist
idx = i
return 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
imag = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
cv_image = cv2.cvtColor(imag, cv2.COLOR_BGR2RGB)
#Get classification
if self.light_classifier is None:
self.light_classifier = TLClassifier()
return self.light_classifier.get_classification(cv_image)
#return TLClassifier().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
state = 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 (len(self.tl_wps) == 0):
for stop_line in stop_line_positions:
point = stop_line[:]
point.append(0)
self.tl_wps.append(
self.get_closest_waypoint(
Point(stop_line[0], stop_line[1], 0)))
if (self.pose):
car_position = self.get_closest_waypoint(self.pose.pose.position)
#TODO find the closest visible5 traffic light (if one exists)
nearest_diff = -1
idx = -1
for i in range(len(self.tl_wps)):
diff = self.tl_wps[i] - car_position
if (diff > 0) and ((nearest_diff == -1) or
(nearest_diff > diff)):
nearest_diff = diff
idx = i
if idx != -1 and nearest_diff < WAYPOINT_THRESHOLD:
light = self.lights[idx]
light_wp = self.tl_wps[idx]
if light:
state = self.get_light_state(light)
# check the result
if light.state != TrafficLight.UNKNOWN:
if state != light.state:
rospy.logfatal(
'tl_detect error, state is %s but should be %s', state,
light.state)
#state = light.state
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.wp_search = 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)
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
self.wp_search = WaypointSearch(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
"""
start_time = rospy.get_time()
if None in (self.pose, self.wp_search):
return
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
rospy.loginfo("Classification duration=%.3f",
rospy.get_time() - start_time)
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")
#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)
"""
closest_light = None
closest_light_idx = -1
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']
car_wp_idx = self.wp_search.get_closest_waypoint_idx_ahead(
self.pose.pose.position.x, self.pose.pose.position.y)
diff = len(self.waypoints.waypoints)
for i, light in enumerate(self.lights):
stop_line_x, stop_line_y = stop_line_positions[i]
temp_wp_idx = self.wp_search.get_closest_waypoint_idx_behind(
stop_line_x, stop_line_y)
d = (temp_wp_idx - car_wp_idx) % len(self.waypoints.waypoints)
if d < diff:
diff = d
closest_light = light
closest_light_idx = i
line_wp_idx = temp_wp_idx
if closest_light:
state = self.get_light_state(closest_light)
distance = self.__calc_distance(car_wp_idx, line_wp_idx)
if hasattr(closest_light, 'state') and (
closest_light.state != state) and (distance < 75.0):
rospy.logwarn(
"Incorrect classification at TL %i: state=%s expected=%s at distance %.1f m",
closest_light_idx, state, closest_light.state, distance)
return line_wp_idx, state
return -1, TrafficLight.UNKNOWN
def __calc_distance(self, start_idx, end_idx):
"""Calculates the distance between two base waypoints"""
def dl(a, b):
return math.sqrt((a.x - b.x)**2 + (a.y - b.y)**2 + (a.z - b.z)**2)
total_dist = 0
num_wps = (end_idx - start_idx) % len(self.waypoints.waypoints)
idx1 = start_idx
for i in range(num_wps):
idx0 = idx1
idx1 = (idx0 + 1) % len(self.waypoints.waypoints)
total_dist += dl(self.waypoints.waypoints[idx0].pose.pose.position,
self.waypoints.waypoints[idx1].pose.pose.position)
return total_dist
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pos = -1
self.pose = None
self.waypoints = None
self.x_ave = 0.0
self.y_ave = 0.0
self.cos_rotate = 0.0
self.sin_rotate = 0.0
self.phi = []
self.stop_lines = None
self.camera_image = None
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()
if USE_KERAS_MODEL:
self.light_classifier = TLKeras()
else:
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.stop_idxs = []
# For image capture...
self.last_capture_idx = 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):
if msg:
self.pos = self.get_index(msg.pose.position.x, msg.pose.position.y)
self.pose = msg
def get_index(self, x, y):
rho = self.get_angle(x, y)
# special case of wrap around when past the last waypoint
if not self.phi or rho > self.phi[-1]:
return 0
idx = 0
while rho > self.phi[idx]:
idx += 1
return idx
def get_angle(self, x, y):
# First center
xc = x - self.x_ave
yc = y - self.y_ave
# and now rotate
xr = xc * self.cos_rotate - yc * self.sin_rotate
yr = yc * self.cos_rotate + xc * self.sin_rotate
# rho now starts at 0 and goes to 2pi for the track waypoints
rho = math.pi - math.atan2(xr, yr)
return rho
def waypoints_cb(self, lane):
#self.waypoints.extend(lane.waypoints)
self.waypoints = lane.waypoints
x_tot = 0.0
y_tot = 0.0
for p in self.waypoints:
x_tot += p.pose.pose.position.x
y_tot += p.pose.pose.position.y
# We use the average values to recenter the self.waypoints
self.x_ave = x_tot / len(self.waypoints)
self.y_ave = y_tot / len(self.waypoints)
# The very first waypoint determines the angle we need to rotate
# all waypoints by
xc = self.waypoints[0].pose.pose.position.x - self.x_ave
yc = self.waypoints[0].pose.pose.position.y - self.y_ave
rotate = math.atan2(xc, yc) + math.pi
self.cos_rotate = math.cos(rotate)
self.sin_rotate = math.sin(rotate)
for p in self.waypoints:
rho = self.get_angle(p.pose.pose.position.x, p.pose.pose.position.y)
self.phi.append(rho)
# We can only process the stop_lines after the waypoints
stop_line_positions = self.config['stop_line_positions']
for stop_line in stop_line_positions:
idx = self.get_index(stop_line[0], stop_line[1])
self.stop_idxs.append(idx)
rospy.loginfo(self.stop_idxs)
def traffic_cb(self, msg):
# It is possible that traffic_cb is called before we've had a
# chance to process the waypoints, so do nothing (returns None
# so we know if anyone tries to use this prematurely)
if not self.waypoints or len(msg.lights) != len(self.stop_idxs):
return
# Note that we depend on the fact that the stop_lines and the
# traffic lights appear in the same order in their config files
self.stop_lines = []
for i, light in enumerate(msg.lights):
sidx = self.stop_idxs[i]
self.stop_lines.append((sidx, light.state, light))
self.stop_lines.sort()
def get_next_stop_line(self):
if not self.stop_lines or not self.stop_idxs:
return (None, None, None)
elif self.pos > self.stop_lines[-1][0]:
return self.stop_lines[0]
idx = 0
num_lights = len(self.stop_lines)
while self.pos > self.stop_lines[idx][0]:
idx += 1
if idx >= len(self.stop_lines):
rospy.logerr("stop lines idx: %d" % idx)
# for debug. a positions past the last stop_line will trigger
# if self.pos > self.stop_lines[idx][0]:
# rospy.loginfo("get_next_stop_line self.pos %d stop_lines: %d" % \
# (self.pos, self.stop_lines[idx][0]))
return self.stop_lines[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
"""
self.has_image = True
self.camera_image = msg
incoming_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
light_wp = self.last_wp
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = incoming_light_wp if state == TrafficLight.RED or \
state == TrafficLight.YELLOW else -1
self.last_wp = light_wp
else:
light_wp = self.last_wp
# We refactored Udacity's code to make sure we publish on every cycle
self.upcoming_red_light_pub.publish(Int32(light_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
"""
return self.get_index(pose.position.x, pose.position.y)
def save_image(self, image):
''' Image capture for generating training images'''
if not COLLECT_IMAGES:
return
sl_idx, state, light = self.get_next_stop_line()
if sl_idx in capture_idxs and \
sl_idx - self.pos < CAPTURE_DISTANCE and \
self.pos > self.last_capture_idx + 10:
self.last_capture_idx = self.pos
fname = CAPTURE_DIR + TRAFFIC_LIGHT_2_NAME[state] + \
str(sl_idx) + "_" + str(self.pos) + ".jpg"
# fake out cv2 to write rgb
img = image[..., ::-1]
cv2.imwrite(fname, img, [cv2.IMWRITE_JPEG_QUALITY, 90])
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
# Changed this to rgb since that is default for tensorflow / keras
# Avoids unnecessary color conversions
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
# only for data collecton
self.save_image(cv_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. If LOOP_ONCE is True, then alway return the last waypoint
as a red light.
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 is now maintained by pose_cb as self.pos
# Find the closest visible traffic light (if one exists)
stop_line_wp, state, light = self.get_next_stop_line()
outstr = "Car position : " + str(self.pos) + " stop_line_wp : " + str(stop_line_wp)
#rospy.loginfo(outstr)
if light:
if USE_CLASSIFICATION:
state = self.get_light_state(light)
#rospy.logwarn("light state: %d" % state)
if LOOP_ONCE:
# Only if our position is past the last traffic light, we return the
# index of the very last waypoint as a red light
if self.pos > self.stop_idxs[-1]:
state = TrafficLight.RED
stop_line_wp = len(self.waypoints) - 1
return stop_line_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_image_cnt = 0
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.stop_line_positions = self.config['stop_line_positions']
self.is_site = self.config[
'is_site'] # get status the code is run sim or real world
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.waypoint_tree = None
self.waypoints_2d = None
self.has_image = False
self.light_image_num = 0
self.light_classifier = TLClassifier(self.is_site)
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 # Simulator send the traffic light position and light color
def image_cb(self, msg):
"""Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light's stop line to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
"""
self.has_image = True
self.camera_image = msg
self.light_image_cnt = self.light_image_cnt + 1
if self.light_image_cnt % 2 == 0:
if self.waypoint_tree:
light_wp, state = self.process_traffic_lights()
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED 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
'''
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.
'''
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]
# Check if closest is ahead or behind vehicles
closest_coord = self.waypoints_2d[closest_idx]
pre_coord = self.waypoints_2d[closest_idx - 1]
# Equation for hyperplane through closest_coords
cl_vect = np.array(closest_coord)
prev_vect = np.array(pre_coord)
pos_vect = np.array([x, y])
val = np.dot(cl_vect - prev_vect, pos_vect - cl_vect)
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 not self.has_image:
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
# cv.namedWindow('input_image', cv.WINDOW_NORMAL)
# cv.imshow('input_image', cv_image)
# cv.waitKey(1)
# self.light_image_num = self.light_image_num + 1
# filename = 'data/light_v2_'+ str(self.light_image_num)+'.png'
# cv.imwrite(filename, cv_image)
#Get classification
return self.light_classifier.get_classification(cv_image)
#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
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):
line = self.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
if closest_light:
state = self.get_light_state(closest_light)
return line_wp_idx, state
# just for test
if self.is_site:
state = self.get_light_state(closest_light)
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 = []
self.prev_wp_idx = 0
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.first_image_time = None
self.image_count = -1
# [x,y] coordinates of stopping lines before traffic lights
self.stop_line_positions = self.config['stop_line_positions']
# Indices in self.waypoints of the waypoints closest to the respective stopping lines, as reported in self.stop_line_positions
self.stop_line_idxs = None # Can be initialised only after receiving the list of waypoints, see waypoints_cb()
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)
''' Subscription to /image_color is inside self.waypoints_cb(), as we need the list of track waypoints before
being able to process messages from /image_color'''
rospy.spin()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints):
self.waypoints = waypoints.waypoints
self.stop_line_idxs = [
np.argmin([
euler_distance(
(wp.pose.pose.position.x, wp.pose.pose.position.y),
stop_line_pos) for wp in self.waypoints
]) for stop_line_pos in self.stop_line_positions
]
rospy.Subscriber('/image_color', Image, self.image_cb)
'''
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.image_count += 1
if self.first_image_time is None:
self.first_image_time = time.time()
if self.image_count >= 1:
rospy.logdebug("Images per second: {}".format(
self.image_count / (time.time() - self.first_image_time)))
self.has_image = True
self.camera_image = msg
light_wp_i, state = self.process_traffic_lights()
rospy.logdebug("light_wp_i={} state={}".format(light_wp_i, 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_i = light_wp_i if state == TrafficLight.RED or state == TrafficLight.YELLOW else -1
self.last_wp = light_wp_i
self.upcoming_red_light_pub.publish(Int32(light_wp_i))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
def get_light_state(self):
"""Determines the current color of the traffic light
Args:
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 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)
"""
# 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 is not None:
car_pose = self.pose.pose
car_x, car_y, _ = unpack_pose(car_pose)
# Find the closest visible traffic light (if one exists)
min_distance = sys.float_info.max
min_distance_i = -1
quaternion = (car_pose.orientation.x, car_pose.orientation.y,
car_pose.orientation.z, car_pose.orientation.w)
_, _, car_yaw = tf.transformations.euler_from_quaternion(
quaternion)
for pos_i, pos in enumerate(stop_line_positions):
dist = ((pos[0] - car_x)**2 + (pos[1] - car_y)**2)**.5
if dist < min_distance:
tl_car_ref_x, _ = universal2car_ref(
pos[0], pos[1], car_x, car_y, car_yaw)
# Check for a light with stop line before or slightly after the car
if tl_car_ref_x >= -1.4:
min_distance = dist
min_distance_i = pos_i
rospy.logdebug('min_distance_i={} min_distance={}\n'.format(
min_distance_i, min_distance))
if min_distance_i >= 0 and min_distance < 80:
state = self.get_light_state()
return self.stop_line_idxs[min_distance_i], state
else:
return -1, TrafficLight.UNKNOWN
else:
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.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.image_lock = threading.RLock()
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.switcher = {
TrafficLight.RED: "RED",
TrafficLight.YELLOW: "YELLOW",
TrafficLight.GREEN: "GREEN",
}
self.colors = {
"RED": (255, 0, 0),
"YELLOW": (255, 255, 0),
"GREEN": (0, 255, 0)
}
self.light_detector = rospy.get_param('~light_detector', False)
self.debug_window = rospy.get_param('~debug_window', False)
rospy.logwarn('light detector = {}'.format(self.light_detector))
rospy.logwarn('debug window = {}'.format(self.debug_window))
self.trafficLightState = rospy.Publisher('/dbg/traffic_light_state',
Image,
queue_size=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)
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, msg):
self.waypoints = msg.waypoints
rospy.loginfo('Waypoints Received')
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()
light_class = self.switcher.get(state, "UNKNOWN")
rospy.logwarn('light {} light_wp {} state {}'.format(
light_class, light_wp, Int32(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 >= rospy.get_param('~state_count_threshold', 3):
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED or state == TrafficLight.YELLOW else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
def get_closest_waypoint(self, pose):
"""Identifies the closest path waypoint to the given position
https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Args:
pose (Pose): position to match a waypoint to
Returns:
int: index of the closest waypoint in self.waypoints
"""
# if way points are empty
if self.waypoints is None:
return None
# transform waypoints to array
waypoints_array = np.asarray([(w.pose.pose.position.x,
w.pose.pose.position.y)
for w in self.waypoints])
position_array = np.asarray([pose.position.x, pose.position.y])
#calculate euclidian distance
distance = np.sum(np.sqrt((waypoints_array - position_array)**2),
axis=1)
# get closest
index = np.argmin(distance)
return index
def get_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 get_stop_line_waypoint(self, stop_line_positions):
closest_idx = -1
min_dist = LARGE_NUMBER
for pos in stop_line_positions:
stop_line = PoseStamped()
stop_line.pose.position.x = float(pos[0])
stop_line.pose.position.y = float(pos[1])
dist = self.get_distance(stop_line.pose.position,
self.pose.pose.position)
if dist < min_dist and self.get_closest_waypoint(
stop_line.pose) > self.get_closest_waypoint(
self.pose.pose):
closest_idx = self.get_closest_waypoint(stop_line.pose)
min_dist = dist
return closest_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
light_wp = None
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
config_string = rospy.get_param("/traffic_light_config")
stop_line_positions = yaml.load(config_string)['stop_line_positions']
#self.stop_line_wps = self.get_stop_line_waypoints(stop_line_positions)
# 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)
# find the closest visible traffic light (if one exists)
if self.light_detector:
time_start = time.time()
state, score = self.light_classifier.get_classification(cv_image)
elapsed_time = time.time() - time_start
rospy.logwarn('light_detector time: {0}'.format(elapsed_time))
#rospy.logwarn('light_wp_idx: {0}'.format(light_wp))
light_wp = self.get_stop_line_waypoint(stop_line_positions)
#rospy.logwarn('light_wp_idx: {0}'.format(light_wp))
if self.debug_window:
if state == TrafficLight.UNKNOWN:
self.trafficLightState.publish(
self.bridge.cv2_to_imgmsg(
cv2.cvtColor(np.zeros((600, 800), np.uint8),
cv2.COLOR_GRAY2RGB), "rgb8"))
else:
light_class = self.switcher.get(state, "UNKNOWN")
cv2.putText(cv_image, "%s %f" % (light_class, score),
(10, 550), cv2.FONT_HERSHEY_SIMPLEX, 1,
self.colors.get(light_class), 2)
self.trafficLightState.publish(
self.bridge.cv2_to_imgmsg(cv_image, "rgb8"))
time_end = time.time()
#rospy.loginfo('Traffic lights detection (ms) {} '.format((time_end - time_start) * 1000))
return light_wp, 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.light_map = None
self.light_distance = None
self.light_is_behind = None
self.closest = 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.
'''
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.STATE_COUNT_THRESHOLD = rospy.get_param("~state_count_threshold")
use_simulator_classifier = rospy.get_param("~traffic_light_classifier_sim")
self.light_classifier = TLClassifier(sim = use_simulator_classifier)
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.has_image = False
rate = rospy.get_param("~rate")
r = rospy.Rate(rate)
while not rospy.is_shutdown():
self.process_image()
r.sleep()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, waypoints): # type: Lane
self.waypoints = waypoints
if self.light_map is None:
stop_line_positions = self.config['stop_line_positions']
self.light_map = LightMap(waypoints, stop_line_positions)
def process_image(self):
if (self.has_image):
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 >= self.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(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 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
"""
return util.get_closest_waypoint(self.waypoints, pose)
def project_to_image_plane(self, point_in_world):
return (0, 0)
def get_light_state(self):
"""Determines the current color of the traffic light
Args:
light (TrafficLight): light to classify
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
if(not self.has_image):
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
#Get classification
pred = self.light_classifier.get_classification(cv_image)
return pred
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)
"""
start_time = rospy.get_time()
if self.pose and self.light_map:
closest_waypoint_to_light = self.light_map.get_closest_waypoint_to_upcoming_light(self.pose)
else:
closest_waypoint_to_light = -1
end_time = rospy.get_time()
get_closest_waypoint_timespan = end_time - start_time
start_time = end_time
state = self.get_light_state()
end_time = rospy.get_time()
get_light_state_timespan = end_time - start_time
start_time = end_time
rospy.loginfo('(get_closest_waypoint, get_light_state): ({}, {})'.format(get_closest_waypoint_timespan, get_light_state_timespan))
return closest_waypoint_to_light, state
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.waypoints_2d = 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)
# Combining queue_size and buff_size to minimize lag, see https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb, queue_size=1, buff_size=10000000)
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_score_confidence_threshold = 0.4 # when score is above this threshold, we are confident about the prediction
self.light_score_unknown_threashold = 0.2 # when score is below this threshold, the light is classified as unknown
#for matching tensorflow class (R:1, G:2, Y:3) with TrafficLight msg (R:0, Y:1, G:2)
self.light_class_dict = {1:0, 2:2, 3:1}
#TrafficLight msg (R:0, Y:1, G:2, Un:4) to names
self.light_name_dict = {0:'Red_traffic_light',
1:'Yellow_traffic_light',
2:'Green_traffic_light',
4:'Unkown'}
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.waypoint_tree = KDTree([[waypoint.pose.pose.position.x, waypoint.pose.pose.position.y]
for waypoint in 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, score = self.process_traffic_lights()
rospy.loginfo('Current detection state: {}, Light waypoint index: {}'.format(
self.light_name_dict[state],
light_wp))
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to eiher occur `STATE_COUNT_THRESHOLD` number
of times or has a high predicted score till we start using it.
Otherwise the previous stable state is used.
'''
if (self.state != state) and (score < self.light_score_confidence_threshold):
self.state_count = 1
self.state = state
elif (self.state_count >= STATE_COUNT_THRESHOLD) or (
score >= self.light_score_confidence_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
if self.last_wp == -1:
rospy.loginfo('Execution: continue driving')
else:
rospy.loginfo('Execution: STOPPING')
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.waypoint_tree.query([x, y], 1)[1]
return closest_idx
def get_light_state(self, light):
"""Calling the classifier to get 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)
score: score of predicted traffic light state. Higher prediction score means higher confidence.
"""
if not self.has_image:
self.prev_light_loc = None
return False
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
# expand_dims to shape [1, None, None, 3].
image_expanded = np.expand_dims(cv_image, axis=0)
t_start = rospy.get_time()
boxes, scores, classes, num = self.light_classifier.get_classification(image_expanded)
classification_time = rospy.get_time() - t_start
rospy.loginfo('Detected Class: {}, Score: {:.4f}, Inference time: {:.4f}s'.format(
self.light_name_dict[self.light_class_dict[classes[0][0]]],
scores[0][0],
classification_time))
if num > 0 and scores[0][0] > self.light_score_unknown_threashold:
return self.light_class_dict[classes[0][0]], scores[0][0]
return TrafficLight.UNKNOWN, 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)
float: predicted score of the traffic light state. Higher score means higher prediction confidence.
"""
closest_light = None
light_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)
rospy.loginfo("Car waypoint idx: " + str(car_wp_idx))
# find the closest visible traffic light (if one exists)
diff = len(self.waypoints.waypoints)
for i, light in enumerate(self.lights):
#get stopline waypoint 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
light_wp_idx = temp_wp_idx
do_detection = self.is_within_detection_distance(self.waypoints.waypoints, car_wp_idx, light_wp_idx)
if closest_light and do_detection:
state, score = self.get_light_state(closest_light)
return light_wp_idx, state, score
else:
if closest_light:
rospy.loginfo("Did not run detector. Next light is too far away.")
return -1, TrafficLight.UNKNOWN, 0
def is_within_detection_distance(self, waypoints, wp1, wp2):
"""
return true if the distance between the two waypoints is
close enough to activate the traffic light classifier.
"""
if wp1 is None or wp2 is None:
return True
dist = 0
dl = lambda a, b: math.sqrt((a.x-b.x)**2 + (a.y-b.y)**2 + (a.z-b.z)**2)
# jumping every 3 wps because we just need a coarse estimate
for i in range(wp1, wp2+1, 3):
dist += dl(waypoints[wp1].pose.pose.position, waypoints[i].pose.pose.position)
wp1 = i
if dist > TL_DETECTION_DISTANCE_THRESHOLD:
return False
return True
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.initialized = False
self.pose_received = False
self.waypoints_received = False
self.image_received = False
self.pose = None
self.waypoints = None
self.waypoints_2d = None
self.waypoint_tree = None
self.camera_image = None
self.lights = []
self.bridge = CvBridge()
self.td_id = 1
self.img_count = 0
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.process_count = 0
self.last_img_processed = -1
self.process_rate = CAMERA_IMG_PROCESS_RATE
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.is_site = self.config["is_site"]
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
self.tl_detector_state_pub = rospy.Publisher('/tl_detector_state', Int32, queue_size=1)
self.is_site_pub = rospy.Publisher('/is_site', Int32, queue_size=1)
self.state_count_th = STATE_COUNT_THRESHOLD
if self.is_site:
self.state_count_th = STATE_COUNT_THRESHOLD_SITE
# Choose classifier
if not self.is_site:
self.classifier = TLClassifier(model_path=os.path.join(MODEL_PATH, 'sim_frozen_inference_graph.pb'),
label_map_path=os.path.join(MODEL_PATH, 'tl_label_map.pbtxt'))
else:
self.classifier = TLClassifier(model_path=os.path.join(MODEL_PATH, 'site_frozen_inference_graph.pb'),
label_map_path=os.path.join(MODEL_PATH, 'tl_label_map.pbtxt'))
self.td_img_path = os.path.join(OUTPUT_PATH, 'IMG')
if COLLECT_TD:
if not os.path.exists(OUTPUT_PATH):
os.mkdir(OUTPUT_PATH)
else:
raise Exception('Directory {} already exists. Please choose a different name.'.format(OUTPUT_PATH))
if not os.path.exists(self.td_img_path):
os.mkdir(self.td_img_path)
rospy.init_node('tl_detector')
self.last_img_processed = 0
self.listener = tf.TransformListener()
self.initialized = True
rospy.spin()
def is_initialized(self):
return self.pose_received and self.waypoints_received and self.image_received and self.initialized
def pose_cb(self, msg):
self.pose = msg
self.pose_received = True
def waypoints_cb(self, waypoints):
rospy.logwarn("Waypoints received")
self.waypoints = waypoints
self.waypoints_received = True
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.image_received = True
is_initialized = self.is_initialized()
self.tl_detector_state_pub.publish(Int32(is_initialized))
self.is_site_pub.publish(Int32(self.is_site))
if not is_initialized:
return
time_elapsed = timer() - self.last_img_processed
# Do not process the camera image unless some time has passed from last processing
if time_elapsed < self.process_rate:
return
self.camera_image = msg
self.last_img_processed = timer()
light_wp, state = self.process_traffic_lights()
rospy.logwarn('INFERENCE TIME: {:.3f} s'.format(timer()-self.last_img_processed))
'''
Collect Training Data
'''
self.img_count += 1
label = tl_utils.tl_state_to_label(state)
if COLLECT_TD and light_wp != -1 and self.img_count % TD_RATE == 0:
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
tl_utils.save_td(uid=self.td_id, cv_image=cv_image, label=label, csv_path=OUTPUT_PATH,
img_path=self.td_img_path)
self.td_id += 1
'''
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.
'''
# Ensure that the light state hasn't changed before taking any option
if self.last_state != state:
self.state_count = 0
self.last_state = state
self.state_count += 1
if self.state_count >= self.state_count_th:
self.last_state = state
light_wp = light_wp if state == TrafficLight.RED else -1
self.last_wp = light_wp
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
def get_closest_waypoint(self, x, y):
"""Identifies the closest path waypoint to the given position
Args:
x: x position
y: y position
Returns:
int: index of the closest waypoint in self.waypoints
"""
return self.waypoint_tree.query([x, y], 1)[1]
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)
"""
# Return light state as ground truth
if COLLECT_TD and not self.is_site:
return light.state
else:
# convert camera image to cv image
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
if self.is_site:
cv_image = prep.run_preprocessor_pipeline(cv_image)
# Get classification
return self.classifier.get_classification(cv_image, min_score=0.65)
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 d >= 0 and d < diff:
diff = d
closest_light = light
line_wp_idx = temp_wp_idx
'''
elif self.is_site:
self.process_count += 1
state = self.get_light_state(None)
if (self.process_count % LOGGING_THROTTLE_FACTOR) == 0:
if state is not TrafficLight.UNKNOWN:
rospy.logwarn("Detected {color} traffic light".format(
color=StateToString[state]))
'''
if closest_light:
distance = line_wp_idx - car_wp_idx
if distance <= WAYPOINT_DIFFERENCE:
self.process_count += 1
state = self.get_light_state(closest_light)
if (self.process_count % LOGGING_THROTTLE_FACTOR) == 0:
rospy.logwarn("TRAFFIC: Detected {color} traffic light at {idx}, distance={distance} ".format(
idx=line_wp_idx, color=StateToString[state], distance=distance))
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 = None
self.light_dict = None
self.light_waypoints = None
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.state = TrafficLight.UNKNOWN
self.state_count = 0
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.closest_waypoint = None
self.image_date = datetime.now()
sub1 = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
self.waypoints_sub = rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
self.closest_waypoint_sub = rospy.Subscriber('/closest_waypoint', Int32, self.closest_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)
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
config_string = rospy.get_param('/traffic_light_config')
self.config = yaml.load(config_string)
rospy.spin()
def pose_cb(self, msg):
self.pose = msg.pose
def waypoints_cb(self, msg):
self.waypoints = msg.waypoints
self.waypoints_sub.unregister()
def closest_waypoint_cb(self, msg):
self.closest_waypoint = int(msg.data)
def traffic_cb(self, msg):
self.lights = msg.lights
# --- Find all waypoints for traffic light.
if (not self.light_waypoints or not self.light_dict) and self.waypoints:
self.light_dict = {}
for idx, alight in enumerate(self.lights):
alight_wp = self.get_closest_waypoint(alight.pose.pose)
self.light_dict[alight_wp] = alight
self.light_waypoints = sorted(self.light_dict.keys())
self.traffic_lights_sub.unregister()
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
"""
# Skip certain images to lower the processing rate.
# TODO: Tune skip time.
now = datetime.now()
if now - self.image_date < timedelta(milliseconds=350):
# Assume same as previous.
light_wp, state = self.last_wp, self.state
else:
self.image_date = now
# Process frame.
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
"""
closest_gap = float('inf')
closest_idx = 0
for idx, awp in enumerate(self.waypoints):
agap = (awp.pose.pose.position.x - pose.position.x)**2 + \
(awp.pose.pose.position.y - pose.position.y)**2
if agap < closest_gap:
closest_gap = agap
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
prediction = self.light_classifier.get_classification(cv_image)
# print '--->', prediction
return prediction
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_positions = self.config['stop_line_positions']
if self.closest_waypoint and self.waypoints and self.lights and self.light_waypoints and self.light_dict:
car_wp = self.closest_waypoint
# --- Determine the next traffice light in waypoint.
light = None
light_wp = None
if car_wp <= self.light_waypoints[0]:
light_wp = self.light_waypoints[0] # Before the first light waypoint.
elif car_wp >= self.light_waypoints[-1]:
light_wp = self.light_waypoints[0] # After the last light waypoint. So loop around.
else:
for light1_wp, light2_wp in zip(self.light_waypoints, self.light_waypoints[1:]):
if car_wp > light1_wp and car_wp <= light2_wp:
light_wp = light2_wp
# --- Get next traffic light ahead.
light = self.light_dict.get(light_wp)
# Found light_wp and if it is close enough.
# if light:
# print(light_wp - car_wp)
if light and (light_wp - car_wp) < 200 and (light_wp - car_wp) > 30:
state = self.get_light_state(light)
# print(state)
# --- TESTING ONLY
# state = light.state
# --- TESTING ONLY
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.waypoints_2d = None
self.waypoint_tree = None
self.use_camera = True
self.camera_image = None
self.has_image = False
self.lights = []
self.img_count = 0
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
if self.config['is_site']:
rospy.loginfo("Carla is being used")
else:
rospy.loginfo("Simulation is being used")
if self.use_camera:
rospy.loginfo("Camera is being currently used")
else:
rospy.loginfo("Camera is not being used now")
sub_pose = rospy.Subscriber('/current_pose', PoseStamped, self.pose_cb)
sub_lane = 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 = rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray, self.traffic_cb)
if self.config['is_site']:
sub_image = rospy.Subscriber('/image_raw', Image, self.image_cb)
else:
sub_image = 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.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
rospy.spin()
def pose_cb(self, msg):
"""
Function : Function that copies the topic information to the class pose
Input Args : topic message
Output : None
"""
self.pose = msg
def waypoints_cb(self, waypoints):
"""
Function : waypoint callback that populates the values in the waypoint class variable
waypoints callback from the subscriber when the system receives the base waypoint
Input Args : waypoints
Output : None
"""
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):
"""
Function : waypoint callback that populates
the values in the waypoint class variable
# traffic callback from the subscriber
# when the system receives trafic light image(s) they are processed
# if a traffic light is found in the image, it gets the closest waypoint
Input Args : traffic image information
Output : None
"""
self.lights = msg.lights
if not self.use_camera and self.waypoints:
light_wp, state = self.process_traffic_lights()
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
light_wp = light_wp if state == TrafficLight.RED 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(self, msg):
"""
Function : image callback
This function identifies red lights in the input camera images
imput variables: image from the camera on the car
output : Publishes the closest waypoint index to the red light's stopping line to /traffic_waypoint
There is no format return
Input Args : traffic image information
Output : None
"""
if self.use_camera and self.waypoints:
if self.img_count >= IMG_COUNT_THRESHOLD:
self.img_count = 0
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
#rospy.loginfo("Publishing new waypoint")
self.upcoming_red_light_pub.publish(Int32(light_wp))
self.last_wp = light_wp
else:
rospy.loginfo("Publishing old waypoints")
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
else:
self.img_count += 1
def get_closest_waypoint(self, x, y):
"""
Function :
This function identifies the closest path waypoint of the car's given position
Reference : https://en.wikipedia.org/wiki/Closest_pair_of_points_problem
Input Args : position for waypoint matching
Output : index of the closest waypoint
"""
#TODO implement
closest_wp_idx = None
if self.waypoint_tree:
closest_wp_idx = self.waypoint_tree.query([x, y], 1)[1]
return closest_wp_idx
def get_light_state(self, light):
"""
Function :
This function obtains the color of the traffic light
Input Args : TrafficLight : which gives the light to classify
Output : ID of traffic light color as given in styx_msgs/TrafficLight
"""
if (not self.has_image):
self.prev_light_loc = None
rospy.loginfo("Traffic light not present in image")
return light.state
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "bgr8")
return self.light_classifier.get_classification(
cv_image, self.config['is_site'])
def process_traffic_lights(self):
"""
Function : This function finds closest visible traffic light and
determines its location and color and hence the state of the light
Input Args : None
Output : index of waypoint closest to the upcoming stop line
for a traffic light (-1 if none exists)
"""
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']
# default code uses pose directly. But I wanted to try using the x,y coordinates for better performance
if (self.pose):
car_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
else:
car_wp_idx = 0
#TODO find the closest visible traffic light (if one exists)
# Find the number of waypoints
wp_len = len(self.waypoints.waypoints)
for i, light in enumerate(self.lights):
# Get stop line waypoint index
line = stop_line_positions[i]
interim_wp_idx = self.get_closest_waypoint(line[0], line[1])
# Find closest stop line waypoint index
dist_wp = interim_wp_idx - car_wp_idx
if dist_wp >= 0 and dist_wp < wp_len:
wp_len = dist_wp
closest_light = light
line_wp_idx = interim_wp_idx
if closest_light:
if wp_len < 180:
state = self.get_light_state(closest_light)
else:
state = TrafficLight.UNKNOWN
return line_wp_idx, state
return -1, TrafficLight.UNKNOWN
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.simulator = True if rospy.get_param('~sim') == 1 else False
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.bridge = CvBridge()
self.light_classifier = TLClassifier(self.simulator)
self.listener = tf.TransformListener()
self.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = -1
self.state_count = 0
self.camera_image = None
self.seq = 0
self.count = 0
self.misscount = 0.
self.totcount = 0.
self.sight = 75. # cautionary distance
self.wpAcquired = False
self.lastCrop = None
self.imageAcquired = 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)
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)
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
#rospy.spin()
# Operations loop to identify 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
rate = rospy.Rate(PUBLISHING_RATE)
while not rospy.is_shutdown():
if self.imageAcquired:
light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at PUBLISHING_RATE frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times until 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
rate.sleep()
def pose_cb(self, msg):
self.pose = msg
def waypoints_cb(self, msg):
if not self.wpAcquired:
self.wpAcquired = True
self.waypoints = self.filterWaypoints(msg)
if not self.simulator:
self.sight = 16. # adjust cautionary distance for site test
print "cautionary distance", self.sight
print self.waypoints[20].twist.twist.linear.x
def traffic_cb(self, msg):
self.lights = msg.lights
def image_cb(self, msg):
"""Capture the inbound camera image
Args:
msg (Image): image from car-mounted camera
"""
self.imageAcquired = True
self.camera_image = msg
def distance(self, pos1, pos2):
return math.sqrt((pos1.position.x - pos2.position.x)**2 +
(pos1.position.y - pos2.position.y)**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
Returns:
int: index of the closest waypoint in self.waypoints
"""
index = -1 #Return if waypoints are empty
if self.waypoints is None:
return index
lowDistance = 0
#rospy.loginfo("len(wp): %d", len(self.waypoints))
for i in range(len(self.waypoints)):
distance = self.distance(pose, self.waypoints[i].pose.pose)
if index == -1 or distance < lowDistance:
index = i
lowDistance = distance
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.imageAcquired):
self.prev_light_loc = None
return TrafficLight.RED
# fix camera encoding
if hasattr(self.camera_image, 'encoding'):
if self.camera_image.encoding == '8UC3':
self.camera_image.encoding = "rgb8"
else:
self.camera_image.encoding = 'rgb8'
cv_image = self.bridge.imgmsg_to_cv2(self.camera_image, "rgb8")
# get classification (simulator classifies whole image)
if self.simulator:
return self.light_classifier.get_classification(cv_image)
# get classification (test site detects boxes using SSD and classifies with OpenCV)
# if previous location is not known, scan most of the image
if self.lastCrop is None:
gostop, found, location = self.light_classifier.get_classification(
cv_image[0:500, 50:50 + 700])
if found:
# check y extents
if location[0] < 150:
top = 0
bot = 300
else:
top = location[0] - 150
if location[0] > 350:
top = 200
bot = 500
else:
bot = location[0] + 150
# check x extents (remember, offset by 50)
if location[1] + 50 < 150:
left = 50
right = 350
else:
left = location[1] + 50 - 150
if location[1] + 50 > 600:
left = 450
right = 750
else:
right = location[1] + 50 + 150
self.lastCrop = (top, bot, left, right)
#print "first", self.lastCrop
# (no else) TL not found, next cycle will be a complete search again
# otherwise, use last known location as crop starting point
else:
(top, bot, left, right) = self.lastCrop
gostop, found, location = self.light_classifier.get_classification(
cv_image[top:bot, left:right])
if found: # determine crop for next cycle
# check y extents, offset by top
otop, oleft = top, left
if location[0] + otop < 150:
top = 0
bot = 300
else:
top = location[0] + otop - 150
if location[0] + otop > 350:
top = 200
bot = 500
else:
bot = location[0] + otop + 150
# check x extents, offset by left
if location[1] + oleft + 50 < 200:
left = 50
right = 350
else:
left = location[1] + oleft + 50 - 150
if location[1] + oleft + 50 > 600:
left = 450
right = 750
else:
right = location[1] + oleft + 50 + 150
self.lastCrop = (top, bot, left, right)
#print "next", self.lastCrop
else:
self.lastCrop = None
#print "none"
return gostop
def get_nearest_stop_line(self, waypoint_start_index):
stop_line = 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']
last_index = 99999
for i in range(0, len(stop_line_positions)):
stopline_pose = PoseStamped()
stopline_pose.pose.position.x = float(stop_line_positions[i][0])
stopline_pose.pose.position.y = float(stop_line_positions[i][1])
index = self.get_closest_waypoint(stopline_pose.pose)
if index > waypoint_start_index and index < last_index:
last_index = index
stop_line = stopline_pose
return stop_line, 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 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)
"""
if (self.pose):
car_position = self.get_closest_waypoint(self.pose.pose)
#rospy.loginfo("car_position: %d", car_position)
if car_position > 0:
stop_pos, stop_waypoint = self.get_nearest_stop_line(
car_position)
if stop_pos is not None:
#rospy.loginfo("stop_pos x: %.2f, stop_waypoint: %d", stop_pos.pose.position.x, stop_waypoint)
state = TrafficLight.UNKNOWN
# if the traffic light is within sight, then attempt to classify
if self.distance(self.pose.pose,
stop_pos.pose) < self.sight:
if CLASSIFIER_ENABLED:
state = self.get_light_state(None)
if self.lights is not None:
stateTruth = TrafficLight.UNKNOWN
for light in self.lights:
# Get the ground truth from /vehicle/traffic_lights
if self.distance(light.pose.pose,
stop_pos.pose) < 30.:
stateTruth = light.state
break
if (((stateTruth is 4) or
(stateTruth is 2)) and
(state is 0)) or (((stateTruth is 0) or
(stateTruth is 1)) and
(state is 4)):
print "Classifier: ", state, " Truth: ", stateTruth
state = stateTruth
#self.saveImage(self.camera_image, stateTruth)
self.misscount += 1.
self.totcount += 1.
#print "mismatch%: ", self.misscount / self.totcount
#self.saveImage(self.camera_image, stateTruth)
else:
for light in self.lights:
# This section uses only /vehicle/traffic_lights
if self.distance(light.pose.pose,
stop_pos.pose) < 30.:
state = light.state
#rospy.loginfo("light state: %d, x: %.2f, y: %.2f", state, self.pose.pose.position.x, self.pose.pose.position.y)
#rospy.loginfo("dist tl to stop: %.2f", self.distance(light.pose.pose, stop_pos.pose))
break
return stop_waypoint, state
#self.waypoints = None
return -1, TrafficLight.UNKNOWN
def saveImage(self, img, state):
#if self.count%2==0:
# fix camera encoding
if hasattr(img, 'encoding'):
if img.encoding == '8UC3':
img.encoding = "rgb8"
else:
img.encoding = 'rgb8'
img = self.bridge.imgmsg_to_cv2(img, "rgb8")
image_data = cv2.resize(img, (224, 224))
img = PIL_Image.fromarray(image_data, 'RGB')
#if state == TrafficLight.RED:
# img.save('/home/student/data/red/out'+str(self.seq).zfill(5)+'.png', 'PNG')
# self.seq += 1
if state == TrafficLight.YELLOW:
img.save(
'/home/student/data/yellow/out' + str(self.seq).zfill(5) +
'.png', 'PNG')
self.seq += 1
elif state == TrafficLight.GREEN:
img.save(
'/home/student/data/green/out' + str(self.seq).zfill(5) +
'.png', 'PNG')
self.seq += 1
#else:
# img.save('/home/student/data/unknown/out'+str(self.seq).zfill(5)+'.png', 'PNG')
# self.seq += 1
def filterWaypoints(self, wp):
if wp.waypoints[0].pose.pose.position.x == 10.4062:
waypoints = []
path = rospy.get_param('~path')
if not os.path.isfile(path):
return wp.waypoints
with open(path) as wfile:
reader = csv.DictReader(wfile, ['x', 'y', 'z', 'yaw'])
for wp in reader:
p = Waypoint()
p.pose.pose.position.x = float(wp['x'])
p.pose.pose.position.y = float(wp['y'])
p.pose.pose.position.z = float(wp['z'])
q = tf.transformations.quaternion_from_euler(
0., 0., float(wp['yaw']))
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = 2.7777778
waypoints.append(p)
return waypoints
return wp.waypoints
class TLDetector(object):
def __init__(self):
rospy.init_node('tl_detector')
self.use_classifier = rospy.get_param('~use_classifier', False)
print(("self.use_classifier:", self.use_classifier))
self.current_pose = None
self.base_waypoints = None
self.has_image = False
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)
#sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
self.sub_image = rospy.Subscriber('/image_color', Image, self.image_cb, queue_size=1) #, buff_size=4*52428800)
print("subscribed")
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.state = TrafficLight.UNKNOWN
self.last_state = TrafficLight.UNKNOWN
self.last_wp = NO_CAMERA_YET
self.state_count = 0
self.stop_waypoints = []
self.car_wp = -1
self.nlight_i = NO_STOP
self.nlight_wp = NO_CAMERA_YET
self.nlight = None
self.last100 = -1
self.initializing = True
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.loop()
def loop(self):
rate = rospy.Rate(1)
while not rospy.is_shutdown():
if not self.has_image:
self.upcoming_red_light_pub.publish(Int32(NO_CAMERA_YET))
else:
if self.current_pose and self.base_waypoints and len(self.lights) > 0 and len(self.stop_waypoints) > 0:
if len(self.lights) != len(self.stop_waypoints):
print("We have different number of stops and lights!!!")
break
# we are near this point
self.car_wp = self.get_closest_waypoint(self.current_pose.pose)
if self.last100 != self.car_wp // 100:
self.last100 = self.car_wp // 100
print(("Car waypoint:", self.last100*100))
self.nlight_i = self.find_next_stop(self.car_wp)
self.nlight_wp = NO_RED
self.nlight = None
if self.nlight_i >= 0 and (self.car_wp < self.stop_waypoints[self.nlight_i] + LOOK_BEHIND) and (self.car_wp > self.stop_waypoints[self.nlight_i] - LOOK_AHEAD):
self.nlight_wp = self.stop_waypoints[self.nlight_i]
self.nlight = self.lights[self.nlight_i]
if self.nlight_wp >= 0 and self.sub_image is None:
self.sub_image = rospy.Subscriber('/image_color', Image, self.image_cb, queue_size=1) #, buff_size=4*52428800)
print("subscribed")
elif self.nlight_wp < 0 and self.sub_image is not None:
self.sub_image.unregister()
self.sub_image = None
print("unsubscribed")
self.last_wp = NO_RED
self.upcoming_red_light_pub.publish(Int32(NO_RED))
else:
if not self.current_pose:
print("No pose!!!")
elif not self.base_waypoints:
print("No waypoints!!!")
elif len(self.lights) <= 0:
print("# of lights is zero!!!")
elif len(self.stop_waypoints) <= 0:
print("# of stops is zero!!!")
break
rate.sleep()
def pose_cb(self, msg):
self.current_pose = msg
def waypoints_cb(self, msg):
self.base_waypoints = msg.waypoints;
# List of positions that correspond to the line to stop in front of for a given intersection
self.stop_positions = self.config['stop_line_positions']
for stop_pos in self.stop_positions:
stop_pose_stamped = self.create_dummy_pose(stop_pos[0], stop_pos[1], stop_pos[2], stop_pos[3]) if len(stop_pos) == 4 \
else self.create_dummy_pose(stop_pos[0], stop_pos[1])
stop_wp = self.get_closest_waypoint(stop_pose_stamped.pose)
self.stop_waypoints.append(stop_wp)
print(self.stop_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 self.initializing:
light_wp = STILL_INIT
else:
light_wp = light_wp if state == TrafficLight.RED or state == TrafficLight.YELLOW else NO_RED
if state == TrafficLight.RED:
print(("nlight_i:", self.nlight_i, "RED", self.state_count, "car_wp:", self.car_wp, "nlight_wp:", self.nlight_wp))
if state == TrafficLight.YELLOW:
print(("nlight_i:", self.nlight_i, "YELLOW", self.state_count, "car_wp:", self.car_wp, "nlight_wp:", self.nlight_wp))
self.last_wp = light_wp
print(("light_wp", light_wp, "car_wp:", self.car_wp))
self.upcoming_red_light_pub.publish(Int32(light_wp))
else:
print(("last_wp", self.last_wp))
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
def create_dummy_pose(self, x, y, z=0., yaw=0.):
pose = PoseStamped()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
q = tf.transformations.quaternion_from_euler(0., 0., math.pi * yaw/180.)
pose.pose.orientation = Quaternion(*q)
return pose
def find_next_stop(self, wp):
for i in range(len(self.stop_waypoints)):
if wp < self.stop_waypoints[i] + LOOK_BEHIND:
return i
return NO_STOP
def distance_sq(self, p1, p2):
x, y, z = p1.x - p2.x, p1.y - p2.y, p1.z - p2.z
return x*x + y*y + z*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.base_waypoints
"""
self_pos = pose.position
distances = np.array([self.distance_sq(self_pos, way_pos.pose.pose.position) for way_pos in self.base_waypoints])
return np.argmin(distances)
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 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)
"""
wp, color = STILL_INIT, TrafficLight.UNKNOWN
if self.nlight_wp >= 0 and self.nlight:
if self.use_classifier:
wp, color = self.nlight_wp, self.get_light_state(self.nlight_wp)
else:
wp, color = self.nlight_wp, self.nlight.state
if self.initializing:
self.initializing = False
elif self.initializing:
wp, color = STILL_INIT, TrafficLight.UNKNOWN
else:
wp, color = NO_STOP, TrafficLight.UNKNOWN
return wp, color
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.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
