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 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 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 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
light.header.stamp = rospy.Time.now() light.header.stamp = rospy.Time.now()
light.header.stamp = rospy.Time.now()
light.header.stamp = rospy.Time.now()
light.header.stamp = rospy.Time.now()
return light
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 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 create_light(self, x, y, z, yaw, state):
"""Creates a new TrafficLight object
Args:
x (float): x coordinate of light
y (float): y coordinate of light
z (float): z coordinate of light
yaw (float): angle of light around z axis
state (int): ID of traffic light color (specified in styx_msgs/TrafficLight)
Returns:
light (TrafficLight): new TrafficLight object
"""
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 light_loc(self, state, lx, ly, lz, lyaw):
# light state initialization
light = TrafficLight()
#
light.state = state
# header position
light.header = Header()
light.header.stamp = rospy.Time.now()
light.header.frame_id = 'world'
# pose position
light.pose = PoseStamped()
light.pose.header.stamp = rospy.Time.now()
light.pose.header.frame_id = 'world'
light.pose.pose.position.x = lx
light.pose.pose.position.y = ly
light.pose.pose.position.z = lz
q_from_euler = tf.transformations.quaternion_from_euler(
0.0, 0.0, math.pi * lyaw / 180.0)
light.pose.pose.orientation = Quaternion(*q_from_euler)
return 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)
"""
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']
#TODO find the closest visible traffic light (if one exists)
if(self.pose and self.waypoints):
car_position = self.get_closest_waypoint(self.pose.pose,self.waypoints.waypoints)
light_position = self.get_closest_waypoint(self.pose.pose, self.lights)
light_wp = self.get_closest_waypoint(self.lights[light_position].pose.pose, self.waypoints.waypoints)
stop_lines = list()
for line_pos in stop_line_positions:
# create pseudo TrafficLight object for easy handling
line = TrafficLight()
line.pose = PoseStamped()
line.pose.pose.position.x = line_pos[0]
line.pose.pose.position.y = line_pos[1]
line.pose.pose.position.z = 0.0
stop_lines.append(line)
line_wp = self.get_closest_waypoint(stop_lines[light_position].pose.pose, self.waypoints.waypoints)
state = self.get_light_state(self.lights[light_position])
return line_wp, state
if light:
state = self.get_light_state(light)
return light_wp, state
return -1, TrafficLight.UNKNOWN
def __init__(self):
rospy.init_node('tl_detector')
print os.getcwd()
print(os.path.dirname(os.path.realpath(__file__)))
MODEL_NAME = 'light_classification/inferencemodel'
self.model_path = MODEL_NAME + '/frozen_inference_graph.pb'
PATH_TO_LABELS = 'light_classification/training_setup/object-detection.pbtxt'
# Build the model
self.detection_graph = tflow.Graph()
# create config
config = tflow.ConfigProto()
# Create the graph
with self.detection_graph.as_default():
self.graph_def = tflow.GraphDef()
with tflow.gfile.GFile(self.model_path, 'rb') as fid:
serialized_graph = fid.read()
self.graph_def.ParseFromString(serialized_graph)
tflow.import_graph_def(self.graph_def, name='')
rospy.loginfo('Loaded frozen tensorflow model: %s',
self.model_path)
# Create a reusable sesion attribute
# self.sess = tflow.Session(graph=self.detection_graph, config=config)
rospy.logwarn("TL detect: init")
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
self.has_wps = False
self.prev_pose = None
self.heading = -3 * math.pi
# 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']
self.stop_lines = []
for sl_pos in stop_line_positions:
a_light = TrafficLight()
a_light.pose = PoseStamped()
a_light.pose.pose.position.x = sl_pos[0]
a_light.pose.pose.position.y = sl_pos[1]
a_light.pose.pose.position.z = 0
self.stop_lines.append(a_light)
update_rate = rospy.Rate(5)
while not rospy.is_shutdown():
update_rate.sleep()
