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 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 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 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 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):
"""
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 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 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 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 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 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 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 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
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
#
# process_traffic_lights() - use detection
# process_traffic_lights_test() - use info from /vehicle/traffic_lights topic
#
tl_info = self.process_traffic_lights()
#tl_info = self.process_traffic_lights_test()
'''
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.saved_tl_info.state != tl_info.state:
self.state_count = 0
self.saved_tl_info.state = tl_info.state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.saved_tl_info = tl_info
else:
tl_info = self.saved_tl_info
self.state_count += 1
# publish TL info
tl = TrafficLight()
tl.state = tl_info.state
tl.pose.pose.position.x = tl_info.x
tl.pose.pose.position.y = tl_info.y
tl.pose.pose.position.z = 0
self.upcoming_red_light_pub.publish(tl)
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
