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.base_waypoints and self.pose and self.light_waypoints:
car_waypoint_idx = self.get_closest_waypoint(self.pose.pose)
light = None
min_light_idx = self.get_closest_waypoint_idx(
self.light_waypoints, car_waypoint_idx)
min_close_idx = None
if min_light_idx:
light = TrafficLight()
light.pose = self.base_waypoints[min_light_idx].pose
min_close_idx = self.get_closest_waypoint_idx(
self.close_waypoints, car_waypoint_idx)
if light:
state = self.get_light_state(light)
return min_close_idx, state
return -1, TrafficLight.UNKNOWN
def makelight(self, x, y, z):
light = TrafficLight()
light.pose = PoseStamped()
light.pose.pose.position.x = x
light.pose.pose.position.y = y
light.pose.pose.position.z = z
return light
def create_light_site(self, x, y, z, yaw, state):
"""
Creates a pose for the traffic light in the format required by
get_closest_waypoint function instead of creating a new one
"""
light = TrafficLight()
light.header = Header()
light.header.stamp = rospy.Time.now()
light.header.frame_id = 'world'
light.pose = PoseStamped()
light.pose.header = Header()
light.pose.header.stamp = rospy.Time.now()
light.pose.header.frame_id = 'world'
light.pose.pose.position.x = x
light.pose.pose.position.y = y
light.pose.pose.position.z = z
q = tf.transformations.quaternion_from_euler(0.0, 0.0,
math.pi * yaw / 180.0)
light.pose.pose.orientation = Quaternion(*q)
light.state = state
return light
def setUp(self):
self.tld = TLDetector(is_testing=True)
base_wps = Lane()
# Waypoints are at (0.2, 0.2) increments.
for pt in np.arange(0, N_WPS, 0.2):
wp = Waypoint()
wp.pose.pose.position.x = pt
wp.pose.pose.position.y = pt
base_wps.waypoints.append(wp)
self.tld.waypoints_cb(base_wps)
lights = TrafficLightArray()
l_arr = [[1.0, 1.0, 0], [2.0, 2.2, 2], [3.0, 2.5, 1], [6.0, 6.0, 0]]
# Set stop lines (0.5, 0.5) in front of the traffic light.
# At runtime, stop_line_positions should be set using the ROS parameter server;
# so for testing, set values by reach into the tld object.
self.tld.stop_line_positions = []
for l in l_arr:
tl = TrafficLight()
tl.state = l[2]
tl.pose.pose.position.x = l[0]
tl.pose.pose.position.y = l[1]
lights.lights.append(tl)
self.tld.stop_line_positions.append([l[0] - 0.5, l[1] - 0.5])
self.tld.traffic_cb(lights)
def convert_tl_config_to_lane_msgs():
#print("In __func__")
traffic_lights = TrafficLightArray()
tl_list = []
tl_height = rospy.get_param("/tl_height_sim")
#traffic_light_positions = traffic_light_config.config['light_positions']
for traffic_light_index, traffic_light_position in enumerate(
traffic_light_config.config['light_positions']):
traffic_light = TrafficLight()
traffic_light.pose.pose.position.x = traffic_light_position[0]
traffic_light.pose.pose.position.y = traffic_light_position[1]
traffic_light.pose.pose.position.z = tl_height
traffic_light.state = TrafficLight.UNKNOWN
tl_list.append(traffic_light)
traffic_lights.lights = tl_list
global DEBUG
if DEBUG == True:
for traffic_light_index, traffic_light_position in enumerate(tl_list):
rospy.loginfo('x:%f,y:%f',
traffic_light_position.pose.pose.position.x,
traffic_light_position.pose.pose.position.y)
return traffic_lights
def get_given_traffic_lights():
"""
Return given traffic light positions
:return: TrafficLightArray
"""
traffic_lights = TrafficLightArray()
traffic_light_list = []
tl_height = rospy.get_param("/tl_height")
config_string = rospy.get_param("/traffic_light_config")
traffic_light_positions = yaml.load(config_string)["light_positions"]
for traffic_light_index, traffic_light_position in enumerate(
traffic_light_positions):
traffic_light = TrafficLight()
traffic_light.pose.pose.position.x = traffic_light_position[0]
traffic_light.pose.pose.position.y = traffic_light_position[1]
traffic_light.pose.pose.position.z = tl_height
traffic_light.state = TrafficLight.UNKNOWN
traffic_light_list.append(traffic_light)
traffic_lights.lights = traffic_light_list
return traffic_lights
def create_light_site(self, x, y, z, yaw, state):
light = TrafficLight()
light.header = Header()
light.header.stamp = rospy.Time.now()
light.header.frame_id = 'world'
# Create a Pose object to place inside the TrafficLight object
light.pose = PoseStamped()
light.pose.header = Header()
light.pose.header.stamp = rospy.Time.now()
light.pose.header.frame_id = 'world'
light.pose.pose.position.x = x
light.pose.pose.position.y = y
light.pose.pose.position.z = z
# For reference: https://answers.ros.org/question/69754/quaternion-transformations-in-python/
q = tf.transformations.quaternion_from_euler(0.0, 0.0,
math.pi * yaw / 180.0)
light.pose.pose.orientation = Quaternion(*q)
light.state = state
return light
def publish_upcoming_light_state(self, idx, light_state):
traffic_light = TrafficLight()
traffic_light.header.frame_id = '/world'
traffic_light.header.stamp = rospy.Time(time.time())
traffic_light.idx = idx
traffic_light.pose = self.lights[idx].pose
traffic_light.state = light_state
self.upcoming_traffic_light_pub.publish(traffic_light)
def image_cb(self, msg):
"""Identifies red lights in the incoming camera image.
The index of the waypoint closest to the red light's stop line gets
published to the /traffic_waypoint topic.
Args:
msg (Image): image from car-mounted camera
"""
self.has_image = True
self.camera_image = msg
state, closest_lightstop_wp_index = self.process_traffic_lights()
# state = out[0]
# light_wp_index = out[1]
# light_state_gt = out[2]
# closest_lightstop_wp_index_gt = out[3]
# light_pose = out[4]
# DEVELOPMENT - remove these lines of code to use the classifier instead of ground truth
# state = light_state_gt
# light_wp_index = closest_lightstop_wp_index_gt
# 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
closest_red_lightstop_wp_index = closest_lightstop_wp_index if state == TrafficLight.RED else -1
self.last_wp = closest_red_lightstop_wp_index
self.upcoming_red_light_pub.publish(
Int32(closest_red_lightstop_wp_index))
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
self.state_count += 1
traffic_light = TrafficLight()
traffic_light.header.stamp = rospy.Time.now()
traffic_light.header.frame_id = '/world'
traffic_light.state = TrafficLight.UNKNOWN
if closest_lightstop_wp_index != -1:
# For visualization publish light location and color
# Only publish traffic light for visualization if light is red,
# green or yellow, and if we found a nearby lightstop waypoint
traffic_light.state = state
light_pose = self.waypoints.waypoints[closest_lightstop_wp_index]
traffic_light.pose.pose = light_pose.pose.pose
self.upcoming_light_pub.publish(traffic_light)
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)
"""
# CW: started with example code from walkthrough video at ~3min
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']
#sys.stderr.write("Debug: tl_detector process_traffic_lights() pose ok=%s\n" % repr(self.pose is not None))
if (self.pose):
car_wp_idx = self.get_closest_waypoint(self.pose.pose.position.x,
self.pose.pose.position.y)
#sys.stderr.write("Debug: tl_detector process_traffic_lights() car_wp_idx=%d\n" % car_wp_idx)
#DONE find the closest visible traffic light (if one exists)
# CW: starting with walkthrough code suggestion; gets closest in terms of
# waypoint index rather than actual distance, but that's fine assuming
# waypoints are sorted in list following the route (which they are of course)
# List of traffic lights not too long so OK to search all
diff = len(self.waypoints.waypoints)
for i, light in enumerate(self.lights):
# get stop line waypoint index
line = stop_line_positions[i]
# Go from coords of light stop line to nearest waypoint in our list
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
light_wp_idx = temp_wp_idx
if not closest_light and self.grab_training_images:
# When playing from the bag file, we don't have pose information but nevertheless
# we want to grab the training image so we still want to call get_light_state(),
# so invent one with unknown state
closest_light = TrafficLight()
closest_light.state = TrafficLight.UNKNOWN
light_wp_idx = -1
if closest_light:
state = self.get_light_state(closest_light)
#sys.stderr.write("Debug: tl_detector process_traffic_lights() returning light_wp_idx=%d state=%d\n" % (light_wp_idx, state))
return light_wp_idx, state
else:
#sys.stderr.write("Debug: tl_detector process_traffic_lights() returning -1 as no closest_light\n")
return -1, TrafficLight.UNKNOWN
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
max_detection_dist = 120 # maximum distance we want to check lights for
min_dist = float('inf') #closest light
# List of positions that correspond to the line to stop in front of for a given intersection
stop_line_positions = self.tl_detector.config['stop_line_positions']
if (self.tl_detector.pose and self.tl_detector.waypoints):
car_position = self.get_closest_waypoint(
self.tl_detector.pose.pose)
# Find the closest visible traffic light (if one exists)
for stop_pos in stop_line_positions:
new_light = TrafficLight()
#new_light.header = Header()
new_light.header.stamp = rospy.Time.now()
new_light.header.frame_id = 'world'
new_light.pose.pose = Pose()
new_light.pose.pose.position.x = stop_pos[0]
new_light.pose.pose.position.y = stop_pos[1]
new_light.state = TrafficLight.UNKNOWN
stop_position = self.get_closest_waypoint(new_light.pose.pose)
distance_to_light = math.sqrt(
(self.tl_detector.waypoints[car_position].pose.pose.
position.x - self.tl_detector.waypoints[stop_position].
pose.pose.position.x)**2 +
(self.tl_detector.waypoints[car_position].pose.pose.
position.y - self.tl_detector.waypoints[stop_position].
pose.pose.position.y)**2)
if distance_to_light < min_dist and distance_to_light < max_detection_dist: # if closer than last light, but not beyond max range we are interested in,
if car_position < stop_position: # and our car has not yet passed the wp the light is at, then...
min_dist = distance_to_light
light = new_light
light_wp = stop_position
if light:
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
def create_light(self, x, y, z, yaw, state):
light = TrafficLight()
light.header = Header()
light.header.stamp = rospy.Time.now()
light.header.frame_id = '/world'
light.pose = self.create_pose(x, y, z, yaw)
light.state = state
return light
def create_light(self, x, y, z, yaw, state):
light = TrafficLight()
light.header = Header()
light.header.stamp = rospy.Time.now()
light.header.frame_id = '/world'
light.pose = self.create_pose(x, y, z, yaw)
light.state = state
return light
def stop_loc(self, pos_x, pos_y):
# light state initialization
light = TrafficLight()
# pose position
light.pose = PoseStamped()
light.pose.header.stamp = rospy.Time.now()
light.pose.header.frame_id = 'world'
light.pose.pose.position.x = pos_x
light.pose.pose.position.y = pos_y
#light.pose.pose.position.z = 0.0
return light
def get_classification(self, image):
x = cv2.resize(image, MODEL_PICTURE_SIZE)
x = np.expand_dims(x, axis=0)
x = np.float64(x)
x = preprocess_input(x)
with self.graph.as_default():
logits = self.model.predict(x)
maxindex = np.argmax(logits)
color = SPARSE_TO_IDX[maxindex]
tl = TrafficLight()
tl.state = color
return tl
def waypoints_cb(self, waypoints):
self.waypoints = waypoints
stop_line_positions = self.config['stop_line_positions']
idx = 0
for line in stop_line_positions:
traffic_light = TrafficLight()
traffic_light.pose = PoseStamped()
traffic_light.pose.pose.position.x = line[0]
traffic_light.pose.pose.position.y = line[1]
traffic_light.pose.pose.position.z = 0.0
self.traffic_light_waypoint_indexes.append([idx,self.get_closest_waypoint(traffic_light.pose.pose)])
idx += 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)
"""
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 and self.waypoints):
#get the light index closest to the current car position
closest_light_idx = self.get_closest_point_idx(
self.pose, self.lights)
#rospy.logwarn("closest light waypoint is %s", closest_light_idx)
if self.distance(self.lights[closest_light_idx].pose.pose.position,
self.pose.position) < HORIZON:
state = self.get_light_state(self.lights[closest_light_idx])
#if state is TrafficLight.GREEN:
# rospy.logwarn("Green light detected")
#if state is TrafficLight.YELLOW:
# rospy.logwarn("Yellow light detected")
if state is TrafficLight.RED:
#rospy.logwarn("Red light detected")
stop_lines = list()
for pos in stop_line_positions:
light = TrafficLight()
light.pose = PoseStamped()
light.pose.pose.position.x = pos[0]
light.pose.pose.position.y = pos[1]
light.pose.pose.position.z = 0.0
stop_lines.append(light)
distances = [
self.distance(
stop_lines[closest_light_idx].pose.pose.position,
wp.pose.pose.position) for wp in self.waypoints
]
line_wp = distances.index(min(distances))
#rospy.logwarn("closest stop line waypoint with red traffic lights %s", self.waypoints[line_wp].pose.pose.position.x)
return line_wp, state
if light:
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
def get_closest_traffic_light(self, pose, light_positions):
"""Identifies the closest traffic light waypoint to the given waypoint.
Args:
pose (Pose): position to match a light to
Returns:
Light: the closest light from self.lights or None
"""
if (light_positions):
min_distance = 1e9
min_light = None
for ndx, light in enumerate(light_positions):
traffic_light = TrafficLight()
traffic_light.header.stamp = rospy.Time(0)
traffic_light.pose.pose.position.x = light[0]
traffic_light.pose.pose.position.y = light[1]
distance = self.pose_distance(traffic_light.pose.pose, pose)
if (distance < min_distance
and self.is_waypoint_in_front_of_vehicle(
traffic_light.pose.pose, pose)):
min_light = traffic_light
min_distance = distance
#rospy.loginfo("Closest Light At: %s\n m", min_distance)
return min_light, min_distance
#else
return None
def get_closest_traffic_light(self, pose, light_positions):
"""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
light_position: the location of the traffic light
Returns:
int: index of the closest traffic light in self.lights
"""
#TODO implement
if (light_positions):
min_light = None
#minimum distance to traffic light
min_distance = 1e9
for index, light in enumerate(light_positions):
#convert into eucledian
traffic_light = TrafficLight()
traffic_light.header.stamp = rospy.Time(0)
traffic_light.pose.pose.position.x = light[0]
traffic_light.pose.pose.position.y = light[1]
distance = self.pose_distance(traffic_light.pose.pose, pose)
if (distance < min_distance
and self.is_waypoint_in_front_of_vehicle(
traffic_light.pose.pose, pose)):
min_distance = distance
min_light = traffic_light
return min_light, min_distance
return None
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 we have received waypoints
if self.waypoints == None:
rospy.logwarn(
"tl_detector: waypoints not yet acquired. Skipping received image information"
)
return -1, TrafficLight.UNKNOWN
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.logwarn(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)
dl = lambda a, b, c, d: math.sqrt((a - b)**2 + (c - d)**2)
closest_light = -1
closest_light_distance = 100000
for i in range(len(stop_line_positions)):
light_x = stop_line_positions[i][0]
light_y = stop_line_positions[i][1]
car_x = self.waypoints[car_position].pose.pose.position.x
car_y = self.waypoints[car_position].pose.pose.position.y
dist = dl(car_x, light_x, car_y, light_y)
if dist < closest_light_distance:
closest_light = i
closest_light_distance = dist
# define traffic light position, reusing cars' orientation
light = TrafficLight()
light.pose.pose.position.x = stop_line_positions[closest_light][0]
light.pose.pose.position.y = stop_line_positions[closest_light][1]
light.pose.pose.orientation = self.waypoints[
car_position].pose.pose.orientation
# find closest waypoint to traffic light
light_wp = self.get_closest_waypoint(light.pose.pose)
if light:
#state = self.get_light_state(light)
state = TrafficLight.RED
rospy.logwarn("tl_detectot: sending light wp " + str(light_wp) +
" and state " + str(state))
return light_wp, state
#self.waypoints = None
return -1, TrafficLight.UNKNOWN
def image_cb(self, msg):
"""Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light's stop line to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
"""
self.has_image = True
self.camera_image = msg
light_wp, state = self.process_traffic_lights()
#print(light_wp)
#print(state)
#print()
'''
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.
'''
our_msg = TL_State()
our_light = TrafficLight()
our_light.header = Header()
our_light.header.stamp = rospy.Time.now()
our_light.header.frame_id = '/world'
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))
our_msg.waypoint = light_wp
our_light.state = state
our_msg.light = our_light
self.custom_state_pub.publish(our_msg)
else:
self.upcoming_red_light_pub.publish(Int32(self.last_wp))
our_msg.waypoint = self.last_wp
our_light.state = self.last_state
our_msg.light = our_light
self.custom_state_pub.publish(our_msg)
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)
"""
light = None
tl_i = None
# List of positions that correspond to the line to stop in front of for a given intersection
stop_line_positions_plain = self.config['stop_line_positions']
stop_line_positions = []
if (self.pose):
for st in stop_line_positions_plain:
s = TrafficLight()
s.pose.pose.position.x = st[0]
s.pose.pose.position.y = st[1]
s.pose.pose.position.z = 0
s.pose.pose.orientation.x = self.pose.pose.orientation.x
s.pose.pose.orientation.y = self.pose.pose.orientation.y
s.pose.pose.orientation.z = self.pose.pose.orientation.z
s.pose.pose.orientation.w = self.pose.pose.orientation.w
stop_line_positions.append(s)
#DONE find the closest visible traffic light (if one exists)
tl_i, a, d = self.get_closest_waypoint(self.pose.pose, self.lights,
'F')
if tl_i == None:
self.visualize_tl_front(None)
self.visualize_tl_front(None, 0)
return -1, TrafficLight.UNKNOWN
# print("angle: {}".format(a))
# import ipdb; ipdb.set_trace()
stop_i, _, _ = self.get_closest_waypoint(self.lights[tl_i].pose.pose,
stop_line_positions)
stop_i_car, _, _ = self.get_closest_waypoint(self.pose.pose,
stop_line_positions, 'F')
if stop_i_car != stop_i:
self.visualize_tl_front(None)
self.visualize_tl_front(None, 0)
return -1, TrafficLight.UNKNOWN
stop_wp_i, _, _ = self.get_closest_waypoint(
stop_line_positions[stop_i].pose.pose, self.waypoints.waypoints)
state = self.get_light_state(self.lights[tl_i])
# state = self.lights[tl_i].state
self.visualize_tl_front(self.waypoints.waypoints[stop_wp_i].pose.pose)
self.visualize_tl_front(self.lights[tl_i].pose.pose, state)
return stop_wp_i, state
def get_classification(self, image):
"""Determines the color of the traffic light in the image
Args:
image (cv::Mat): image containing the traffic light
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
resized = cv2.resize(image, (self.width, self.height))
resized = resized / 255.; # Normalization
# necessary work around to avoid troubles with keras
with self.graph.as_default():
predictions = self.model.predict(resized.reshape((1, self.height, self.width, self.channels)))
color = predictions[0].tolist().index(np.max(predictions[0]))
tl = TrafficLight()
tl.state = color
return tl.state
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.tlclasses_d = { TrafficLight.RED : "RED_LIGHT", TrafficLight.YELLOW:"YELLOW_LIGHT", TrafficLight.GREEN:"GREEN_LIGHT", TrafficLight.UNKNOWN:"UNKNOWN" }
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) #Simulator has state of traffic light built in
#sub6 = rospy.Subscriber('/image_color', Image, self.image_cb)
sub6 = rospy.Subscriber('/image_color', Image, self.image_cb, queue_size=1, buff_size=32*10**6) #Camera data; set buffer to reduce lag
sub7 = rospy.Subscriber('/image_raw', Image, self.raw_image_cb) #Image raw used for classifier for more data
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
#setup stop line positions in TrafficLight-style object for use later on closestwaypoint
self.stop_line_positions_poses = []
for stop in self.config['stop_line_positions']:
s = TrafficLight()
s.pose.pose.position.x = stop[0]
s.pose.pose.position.y = stop[1]
s.pose.pose.position.z = 0
self.stop_line_positions_poses.append(s)
#Publishing
self.upcoming_red_light_pub = rospy.Publisher('/traffic_waypoint', Int32, queue_size=1)
self.tl_detector_initialized_pub = rospy.Publisher('/tl_detector_initialized', Bool, queue_size=1)
#Set up Classifier
self.bridge = CvBridge()
self.light_classifier = TLClassifier()
self.listener = tf.TransformListener()
self.tl_detector_initialized_pub.publish(Bool(True))
rospy.loginfo('Traffic light detector initialized')
rospy.spin()
def __init__(self):
rospy.init_node('tl_detector')
self.pose = None
self.waypoints = None
self.camera_image = None
self.lights = []
self.slps = []
self.listener = None
sub1 = rospy.Subscriber('/current_pose',
PoseStamped,
self.pose_cb,
queue_size=1)
self.waypoints = rospy.wait_for_message(
'/base_waypoints',
Lane).waypoints # Only need to get base_waypoints once
'''
/vehicle/traffic_lights provides you with the location of the traffic light in 3D map space and
helps you acquire an accurate ground truth data source for the traffic light
classifier by sending the current color state of all traffic lights in the
simulator. When testing on the vehicle, the color state will not be available. You'll need to
rely on the position of the light and the camera image to predict it.
'''
sub3 = rospy.Subscriber('/vehicle/traffic_lights',
TrafficLightArray,
self.traffic_cb,
queue_size=1)
config_string = rospy.get_param("/traffic_light_config")
self.config = yaml.load(config_string)
# List of positions that correspond to the line to stop in front of a given intersection. Convert to more appropriate format
for slp in self.config['stop_line_positions']:
tl = TrafficLight()
tl.pose.pose.position.x, tl.pose.pose.position.y, tl.pose.pose.position.z = slp[
0], slp[1], 0
self.slps.append(self.get_closest_waypoint(tl.pose.pose.position))
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.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 create_tl(self, yaw, state, x, y, z):
traffic_light = TrafficLight()
traffic_light.header = Header()
traffic_light.pose.header = Header()
traffic_light.pose = PoseStamped()
traffic_light.state = state
traffic_light.pose.pose.position.x = x
traffic_light.pose.pose.position.y = y
traffic_light.pose.pose.position.z = z
traffic_light.pose.header.stamp = rospy.Time.now()
traffic_light.pose.header.frame_id = 'world'
traffic_light.header.stamp = rospy.Time.now()
traffic_light.header.frame_id = 'world'
q = tf.transformations.quaternion_from_euler(0.0, 0.0,
math.pi * yaw / 180.0)
traffic_light.pose.pose.orientation = Quaternion(*q)
return traffic_light
def tl_cb(self, msg):
if self.current_pose == None : return
self.tl_array = msg.lights
#print "light count = ", len(self.tl_array) # array of all the traffic lights
dll = lambda a, b: math.sqrt((a.x-b.x)**2 + (a.y-b.y)**2 + (a.z-b.z)**2)
dist = float("inf")
ind = 0
for i in range(len(self.tl_array)):
if dll(self.current_pose.position,self.tl_array[i].pose.pose.position) < dist :
dist = dll(self.current_pose.position,self.tl_array[i].pose.pose.position)
ind = i
#only report incoming traffic within visibility range, vb_range
if dist - self.pre_dist_tl < 0.05 and ind != self.pre_ind_tl and dist < self.vb_range:
#assign the to the next_tl
self.next_tl = self.tl_array[ind]
#print "tl_dist", dist, "state ",self.next_tl.state
# compute the traffic_light_waypoint_index
if len(self.all_waypoints) > 0 and (self.next_tl.state == TrafficLight.YELLOW or self.next_tl.state == TrafficLight.RED):
closest_distance_tl = float('inf')
closest_waypoint_tl = 0
for i in range(len(self.all_waypoints)):
this_distance = self.distance_to_position(self.all_waypoints, i, self.next_tl.pose.pose.position)
if this_distance < closest_distance_tl:
closest_distance_tl = this_distance
closest_waypoint_tl = i
self.traffic_light_waypoint_index = closest_waypoint_tl
else:
self.next_tl = TrafficLight()
self.next_tl.state = TrafficLight.UNKNOWN
else:
self.next_tl = TrafficLight()
self.next_tl.state = TrafficLight.UNKNOWN
if dist - self.pre_dist_tl > 0.01 and dist < self.vb_range:
self.pre_ind_tl = ind
#update previous distance, used to track incoming traffic light.
self.pre_dist_tl = 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 = TrafficLight()
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'] #assume that in real enviroment this is estimated in each loop
if (self.pose):
car_position = self.get_closest_waypoint(self.pose.pose)
#TODO find the closest visible traffic light (if one exists)
if (self.waypoints):
############################################################################################################
if self.loadtllidxflag == False:
for stop_line_position in stop_line_positions:
light.pose = PoseStamped()
light.pose.pose.position.x = stop_line_position[0]
light.pose.pose.position.y = stop_line_position[1]
light.pose.pose.position.z = 0
self.trafficlight_line_position.append(
self.get_closest_waypoint(light.pose.pose))
self.loadtllidxflag = True
############################################################################################################
idx_diff = lambda a: self.trafficlight_line_position[a] - car_position \
if self.trafficlight_line_position[a] >= car_position \
else len(self.waypoints.waypoints)-car_position+self.trafficlight_line_position[a]
tll_idx = min(range(len(self.trafficlight_line_position)),
key=idx_diff) #traffic light line index
light_wp = self.trafficlight_line_position[tll_idx]
if light:
state = self.get_light_state(light)
return light_wp, state
self.waypoints = None
return -1, TrafficLight.UNKNOWN
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
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)
#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]
check_traffic_light = TrafficLight()
check_traffic_light.pose = PoseStamped()
check_traffic_light.pose.pose.position.x = line[0]
check_traffic_light.pose.pose.position.y = line[1]
temp_wp_idx = self.get_closest_waypoint(check_traffic_light.pose.pose)
#Find closest stop line waypoint index
d = temp_wp_idx - car_wp_idx
if d >= 0 and d < diff:
diff = d
closest_light = light
light_wp_idx = temp_wp_idx
if closest_light and diff < 300:
state = self.get_light_state(closest_light)
return light_wp_idx, state
#self.waypoints = None
return -1, TrafficLight.UNKNOWN
def publish_traffic_light(self):
light_array = TrafficLightArray()
light_array.header.frame_id = "/world"
light = TrafficLight()
light.header.frame_id = "/world"
light.pose.pose.position.x = 1172.183
light.pose.pose.position.y = 1186.299
light.pose.pose.position.z = 5.576891
light.pose.pose.orientation.z = 0.00061619942315
light.pose.pose.orientation.w = 0.999999810149
light.state = TrafficLight.RED
light_array.lights.append(light)
light = TrafficLight()
light.header.frame_id = "/world"
light.pose.pose.position.x = 1272.183
light.pose.pose.position.y = 1286.299
light.pose.pose.position.z = 5.576891
light.pose.pose.orientation.z = 0.00061619942315
light.pose.pose.orientation.w = 0.999999810149
light.state = TrafficLight.GREEN
light_array.lights.append(light)
self.traffic_lights_pub.publish(light_array)
def get_classification(self, img):
"""Determines the color of the traffic light in the image
Args:
image (cv::Mat): image containing the traffic light
Returns:
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
#TODO implement light color prediction
resized = cv2.resize(img, (self.width, self.height))
resized = resized / 255.;
with self.graph.as_default():
predictions = self.model.predict(resized.reshape((1, self.height, self.width, self.channels)))
color = predictions[0].tolist().index(np.max(predictions[0]))
tl = TrafficLight()
tl.state = color
#return TrafficLight.UNKNOWN
return tl.state
