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