class BasicAgent(object): """ BasicAgent implements an agent that navigates the scene. This agent respects traffic lights and other vehicles, but ignores stop signs. It has several functions available to specify the route that the agent must follow, as well as to change its parameters in case a different driving mode is desired. """ def __init__(self, vehicle, target_speed=20, opt_dict={}): """ Initialization the agent paramters, the local and the global planner. :param vehicle: actor to apply to agent logic onto :param target_speed: speed (in Km/h) at which the vehicle will move :param opt_dict: dictionary in case some of its parameters want to be changed. This also applies to parameters related to the LocalPlanner. """ self._vehicle = vehicle self._world = self._vehicle.get_world() self._map = self._world.get_map() self._last_traffic_light = None # Base parameters self._ignore_traffic_lights = False self._ignore_stop_signs = False self._ignore_vehicles = False self._target_speed = target_speed self._sampling_resolution = 2.0 self._base_tlight_threshold = 5.0 # meters self._base_vehicle_threshold = 5.0 # meters self._max_brake = 0.5 # Change parameters according to the dictionary opt_dict['target_speed'] = target_speed if 'ignore_traffic_lights' in opt_dict: self._ignore_traffic_lights = opt_dict['ignore_traffic_lights'] if 'ignore_stop_signs' in opt_dict: self._ignore_stop_signs = opt_dict['ignore_stop_signs'] if 'ignore_vehicles' in opt_dict: self._ignore_vehicles = opt_dict['ignore_vehicles'] if 'sampling_resolution' in opt_dict: self._sampling_resolution = opt_dict['sampling_resolution'] if 'base_tlight_threshold' in opt_dict: self._base_tlight_threshold = opt_dict['base_tlight_threshold'] if 'base_vehicle_threshold' in opt_dict: self._base_vehicle_threshold = opt_dict['base_vehicle_threshold'] if 'max_brake' in opt_dict: self._max_steering = opt_dict['max_brake'] # Initialize the planners self._local_planner = LocalPlanner(self._vehicle, opt_dict=opt_dict) self._global_planner = GlobalRoutePlanner(self._map, self._sampling_resolution) def add_emergency_stop(self, control): """ Overwrites the throttle a brake values of a control to perform an emergency stop. The steering is kept the same to avoid going out of the lane when stopping during turns :param speed (carl.VehicleControl): control to be modified """ control.throttle = 0.0 control.brake = self._max_brake control.hand_brake = False return control def set_target_speed(self, speed): """ Changes the target speed of the agent :param speed (float): target speed in Km/h """ self._local_planner.set_speed(speed) def follow_speed_limits(self, value=True): """ If active, the agent will dynamically change the target speed according to the speed limits :param value (bool): whether or not to activate this behavior """ self._local_planner.follow_speed_limits(value) def get_local_planner(self): """Get method for protected member local planner""" return self._local_planner def get_global_planner(self): """Get method for protected member local planner""" return self._global_planner def set_destination(self, end_location, start_location=None): """ This method creates a list of waypoints between a starting and ending location, based on the route returned by the global router, and adds it to the local planner. If no starting location is passed, the vehicle local planner's target location is chosen, which corresponds (by default), to a location about 5 meters in front of the vehicle. :param end_location (carla.Location): final location of the route :param start_location (carla.Location): starting location of the route """ if not start_location: start_location = self._local_planner.target_waypoint.transform.location clean_queue = True else: start_location = self._vehicle.get_location() clean_queue = False start_waypoint = self._map.get_waypoint(start_location) end_waypoint = self._map.get_waypoint(end_location) route_trace = self.trace_route(start_waypoint, end_waypoint) self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue) def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True): """ Adds a specific plan to the agent. :param plan: list of [carla.Waypoint, RoadOption] representing the route to be followed :param stop_waypoint_creation: stops the automatic random creation of waypoints :param clean_queue: resets the current agent's plan """ self._local_planner.set_global_plan( plan, stop_waypoint_creation=stop_waypoint_creation, clean_queue=clean_queue) def trace_route(self, start_waypoint, end_waypoint): """ Calculates the shortest route between a starting and ending waypoint. :param start_waypoint (carla.Waypoint): initial waypoint :param end_waypoint (carla.Waypoint): final waypoint """ start_location = start_waypoint.transform.location end_location = end_waypoint.transform.location return self._global_planner.trace_route(start_location, end_location) def run_step(self): """Execute one step of navigation.""" hazard_detected = False # Retrieve all relevant actors actor_list = self._world.get_actors() vehicle_list = actor_list.filter("*vehicle*") lights_list = actor_list.filter("*traffic_light*") vehicle_speed = get_speed(self._vehicle) / 3.6 # Check for possible vehicle obstacles max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed affected_by_vehicle, _, _ = self._vehicle_obstacle_detected( vehicle_list, max_vehicle_distance) if affected_by_vehicle: hazard_detected = True # Check if the vehicle is affected by a red traffic light max_tlight_distance = self._base_tlight_threshold + vehicle_speed affected_by_tlight, _ = self._affected_by_traffic_light( lights_list, max_tlight_distance) if affected_by_tlight: hazard_detected = True control = self._local_planner.run_step() if hazard_detected: control = self.add_emergency_stop(control) return control def done(self): """Check whether the agent has reached its destination.""" return self._local_planner.done() def ignore_traffic_lights(self, active=True): """(De)activates the checks for traffic lights""" self._ignore_traffic_lights = active def ignore_stop_signs(self, active=True): """(De)activates the checks for stop signs""" self._ignore_stop_signs = active def ignore_vehicles(self, active=True): """(De)activates the checks for stop signs""" self._ignore_vehicles = active def _affected_by_traffic_light(self, lights_list=None, max_distance=None): """ Method to check if there is a red light affecting the vehicle. :param lights_list (list of carla.TrafficLight): list containing TrafficLight objects. If None, all traffic lights in the scene are used :param max_distance (float): max distance for traffic lights to be considered relevant. If None, the base threshold value is used """ if self._ignore_traffic_lights: return (False, None) if not lights_list: lights_list = self._world.get_actors().filter("*traffic_light*") if not max_distance: max_distance = self._base_tlight_threshold if self._last_traffic_light: if self._last_traffic_light.state != carla.TrafficLightState.Red: self._last_traffic_light = None else: return (True, self._last_traffic_light) ego_vehicle_location = self._vehicle.get_location() ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) for traffic_light in lights_list: object_location = get_trafficlight_trigger_location(traffic_light) object_waypoint = self._map.get_waypoint(object_location) if object_waypoint.road_id != ego_vehicle_waypoint.road_id: continue ve_dir = ego_vehicle_waypoint.transform.get_forward_vector() wp_dir = object_waypoint.transform.get_forward_vector() dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z if dot_ve_wp < 0: continue if traffic_light.state != carla.TrafficLightState.Red: continue if is_within_distance(object_waypoint.transform, self._vehicle.get_transform(), max_distance, [0, 90]): self._last_traffic_light = traffic_light return (True, traffic_light) return (False, None) def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None, up_angle_th=90, low_angle_th=0, lane_offset=0): """ Method to check if there is a vehicle in front of the agent blocking its path. :param vehicle_list (list of carla.Vehicle): list contatining vehicle objects. If None, all vehicle in the scene are used :param max_distance: max freespace to check for obstacles. If None, the base threshold value is used """ if self._ignore_vehicles: return (False, None, -1) if not vehicle_list: vehicle_list = self._world.get_actors().filter("*vehicle*") if not max_distance: max_distance = self._base_vehicle_threshold ego_transform = self._vehicle.get_transform() ego_wpt = self._map.get_waypoint(self._vehicle.get_location()) # Get the right offset if ego_wpt.lane_id < 0 and lane_offset != 0: lane_offset *= -1 # Get the transform of the front of the ego ego_forward_vector = ego_transform.get_forward_vector() ego_extent = self._vehicle.bounding_box.extent.x ego_front_transform = ego_transform ego_front_transform.location += carla.Location( x=ego_extent * ego_forward_vector.x, y=ego_extent * ego_forward_vector.y, ) for target_vehicle in vehicle_list: target_transform = target_vehicle.get_transform() target_wpt = self._map.get_waypoint(target_transform.location, lane_type=carla.LaneType.Any) # Simplified version for outside junctions if not ego_wpt.is_junction or not target_wpt.is_junction: if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id + lane_offset: next_wpt = self._local_planner.get_incoming_waypoint_and_direction( steps=3)[0] if not next_wpt: continue if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id + lane_offset: continue target_forward_vector = target_transform.get_forward_vector() target_extent = target_vehicle.bounding_box.extent.x target_rear_transform = target_transform target_rear_transform.location -= carla.Location( x=target_extent * target_forward_vector.x, y=target_extent * target_forward_vector.y, ) if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [low_angle_th, up_angle_th]): return (True, target_vehicle, compute_distance(target_transform.location, ego_transform.location)) # Waypoints aren't reliable, check the proximity of the vehicle to the route else: route_bb = [] ego_location = ego_transform.location extent_y = self._vehicle.bounding_box.extent.y r_vec = ego_transform.get_right_vector() p1 = ego_location + carla.Location(extent_y * r_vec.x, extent_y * r_vec.y) p2 = ego_location + carla.Location(-extent_y * r_vec.x, -extent_y * r_vec.y) route_bb.append([p1.x, p1.y, p1.z]) route_bb.append([p2.x, p2.y, p2.z]) for wp, _ in self._local_planner.get_plan(): if ego_location.distance( wp.transform.location) > max_distance: break r_vec = wp.transform.get_right_vector() p1 = wp.transform.location + carla.Location( extent_y * r_vec.x, extent_y * r_vec.y) p2 = wp.transform.location + carla.Location( -extent_y * r_vec.x, -extent_y * r_vec.y) route_bb.append([p1.x, p1.y, p1.z]) route_bb.append([p2.x, p2.y, p2.z]) if len(route_bb) < 3: # 2 points don't create a polygon, nothing to check return (False, None, -1) ego_polygon = Polygon(route_bb) # Compare the two polygons for target_vehicle in vehicle_list: target_extent = target_vehicle.bounding_box.extent.x if target_vehicle.id == self._vehicle.id: continue if ego_location.distance( target_vehicle.get_location()) > max_distance: continue target_bb = target_vehicle.bounding_box target_vertices = target_bb.get_world_vertices( target_vehicle.get_transform()) target_list = [[v.x, v.y, v.z] for v in target_vertices] target_polygon = Polygon(target_list) if ego_polygon.intersects(target_polygon): return (True, target_vehicle, compute_distance(target_vehicle.get_location(), ego_location)) return (False, None, -1) return (False, None, -1)
class BasicAgent(Agent): """ BasicAgent implements a basic agent that navigates scenes to reach a given target destination. This agent respects traffic lights and other vehicles. """ def __init__(self, vehicle, target_speed=20): """ :param vehicle: actor to apply to local planner logic onto """ super(BasicAgent, self).__init__(vehicle) self.stopping_for_traffic_light = False self._proximity_threshold = 10.0 # meters self._state = AgentState.NAVIGATING args_lateral_dict = { 'K_P': 0.75, 'K_D': 0.001, 'K_I': 1, 'dt': 1.0 / 20.0 } self._local_planner = LocalPlanner(self._vehicle, opt_dict={ 'target_speed': target_speed, 'lateral_control_dict': args_lateral_dict }) self._hop_resolution = 2.0 self._path_seperation_hop = 2 self._path_seperation_threshold = 0.5 self._target_speed = target_speed self._grp = None self.drawn_lights = False self.is_affected_by_traffic_light = False def set_destination(self, location): """ This method creates a list of waypoints from agent's position to destination location based on the route returned by the global router """ start_waypoint = self._map.get_waypoint(self._vehicle.get_location()) end_waypoint = self._map.get_waypoint( carla.Location(location[0], location[1], location[2])) route_trace = self._trace_route(start_waypoint, end_waypoint) assert route_trace self._local_planner.set_global_plan(route_trace) def _trace_route(self, start_waypoint, end_waypoint): """ This method sets up a global router and returns the optimal route from start_waypoint to end_waypoint """ # Setting up global router if self._grp is None: dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() self._grp = grp # Obtain route plan route = self._grp.trace_route(start_waypoint.transform.location, end_waypoint.transform.location) return route def run_step(self, debug=False): """ Execute one step of navigation. :return: carla.VehicleControl """ # is there an obstacle in front of us? hazard_detected = False # retrieve relevant elements for safe navigation, i.e.: traffic lights # and other vehicles actor_list = self._world.get_actors() # type: ActorList vehicle_list = actor_list.filter("*vehicle*") # type: List[Actor] pedestrians_list = actor_list.filter("*walker.pedestrian*") lights_list = actor_list.filter( "*traffic_light*") # type: List[carla.TrafficLight] if not self.drawn_lights and debug: for light in lights_list: self._world.debug.draw_box( carla.BoundingBox( light.trigger_volume.location + light.get_transform().location, light.trigger_volume.extent * 2), carla.Rotation(0, 0, 0), 0.05, carla.Color(255, 128, 0, 0), 0) self.drawn_lights = True # check possible obstacles vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list) if vehicle_state: if debug: print('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id)) self._state = AgentState.BLOCKED_BY_VEHICLE hazard_detected = True # Check for pedestrians pedestrian_state, pedestrian = self._is_pedestrian_hazard( pedestrians_list) if pedestrian_state: if debug: print('!!! PEDESTRIAN BLOCKING AHEAD [{}])'.format( pedestrian.id)) self._state = AgentState.BLOCKED_BY_VEHICLE hazard_detected = True # check for the state of the traffic lights light_state, traffic_light = self._is_light_red(lights_list) if light_state: if debug: print('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id)) self._state = AgentState.BLOCKED_RED_LIGHT hazard_detected = True new_target_speed = self._update_target_speed(hazard_detected, debug) # if hazard_detected: # control = self.emergency_stop() # else: # self._state = AgentState.NAVIGATING # self.braking_intial_speed = None # # standard local planner behavior # control = self._local_planner.run_step(debug=debug) # if self.stopping_for_traffic_light: # control.steer = 0.0 self._state = AgentState.NAVIGATING self.braking_intial_speed = None # standard local planner behavior control = self._local_planner.run_step(debug=debug) if self.stopping_for_traffic_light: control.steer = 0.0 # Prevent from steering randomly when stopped if math.fabs(get_speed(self._vehicle)) < 0.1: control.steer = 0 return control def done(self): """ Check whether the agent has reached its destination. :return bool """ return self._local_planner.done() def _update_target_speed(self, hazard_detected, debug): if hazard_detected: self._set_target_speed(0) return 0 MAX_PERCENTAGE_OF_SPEED_LIMIT = 0.75 speed_limit = self._vehicle.get_speed_limit() # km/h current_speed = get_speed(self._vehicle) new_target_speed = speed_limit * MAX_PERCENTAGE_OF_SPEED_LIMIT use_custom_traffic_light_speed = False if use_custom_traffic_light_speed: TRAFFIC_LIGHT_SECONDS_AWAY = 3 METERS_TO_STOP_BEFORE_TRAFFIC_LIGHT = 8 get_traffic_light = self._vehicle.get_traffic_light( ) # type: carla.TrafficLight nearest_traffic_light, distance = get_nearest_traffic_light( self._vehicle) # type: carla.TrafficLight, float distance_to_light = distance distance -= METERS_TO_STOP_BEFORE_TRAFFIC_LIGHT if nearest_traffic_light is None: nearest_traffic_light = get_traffic_light # Draw debug info if debug and nearest_traffic_light is not None: self._world.debug.draw_point( nearest_traffic_light.get_transform().location, size=1, life_time=0.1, color=carla.Color(255, 15, 15)) """ if get_traffic_light is not None: print("get_traffic_light: ", get_traffic_light.get_location() if get_traffic_light is not None else "None", " ", get_traffic_light.state if get_traffic_light is not None else "None") if nearest_traffic_light is not None: print("nearest_traffic_light: ", nearest_traffic_light.get_location() if nearest_traffic_light is not None else "None", " ", nearest_traffic_light.state if nearest_traffic_light is not None else "None") """ ego_vehicle_location = self._vehicle.get_location() ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) self.is_affected_by_traffic_light = False self.stopping_for_traffic_light = False if ego_vehicle_waypoint.is_junction: # It is too late. Do not block the intersection! Keep going! pass # Check if we should start braking elif distance_to_light <= TRAFFIC_LIGHT_SECONDS_AWAY * new_target_speed / 3.6 and nearest_traffic_light is not None and nearest_traffic_light.state != carla.TrafficLightState.Green: self.is_affected_by_traffic_light = True brake_distance = current_speed / 3.6 * TRAFFIC_LIGHT_SECONDS_AWAY print("TL distance: ", distance_to_light, ", distance (to stop): ", distance, ", distance travel 4 secs: ", brake_distance) new_target_speed = self._target_speed if distance <= 0: new_target_speed = 0 self.stopping_for_traffic_light = True print("Stopping before traffic light, distance ", distance, "m") elif brake_distance >= distance and brake_distance != 0: percent_before_light = (brake_distance - distance) / brake_distance new_target_speed = speed_limit - max( 0, percent_before_light) * speed_limit print("Slowing down before traffic light ", percent_before_light * 100, "% ", new_target_speed, " km/h") self._set_target_speed(max(0, new_target_speed)) return new_target_speed def _set_target_speed(self, target_speed: int): """ This function updates all the needed values required to actually set a new target speed """ self._target_speed = target_speed self._local_planner.set_speed(target_speed)
class NavigationAssistant: def __init__(self, vehicle): self._vehicle = vehicle self._map = vehicle.get_world().get_map() args_lateral_dict = { 'K_P': 1, 'K_D': 0.4, 'K_I': 0, 'dt': 1.0/20.0} self._local_planner = LocalPlanner( self._vehicle, opt_dict={ 'target_speed' : 0, 'lateral_control_dict' :args_lateral_dict } ) self._hop_resolution = 2.0 self._grp = self._initialize_global_route_planner() # Initializes the global route planner. def _initialize_global_route_planner(self): dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution) grp = GlobalRoutePlanner(dao) grp.setup() return grp # Sets vehicle's speed. def set_speed(self, target_speed): self._local_planner.set_speed(target_speed) # Sets vehicle's destination & computes the optimal route towards the destination. def set_destination(self, location): start_waypoint = self._map.get_waypoint( self._vehicle.get_location() ) end_waypoint = self._map.get_waypoint(location) # Computes the optimal route for the starting location. route_trace = self._grp.trace_route(start_waypoint.transform.location, end_waypoint.transform.location) self._local_planner.set_global_plan(route_trace) # Executes one step of navigation. def run_step(self, hazard_detected, debug=False): if hazard_detected: control = self._emergency_stop() else: control = self._local_planner.run_step(debug=debug) return control # Checks whether the vehicle reached its destination. def done(self): return self._local_planner.done() # Returns a control that forces the vehicle to stop. @staticmethod def _emergency_stop(): control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False return control