diff --git a/PythonAPI/carla/agents/navigation/basic_agent.py b/PythonAPI/carla/agents/navigation/basic_agent.py index d6ee74293..d2a1a11e5 100644 --- a/PythonAPI/carla/agents/navigation/basic_agent.py +++ b/PythonAPI/carla/agents/navigation/basic_agent.py @@ -54,7 +54,6 @@ class BasicAgent(Agent): end_waypoint = self._map.get_waypoint(location) route_trace = self.trace_route(start_waypoint, end_waypoint) - self._local_planner.set_global_plan(route_trace) def trace_route(self, start_waypoint, end_waypoint): diff --git a/PythonAPI/carla/agents/navigation/behavior_agent.py b/PythonAPI/carla/agents/navigation/behavior_agent.py index c2f25a662..84055015a 100644 --- a/PythonAPI/carla/agents/navigation/behavior_agent.py +++ b/PythonAPI/carla/agents/navigation/behavior_agent.py @@ -112,8 +112,7 @@ class BehaviorAgent(Agent): self.end_waypoint = self._map.get_waypoint(end_location) route_trace = self.trace_route(self.start_waypoint, self.end_waypoint) - - self._local_planner.set_global_plan(route_trace) + self._local_planner.set_global_plan(route_trace, stop_waypoint_creation=False) def reroute(self, spawn_points): """ @@ -337,21 +336,29 @@ class BehaviorAgent(Agent): # Under safety time distance, slow down. if self.behavior.safety_time > ttc > 0.0: + target_speed = min([ + positive(vehicle_speed - self.behavior.speed_decrease), + self.behavior.max_speed, + self.speed_limit - self.behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) - # control = self._local_planner.run_step( - # target_speed=min(positive(vehicle_speed - self.behavior.speed_decrease), - # min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist)), debug=debug) + # Actual safety distance area, try to follow the speed of the vehicle in front. elif 2 * self.behavior.safety_time > ttc >= self.behavior.safety_time: + target_speed = min([ + max(self.min_speed, vehicle_speed), + self.behavior.max_speed, + self.speed_limit - self.behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) - # control = self._local_planner.run_step( - # target_speed=min(max(self.min_speed, vehicle_speed), - # min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist)), debug=debug) + # Normal behavior. else: + target_speed = min([ + self.behavior.max_speed, + self.speed_limit - self.behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) - # control = self._local_planner.run_step( - # target_speed= min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug) return control @@ -362,7 +369,6 @@ class BehaviorAgent(Agent): :param debug: boolean for debugging :return control: carla.VehicleControl """ - # TODO: Everytime the local_planner run step is called, reset the speed control = None if self.behavior.tailgate_counter > 0: self.behavior.tailgate_counter -= 1 @@ -373,12 +379,10 @@ class BehaviorAgent(Agent): ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) # 1: Red lights and stops behavior - if self.traffic_light_manager(ego_vehicle_wp) != 0: return self.emergency_stop() - # 2.1: Pedestrian avoidancd behaviors - + # 2.1: Pedestrian avoidance behaviors walker_state, walker, w_distance = self.pedestrian_avoid_manager( ego_vehicle_loc, ego_vehicle_wp) @@ -410,20 +414,20 @@ class BehaviorAgent(Agent): else: control = self.car_following_manager(vehicle, distance) - # 4: Intersection behavior - - # Checking if there's a junction nearby to slow down - elif self.incoming_waypoint.is_junction and (self.incoming_direction == RoadOption.LEFT or self.incoming_direction == RoadOption.RIGHT): + # 3: Intersection behavior + elif self.incoming_waypoint.is_junction and (self.incoming_direction in [RoadOption.LEFT, RoadOption.RIGHT]): + target_speed = min([ + self.behavior.max_speed, + self.speed_limit - 5]) + self._local_planner.set_speed(target_speed) control = self._local_planner.run_step(debug=debug) - # control = self._local_planner.run_step( - # target_speed=min(self.behavior.max_speed, self.speed_limit - 5), debug=debug) - # 5: Normal behavior - - # Calculate controller based on no turn, traffic light or vehicle in front + # 4: Normal behavior else: - control = self._local_planner.run_step(debug=debug) - # control = self._local_planner.run_step( - # target_speed= min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug) + target_speed = min([ + self.behavior.max_speed, + self.speed_limit - self.behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) return control diff --git a/PythonAPI/carla/agents/navigation/local_planner.py b/PythonAPI/carla/agents/navigation/local_planner.py index f1766ecb2..4420ac2b7 100644 --- a/PythonAPI/carla/agents/navigation/local_planner.py +++ b/PythonAPI/carla/agents/navigation/local_planner.py @@ -29,17 +29,16 @@ class RoadOption(Enum): class LocalPlanner(object): """ - LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly. - The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control - and the other for the longitudinal control (cruise speed). + LocalPlanner implements the basic behavior of following a + trajectory of waypoints that is generated on-the-fly. - When multiple paths are available (intersections) this local planner makes a random choice. + The low-level motion of the vehicle is computed by using two PID controllers, + one is used for the lateral control and the other for the longitudinal control (cruise speed). + + When multiple paths are available (intersections) this local planner makes a random choice, + unless a given global plan has already been specified. """ - # minimum distance to target waypoint as a percentage (e.g. within 90% of - # total distance) - MIN_DISTANCE_PERCENTAGE = 0.9 - def __init__(self, vehicle, opt_dict=None): """ :param vehicle: actor to apply to local planner logic onto @@ -58,22 +57,34 @@ class LocalPlanner(object): {'K_P':, 'K_D':, 'K_I':, 'dt'} """ self._vehicle = vehicle - self._map = self._vehicle.get_world().get_map() + self._world = self._vehicle.get_world() + self._map = self._world.get_map() + + self._target_speed = 20.0 # Km/h + self._sampling_radius = 1.0 + self._min_distance = 5.0 + self._dt = 1.0 / 20.0 + self._max_brake = 0.3 + self._max_throt = 0.75 + self._max_steer = 0.8 + self._offset = 0 + + self._args_lateral_dict = {'K_P': 1.95, 'K_D': 0.2, 'K_I': 0.07, 'dt': self._dt} + self._args_longitudinal_dict = {'K_P': 1.0, 'K_D': 0, 'K_I': 0.05, 'dt': self._dt} + # self.args_lat_hw_dict = {'K_P': 0.75, 'K_D': 0.02, 'K_I': 0.4, 'dt': 1.0 / self.FPS} + # self.args_lat_city_dict = {'K_P': 0.58, 'K_D': 0.02, 'K_I': 0.5, 'dt': 1.0 / self.FPS} + # self.args_long_hw_dict = {'K_P': 0.37, 'K_D': 0.024, 'K_I': 0.032, 'dt': 1.0 / self.FPS} + # self.args_long_city_dict = {'K_P': 0.15, 'K_D': 0.05, 'K_I': 0.07, 'dt': 1.0 / self.FPS} - self._dt = None - self._target_speed = None - self._sampling_radius = None - self._min_distance = None - self._current_waypoint = None - self.target_road_option = None - self._next_waypoints = None - self.target_waypoint = None self._vehicle_controller = None - self._stop_waypoint_creation = None - # queue with tuples of (waypoint, RoadOption) - self._waypoints_queue = deque(maxlen=20000) - self._buffer_size = 5 - self._waypoint_buffer = deque(maxlen=self._buffer_size) + self.target_waypoint = None + self.target_road_option = None + + self._waypoints_queue = deque(maxlen=10000) + self._min_waypoint_queue_length = 100 + + self._stop_waypoint_creation = False + self._follow_speed_limits = False # initializing controller self._init_controller(opt_dict) @@ -81,7 +92,6 @@ class LocalPlanner(object): def __del__(self): if self._vehicle: self._vehicle.destroy() - print("Destroying ego-vehicle!") def reset_vehicle(self): """Reset the ego-vehicle""" @@ -94,46 +104,6 @@ class LocalPlanner(object): :param opt_dict: dictionary of arguments. :return: """ - # default params - self._dt = 1.0 / 20.0 - self._target_speed = 20.0 # Km/h - self._sampling_radius = self._target_speed * 1 / 3.6 # 1 seconds horizon - self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE - self._max_brake = 0.3 - self._max_throt = 0.75 - self._max_steer = 0.8 - args_lateral_dict = { - 'K_P': 1.95, - 'K_D': 0.2, - 'K_I': 0.07, - 'dt': self._dt} - args_longitudinal_dict = { - 'K_P': 1.0, - 'K_D': 0, - 'K_I': 0.05, - 'dt': self._dt} - # self.args_lat_hw_dict = { - # 'K_P': 0.75, - # 'K_D': 0.02, - # 'K_I': 0.4, - # 'dt': 1.0 / self.FPS} - # self.args_lat_city_dict = { - # 'K_P': 0.58, - # 'K_D': 0.02, - # 'K_I': 0.5, - # 'dt': 1.0 / self.FPS} - # self.args_long_hw_dict = { - # 'K_P': 0.37, - # 'K_D': 0.024, - # 'K_I': 0.032, - # 'dt': 1.0 / self.FPS} - # self.args_long_city_dict = { - # 'K_P': 0.15, - # 'K_D': 0.05, - # 'K_I': 0.07, - # 'dt': 1.0 / self.FPS} - self._offset = 0 - # parameters overload if opt_dict: if 'dt' in opt_dict: @@ -141,12 +111,11 @@ class LocalPlanner(object): if 'target_speed' in opt_dict: self._target_speed = opt_dict['target_speed'] if 'sampling_radius' in opt_dict: - self._sampling_radius = self._target_speed * \ - opt_dict['sampling_radius'] / 3.6 + self._sampling_radius = opt_dict['sampling_radius'] if 'lateral_control_dict' in opt_dict: - args_lateral_dict = opt_dict['lateral_control_dict'] + self._args_lateral_dict = opt_dict['lateral_control_dict'] if 'longitudinal_control_dict' in opt_dict: - args_longitudinal_dict = opt_dict['longitudinal_control_dict'] + self._args_longitudinal_dict = opt_dict['longitudinal_control_dict'] if 'max_throttle' in opt_dict: self._max_throt = opt_dict['max_throttle'] if 'max_brake' in opt_dict: @@ -156,31 +125,46 @@ class LocalPlanner(object): if 'offset' in opt_dict: self._offset = opt_dict['offset'] - self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) self._vehicle_controller = VehiclePIDController(self._vehicle, - args_lateral=args_lateral_dict, - args_longitudinal=args_longitudinal_dict, + args_lateral=self._args_lateral_dict, + args_longitudinal=self._args_longitudinal_dict, offset=self._offset, max_throttle=self._max_throt, max_brake=self._max_brake, max_steering=self._max_steer) - self._stop_waypoint_creation = False + # Compute the current vehicle waypoint + current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + self.target_waypoint, self.target_road_option = (current_waypoint, RoadOption.LANEFOLLOW) + print("Starting position: {}".format(self.target_waypoint)) - # compute initial waypoints - self._waypoints_queue.append((self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW)) + # Fill the waypoint queue + self._waypoints_queue.append((self.target_waypoint, self.target_road_option)) + self._compute_next_waypoints(k=self._min_waypoint_queue_length) - # fill waypoint trajectory queue - self._compute_next_waypoints(k=200) + self._world.debug.draw_point(self.target_waypoint.transform.location + carla.Location(z=1), life_time=20, size=0.5) def set_speed(self, speed): """ - Request new target speed. + Changes the target speed :param speed: new target speed in Km/h :return: """ + if self._follow_speed_limits: + print("WARNING: The max speed is currently set to follow the speed limits. " + "Use 'follow_speed_limits' to deactivate this") self._target_speed = speed + # TODO: Change the sampling distance too? + + def follow_speed_limits(self, value=True): + """ + Activates a flag that makes the max speed dynamically vary according to the spped limits + + :param value: bool + :return: + """ + self._follow_speed_limits = value def _compute_next_waypoints(self, k=1): """ @@ -213,30 +197,23 @@ class LocalPlanner(object): self._waypoints_queue.append((next_waypoint, road_option)) - def set_global_plan(self, current_plan, stop_waypoint_creation=True): + def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True): """ - Resets the waypoint queue and buffer to match the new plan. Also - handles the stop_waypoint_creation flag to avoid creating more waypoints + Adds a new plan to the local planner. + If 'clean_queue`, erases the previous plan, and if note, it is added to the old one + The 'stop_waypoint_creation' flag avoids creating more random waypoints :param current_plan: list of (carla.Waypoint, RoadOption) :param stop_waypoint_creation: bool + :param ceal_queue: bool :return: """ + if clean_queue: + self._waypoints_queue.clear() - # Reset the queue - self._waypoints_queue.clear() for elem in current_plan: self._waypoints_queue.append(elem) - # and the buffer - self._waypoint_buffer.clear() - for _ in range(self._buffer_size): - if self._waypoints_queue: - self._waypoint_buffer.append( - self._waypoints_queue.popleft()) - else: - break - self._stop_waypoint_creation = stop_waypoint_creation def run_step(self, debug=False): @@ -247,48 +224,38 @@ class LocalPlanner(object): :param debug: boolean flag to activate waypoints debugging :return: control to be applied """ + if self._follow_speed_limits: + self._target_speed = self._vehicle.get_speed_limit() # Add more waypoints too few in the horizon - if not self._stop_waypoint_creation and len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5): - self._compute_next_waypoints(k=100) + if not self._stop_waypoint_creation and len(self._waypoints_queue) < self._min_waypoint_queue_length: + self._compute_next_waypoints(k=self._min_waypoint_queue_length) - if len(self._waypoints_queue) == 0 and len(self._waypoint_buffer) == 0: + if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False - return control - # Buffering the waypoints - if not self._waypoint_buffer: - for _ in range(self._buffer_size): - if self._waypoints_queue: - self._waypoint_buffer.append( - self._waypoints_queue.popleft()) - else: - break - - # Current vehicle waypoint - vehicle_transform = self._vehicle.get_transform() - self._current_waypoint = self._map.get_waypoint(vehicle_transform.location) - # Target waypoint - self.target_waypoint, self.target_road_option = self._waypoint_buffer[0] - - # Move using PID controllers + # Get the target waypoint and move using the PID controllers + self.target_waypoint, self.target_road_option = self._waypoints_queue[0] control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) - # Purge the queue of obsolete waypoints - max_index = -1 + # # Purge the queue of obsolete waypoints + veh_location = self._vehicle.get_location() + num_waypoint_removed = 0 + for waypoint, _ in self._waypoints_queue: + if veh_location.distance(waypoint.transform.location) < self._min_distance: + num_waypoint_removed += 1 + else: + break - for i, (waypoint, _) in enumerate(self._waypoint_buffer): - if waypoint.transform.location.distance(vehicle_transform.location) < self._min_distance: - max_index = i - if max_index >= 0: - for i in range(max_index + 1): - self._waypoint_buffer.popleft() + if num_waypoint_removed > 0: + for _ in range(num_waypoint_removed): + self._waypoints_queue.popleft() if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) @@ -311,14 +278,13 @@ class LocalPlanner(object): except IndexError as i: return None, RoadOption.VOID - def done(self): """ Returns whether or not the planner has finished :return: boolean """ - return len(self._waypoints_queue) == 0 and len(self._waypoint_buffer) == 0 + return len(self._waypoints_queue) == 0 def _retrieve_options(list_waypoints, current_waypoint): """ diff --git a/PythonAPI/examples/automatic_control.py b/PythonAPI/examples/automatic_control.py index a7b1dbc05..5936dfb96 100755 --- a/PythonAPI/examples/automatic_control.py +++ b/PythonAPI/examples/automatic_control.py @@ -173,6 +173,7 @@ class World(object): actor.apply_physics_control(physics_control) except Exception: pass + def tick(self, clock): """Method for every tick""" self.hud.tick(self, clock) @@ -695,22 +696,18 @@ def game_loop(args): world = World(client.get_world(), hud, args) controller = KeyboardControl(world) + spawn_points = world.map.get_spawn_points() + random.shuffle(spawn_points) + if spawn_points[0].location != world.player.get_location(): + destination = spawn_points[0].location + else: + destination = spawn_points[1].location + if args.agent == "Basic": agent = BasicAgent(world.player) - spawn_point = world.map.get_spawn_points()[0] - agent.set_destination(spawn_point.location) + agent.set_destination(destination) else: - # TODO: Change PIDS and target speed to make it on spped limti agent = BehaviorAgent(world.player, behavior=args.behavior) - - spawn_points = world.map.get_spawn_points() - random.shuffle(spawn_points) - - if spawn_points[0].location != agent.vehicle.get_location(): - destination = spawn_points[0].location - else: - destination = spawn_points[1].location - agent.set_destination(agent.vehicle.get_location(), destination) clock = pygame.time.Clock() @@ -719,31 +716,17 @@ def game_loop(args): clock.tick_busy_loop(60) if controller.parse_events(): return - - # As soon as the server is ready continue! if not world.world.wait_for_tick(10.0): continue - if args.agent == "Basic": - if controller.parse_events(): - return + world.world.wait_for_tick(10.0) + world.tick(clock) + world.render(display) + pygame.display.flip() - # as soon as the server is ready continue! - world.world.wait_for_tick(10.0) - - world.tick(clock) - world.render(display) - pygame.display.flip() - control = agent.run_step() - control.manual_gear_shift = False - world.player.apply_control(control) - else: + if args.agent == "Behavior": agent.update_information() - world.tick(clock) - world.render(display) - pygame.display.flip() - # Set new destination when target has been reached if len(agent.get_local_planner()._waypoints_queue) < num_min_waypoints and args.loop: agent.reroute(spawn_points) @@ -758,8 +741,9 @@ def game_loop(args): speed_limit = world.player.get_speed_limit() agent.get_local_planner().set_speed(speed_limit) - control = agent.run_step() - world.player.apply_control(control) + control = agent.run_step() + control.manual_gear_shift = False + world.player.apply_control(control) finally: if world is not None: