def _is_light_red_us_style(self, lights_list, debug=False): ego_vehicle_location = self._vehicle.get_location() ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) traffic_light_color = "NONE" # default, if no traffic lights are seen if ego_vehicle_waypoint.is_junction: # It is too late. Do not block the intersection! Keep going! return "JUNCTION" if self._local_planner.target_waypoint is not None: if self._local_planner.target_waypoint.is_junction: min_angle = 180.0 sel_magnitude = 0.0 sel_traffic_light = None for traffic_light in lights_list: loc = traffic_light.get_location() magnitude, angle = compute_magnitude_angle( loc, ego_vehicle_location, self._vehicle.get_transform().rotation.yaw, ) if magnitude < 60.0 and angle < min(25.0, min_angle): sel_magnitude = magnitude sel_traffic_light = traffic_light min_angle = angle if sel_traffic_light is not None: if debug: print( "=== Magnitude = {} | Angle = {} | ID = {}".format( sel_magnitude, min_angle, sel_traffic_light.id)) if self._last_traffic_light is None: self._last_traffic_light = sel_traffic_light if self._last_traffic_light.state == carla.TrafficLightState.Red: return "RED" elif (self._last_traffic_light.state == carla.TrafficLightState.Yellow): traffic_light_color = "YELLOW" elif (self._last_traffic_light.state == carla.TrafficLightState.Green): if traffic_light_color is not "YELLOW": # (more severe) traffic_light_color = "GREEN" else: import pdb pdb.set_trace() # investigate https://carla.readthedocs.io/en/latest/python_api/#carlatrafficlightstate else: self._last_traffic_light = None return traffic_light_color
def _is_light_red_us_style(self, lights_list, debug=False): """ This method is specialized to check US style traffic lights. :param lights_list: list containing TrafficLight objects :return: a tuple given by (bool_flag, traffic_light), where - bool_flag is True if there is a traffic light in RED affecting us and False otherwise - traffic_light is the object itself or None if there is no red traffic light affecting us """ ego_vehicle_location = self._vehicle.get_location() ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) if ego_vehicle_waypoint.is_intersection: # It is too late. Do not block the intersection! Keep going! return (False, None) if self._local_planner._target_waypoint is not None: if self._local_planner._target_waypoint.is_intersection: potential_lights = [] min_angle = 180.0 sel_magnitude = 0.0 sel_traffic_light = None for traffic_light in lights_list: loc = traffic_light.get_location() magnitude, angle = compute_magnitude_angle( loc, ego_vehicle_location, self._vehicle.get_transform().rotation.yaw) if magnitude < 80.0 and angle < min(25.0, min_angle): sel_magnitude = magnitude sel_traffic_light = traffic_light min_angle = angle if sel_traffic_light is not None: if debug: print( '=== Magnitude = {} | Angle = {} | ID = {}'.format( sel_magnitude, min_angle, sel_traffic_light.id)) if self._last_traffic_light is None: self._last_traffic_light = sel_traffic_light if self._last_traffic_light.state == carla.libcarla.TrafficLightState.Red: return (True, self._last_traffic_light) print(traffic_light.state) else: self._last_traffic_light = None return (False, None)
def _is_light_red_us_style(self, lights_list, debug=False): """This method is specialized to check US style traffic lights.""" ego_vehicle_location = self._vehicle.get_location() ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) if ego_vehicle_waypoint.is_junction: # It is too late. Do not block the intersection! Keep going! return (False, None) if self._local_planner.target_waypoint is not None: if self._local_planner.target_waypoint.is_junction: min_angle = 180.0 sel_magnitude = 0.0 sel_traffic_light = None for traffic_light in lights_list: loc = traffic_light.get_location() magnitude, angle = compute_magnitude_angle( loc, ego_vehicle_location, self._vehicle.get_transform().rotation.yaw) if magnitude < 60.0 and angle < min(25.0, min_angle): sel_magnitude = magnitude sel_traffic_light = traffic_light min_angle = angle if sel_traffic_light is not None: if debug: logging.debug( '=== Magnitude = {} | Angle = {} | ID = {}'.format( sel_magnitude, min_angle, sel_traffic_light.id)) if self._last_traffic_light is None: self._last_traffic_light = sel_traffic_light if self._last_traffic_light.state == carla.TrafficLightState.Red: # pylint: disable=no-member return (True, self._last_traffic_light) else: self._last_traffic_light = None return (False, None)
def run_step(self, debug=True): # not enough waypoints in the horizon? => add more! if len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): if not self._global_plan: self._compute_next_waypoints(k=100) if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 0.0 control.hand_brake = False control.manual_gear_shift = False return control veh_location = self._vehicle.get_location() # type: carla.Location veh_waypoint = self._map.get_waypoint( veh_location) # type: carla.Waypoint veh_yaw = self._vehicle.get_transform( ).rotation.yaw # TODO type: float, range TODO local_plan = self.get_filled_waypoint_buffer( ) # type: List[carla.Waypoint] # Calculate best waypoint to follow considering current yaw L = 2.9 fx = veh_location.x + L * np.cos(veh_yaw) fy = veh_location.y + L * np.sin(veh_yaw) distances = [] for waypoint in local_plan: wp = waypoint.transform.location dx = fx - wp.x dy = fy - wp.y distance = np.sqrt(dx**2 + dy**2) distances.append(distance) target_idx = np.argmin(distances) closest_error = distances[target_idx] self._target_waypoint = local_plan[target_idx] # Calculate path curvature waypoints_to_look_ahead = 6 reference_waypoint = local_plan[target_idx + waypoints_to_look_ahead] ref_location = reference_waypoint.transform.location # delta_x = ref_location.x - veh_location.x # delta_y = ref_location.y - veh_location.y # theta_radians = math.atan2(delta_y, delta_x) # FIXME Sometimes yaw oscilates from 179 to -179 which leads to temporarily bad calculations distance, relative_angle = compute_magnitude_angle( ref_location, veh_location, veh_yaw) #np.rad2deg(theta_radians) - veh_yaw # plt.cla() # plt.plot([self._vehicle.get_transform().location.x, lookahead_waypoint.transform.location.x], [self._vehicle.get_transform().location.y, lookahead_waypoint.transform.location.y], "-r", label="debug") # # plt.plot(, , ".b", label="lookahead") # plt.axis("equal") # plt.legend() # plt.grid(True) # plt.title("Rel angle: {}, yaw {}".format(str(angle), yaw)) # plt.pause(0.0001) if abs(relative_angle) < 15: target_speed = 50 elif abs(relative_angle) < 20: target_speed = 40 elif abs(relative_angle) < 30: target_speed = 30 else: target_speed = 20 print( 'Relative angle to reference waypoint: {:3d} | Vehicle yaw angle: {:3d} | Target speed {} km/h' .format(int(relative_angle), int(veh_yaw), target_speed)) control = self._vehicle_controller.run_step(target_speed, self._target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 # i, (waypoint, _) for i, waypoint in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() if debug: #draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0, color=carla.Color(0, 255, 0)) draw_waypoints(self._vehicle.get_world(), [reference_waypoint], veh_location.z + 1.0) return control