# Adapted from code licensed under:
# Copyright (c) # Copyright (c) 2018-2020 CVC.
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
""" This module contains a local planner to perform low-level waypoint following based on PID controllers. """
from enum import Enum
from collections import deque
import random
import carla
from igp2.carlasim.controller import VehiclePIDController
from igp2.carlasim.util import draw_waypoints, get_speed
[docs]class RoadOption(Enum):
"""
RoadOption represents the possible topological configurations when moving from a segment of lane to other.
"""
VOID = -1
LEFT = 1
RIGHT = 2
STRAIGHT = 3
LANEFOLLOW = 4
CHANGELANELEFT = 5
CHANGELANERIGHT = 6
[docs]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).
When multiple paths are available (intersections) this local planner makes a random choice,
unless a given global plan has already been specified.
"""
def __init__(self,
vehicle: carla.Actor,
world: carla.World,
map: carla.Map,
dt: float = 0.05, **kwargs):
"""
Args:
vehicle: actor to apply to local planner logic onto
Keyword Args:
target_speed: desired cruise speed in Km/h
sampling_radius: distance between the waypoints part of the plan
lateral_control_dict: values of the lateral PID controller
longitudinal_control_dict: values of the longitudinal PID controller
max_throttle: maximum throttle applied to the vehicle
max_brake: maximum brake applied to the vehicle
max_steering: maximum steering applied to the vehicle
offset: distance between the route waypoints and the center of the lane
"""
self._vehicle = vehicle
self._world = world
self._map = map
self._vehicle_controller = None
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
# Base parameters
self._dt = dt
self._target_speed = 20.0 # Km/h
self._sampling_radius = 2.0
self._args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': self._dt}
self._args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0.0, 'dt': self._dt}
self._max_throt = 0.75
self._max_brake = 0.3
self._max_steer = 0.8
self._offset = 0
self._base_min_distance = 3.0
self._follow_speed_limits = False
# Overload parameters
if kwargs is not None:
if 'target_speed' in kwargs:
self._target_speed = kwargs['target_speed']
if 'sampling_radius' in kwargs:
self._sampling_radius = kwargs['sampling_radius']
if 'lateral_control_dict' in kwargs:
self._args_lateral_dict = kwargs['lateral_control_dict']
if 'longitudinal_control_dict' in kwargs:
self._args_longitudinal_dict = kwargs['longitudinal_control_dict']
if 'max_throttle' in kwargs:
self._max_throt = kwargs['max_throttle']
if 'max_brake' in kwargs:
self._max_brake = kwargs['max_brake']
if 'max_steering' in kwargs:
self._max_steer = kwargs['max_steering']
if 'offset' in kwargs:
self._offset = kwargs['offset']
if 'base_min_distance' in kwargs:
self._base_min_distance = kwargs['base_min_distance']
if 'follow_speed_limits' in kwargs:
self._follow_speed_limits = kwargs['follow_speed_limits']
# initializing controller
self._init_controller()
[docs] def reset_vehicle(self):
"""Reset the ego-vehicle"""
self._vehicle = None
def _init_controller(self):
"""Controller initialization"""
self._vehicle_controller = VehiclePIDController(self._vehicle,
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)
# 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)
self._waypoints_queue.append((self.target_waypoint, self.target_road_option))
[docs] def set_speed(self, speed):
"""
Changes the target speed
Args:
speed: new target speed in Km/h
"""
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
[docs] def follow_speed_limits(self, value: bool = True):
"""
Activates a flag that makes the max speed dynamically vary according to the spped limits
Args:
value: Whether to follow speed limit or not.
"""
self._follow_speed_limits = value
def _compute_next_waypoints(self, k=1):
"""
Add new waypoints to the trajectory queue.
:param k: how many waypoints to compute
:return:
"""
# check we do not overflow the queue
available_entries = self._waypoints_queue.maxlen - len(self._waypoints_queue)
k = min(available_entries, k)
for _ in range(k):
last_waypoint = self._waypoints_queue[-1][0]
next_waypoints = list(last_waypoint.next(self._sampling_radius))
if len(next_waypoints) == 0:
break
elif len(next_waypoints) == 1:
# only one option available ==> lanefollowing
next_waypoint = next_waypoints[0]
road_option = RoadOption.LANEFOLLOW
else:
# random choice between the possible options
road_options_list = _retrieve_options(next_waypoints, last_waypoint)
road_option = random.choice(road_options_list)
next_waypoint = next_waypoints[road_options_list.index(road_option)]
self._waypoints_queue.append((next_waypoint, road_option))
[docs] def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True):
"""
Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs
If 'clean_queue`, erases the previous plan, and if not, it is added to the old one
The 'stop_waypoint_creation' flag avoids creating more random waypoints
Args:
current_plan: list of (carla.Waypoint, RoadOption)
stop_waypoint_creation: bool
clean_queue: bool
"""
if clean_queue:
self._waypoints_queue.clear()
# Remake the waypoints queue if the new plan has a higher length than the queue
new_plan_length = len(current_plan) + len(self._waypoints_queue)
if new_plan_length > self._waypoints_queue.maxlen:
new_waypoint_queue = deque(maxlen=new_plan_length)
for wp in self._waypoints_queue:
new_waypoint_queue.append(wp)
self._waypoints_queue = new_waypoint_queue
for elem in current_plan:
self._waypoints_queue.append(elem)
self._stop_waypoint_creation = stop_waypoint_creation
[docs] def run_step(self, debug=False):
"""
Execute one step of local planning which involves running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
Args:
debug: boolean flag to activate waypoints debugging
Returns:
The 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) < self._min_waypoint_queue_length:
self._compute_next_waypoints(k=self._min_waypoint_queue_length)
# Purge the queue of obsolete waypoints
veh_location = self._vehicle.get_location()
vehicle_speed = get_speed(self._vehicle) / 3.6
self._min_distance = self._base_min_distance + 0.5 * vehicle_speed
num_waypoint_removed = 0
for waypoint, _ in self._waypoints_queue:
if len(self._waypoints_queue) - num_waypoint_removed == 1:
min_distance = 1 # Don't remove the last waypoint until very close by
else:
min_distance = self._min_distance
if veh_location.distance(waypoint.transform.location) < min_distance:
num_waypoint_removed += 1
else:
break
if num_waypoint_removed > 0:
for _ in range(num_waypoint_removed):
self._waypoints_queue.popleft()
# Get the target waypoint and move using the PID controllers. Stop if no target waypoint
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
else:
self.target_waypoint, self.target_road_option = self._waypoints_queue[0]
control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint)
if debug:
draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0)
return control
[docs] def get_incoming_waypoint_and_direction(self, steps=3):
"""
Returns direction and waypoint at a distance ahead defined by the user.
:param steps: number of steps to get the incoming waypoint.
"""
if len(self._waypoints_queue) > steps:
return self._waypoints_queue[steps]
else:
try:
wpt, direction = self._waypoints_queue[-1]
return wpt, direction
except IndexError as i:
return None, RoadOption.VOID
[docs] def done(self):
"""
Returns whether or not the planner has finished
:return: boolean
"""
return len(self._waypoints_queue) == 0
def _retrieve_options(list_waypoints, current_waypoint):
"""
Compute the type of connection between the current active waypoint and the multiple waypoints present in
list_waypoints. The result is encoded as a list of RoadOption enums.
:param list_waypoints: list with the possible target waypoints in case of multiple options
:param current_waypoint: current active waypoint
:return: list of RoadOption enums representing the type of connection from the active waypoint to each
candidate in list_waypoints
"""
options = []
for next_waypoint in list_waypoints:
# this is needed because something we are linking to
# the beggining of an intersection, therefore the
# variation in angle is small
next_next_waypoint = next_waypoint.next(3.0)[0]
link = _compute_connection(current_waypoint, next_next_waypoint)
options.append(link)
return options
def _compute_connection(current_waypoint, next_waypoint, threshold=35):
"""
Compute the type of topological connection between an active waypoint (current_waypoint) and a target waypoint
(next_waypoint).
:param current_waypoint: active waypoint
:param next_waypoint: target waypoint
:return: the type of topological connection encoded as a RoadOption enum:
RoadOption.STRAIGHT
RoadOption.LEFT
RoadOption.RIGHT
"""
n = next_waypoint.transform.rotation.yaw
n = n % 360.0
c = current_waypoint.transform.rotation.yaw
c = c % 360.0
diff_angle = (n - c) % 180.0
if diff_angle < threshold or diff_angle > (180 - threshold):
return RoadOption.STRAIGHT
elif diff_angle > 90.0:
return RoadOption.LEFT
else:
return RoadOption.RIGHT