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 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