def create_agent(self, spawn_point):
try:
blueprints = self.world.get_blueprint_library().filter('vehicle.*')
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
# spawn_points = list(self.world.get_map().get_spawn_points())
# print('found %d spawn points.' % len(spawn_points))
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
blueprint.set_attribute('role_name', 'autopilot')
self.vehicle = self.world.try_spawn_actor(blueprint, spawn_point)
if self.vehicle is not None:
self.vehicle.set_autopilot()
print('*Collision* Spawned %r at %s' %
(self.vehicle.type_id, spawn_point.location))
else:
print("No vehicles to spawn")
return False
self.vehicle_controller = VehiclePIDController(self.vehicle, args_lateral=self.args_lateral_dict, \
args_longitudinal=self.args_longitudinal_dict)
except:
print("Unable to import vehicle")
def _init_controller(self, dest, opt_dict):
"""
Controller initialization.
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
self._dt = 1.0 / 10.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
args_lateral_dict = {
'K_P': 1.0,
'K_D': 0.0,
'K_I': 0.5,
'dt': self._dt
}
args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 1,
'dt': self._dt
}
# parameters overload
if opt_dict:
print("opt_dict is passed into route_planner")
if 'dt' in opt_dict:
self._dt = opt_dict['dt']
if 'target_speed' in opt_dict:
self._target_speed = opt_dict['target_speed']
print("target speed modified to " + str(self._target_speed))
if 'sampling_radius' in opt_dict:
self._sampling_radius = self._target_speed * \
opt_dict['sampling_radius'] / 3.6
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
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)
self._global_plan = False
# compute initial waypoints
self._waypoints_queue.append(
(self._current_waypoint.next(self._sampling_radius)[0],
RoadOption.LANEFOLLOW))
self._target_road_option = RoadOption.LANEFOLLOW
# fill waypoint trajectory queue
self._compute_next_waypoints(dest, k=200)
def init_controller(self, opt_dict):
"""
Controller initialization.
: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 * 0.5 / 3.6 # 0.5 seconds horizon
self.min_distance = self.sampling_radius * self.MIN_DISTANCE_PERCENTAGE
args_lateral_dict = {
'K_P': 1.95,
'K_D': 0.01,
'K_I': 1.4,
'dt': self.dt
}
args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 1,
'dt': self.dt
}
# parameters overload
if opt_dict:
if 'dt' in opt_dict:
self.dt = opt_dict['dt']
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
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
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)
self.global_plan = False
# compute initial waypoints
self.waypoints_queue.append(
(self.current_waypoint.next(self.sampling_radius)[0],
RoadOption.LANEFOLLOW))
self.target_road_option = RoadOption.LANEFOLLOW
# fill waypoint trajectory queue
self._compute_next_waypoints(k=200)
def _init_controller(self, opt_dict):
"""
Controller initialization.
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
self._dt = 1.0 / 10.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
args_lateral_dict = {
"K_P": 1.0,
"K_D": 0.0,
"K_I": 0.5,
"dt": self._dt
}
args_longitudinal_dict = {
"K_P": 1.0,
"K_D": 0,
"K_I": 1,
"dt": self._dt
}
# parameters overload
if opt_dict:
if "dt" in opt_dict:
self._dt = opt_dict["dt"]
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
if "lateral_control_dict" in opt_dict:
args_lateral_dict = opt_dict["lateral_control_dict"]
if "longitudinal_control_dict" in opt_dict:
args_longitudinal_dict = opt_dict["longitudinal_control_dict"]
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)
self._global_plan = False
# compute initial waypoints
self._waypoints_queue.append(
(self._current_waypoint.next(self._sampling_radius)[0],
RoadOption.LANEFOLLOW))
self._target_road_option = RoadOption.LANEFOLLOW
# fill waypoint trajectory queue
self._compute_next_waypoints(k=200)
def _init_controller(self, opt_dict):
"""
Controller initialization.
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
self._dt = 1.0 / 20.0
self.set_speed(20.0) # Km/h
args_lateral_dict = {
'K_P': 1.95,
'K_D': 0.01,
'K_I': 1.4,
'dt': self._dt
}
args_longitudinal_dict = {
'K_P': 0.1,
'K_D': 0.0,
'K_I': 2,
'dt': self._dt
}
# parameters overload
if opt_dict:
if 'dt' in opt_dict:
self._dt = opt_dict['dt']
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
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
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)
self._global_plan = False
# compute initial waypoints
self._waypoints_queue.append(
(self._current_waypoint.next(self._sampling_radius)[0],
RoadOption.LANEFOLLOW))
self._target_road_option = RoadOption.LANEFOLLOW
# fill waypoint trajectory queue
self._compute_next_waypoints(k=200)
def init_controller(self, opt_dict):
"""
Controller initialization.
: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 * 0.5 / 3.6 # 0.5 seconds horizon
self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE
args_lateral_dict = {
'K_P': 1.95,
'K_D': 0.01,
'K_I': 1.4,
'dt': self._dt
}
args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 1,
'dt': self._dt
}
# parameters overload
if 'dt' in opt_dict:
self._dt = opt_dict['dt']
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
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# print("current waypoint: ", self._current_waypoint)
self._vehicle_controller = VehiclePIDController(
self._vehicle,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
self._global_plan = False
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))
def __init__(self, vehicle, opt_dict=None):
super(PIDAgent, self).__init__(vehicle)
# Default agent params:
self.target_speed = 20.0
# Default PID params:
self.lateral_pid_dict = {
'K_P': 1.0,
'K_D': 0.01,
'K_I': 0.6,
'dt': 0.05
}
self.longitudinal_pid_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 1,
'dt': 0.05
}
if opt_dict:
if 'target_speed' in opt_dict:
self.target_speed = opt_dict['target_speed']
self.lateral_pid_dict['dt'] = 1.0 / self.target_speed
self.longitudinal_pid_dict['dt'] = 1.0 / self.target_speed
if 'radius' in opt_dict:
self.radius = opt_dict['radius']
if 'min_dist' in opt_dict:
self.min_dist = opt_dict['min_dist']
if 'lateral_pid_dict' in opt_dict:
self.lateral_pid_dict = opt_dict['lateral_pid_dict']
if 'longitudinal_pid_dict' in opt_dict:
self.longitudinal_pid_dict = opt_dict['longitudinal_pid_dict']
self.controller = VehiclePIDController(
vehicle,
args_lateral=self.lateral_pid_dict,
args_longitudinal=self.longitudinal_pid_dict)
self.waypoints = []
self.max_waypoints = 200
self.radius = self.target_speed / 3.6 # Radius at which next waypoint is sampled
self.min_dist = 0.9 * self.radius # If min_dist away from waypoint[0], move on to next one.
def __init__(self, vehicle, route, target_speed=20):
self._vehicle = vehicle
self._route = route
self._target_speed = target_speed
self._driver = IDM(self._vehicle, self._target_speed, TIME_INTERVAL)
self._world = self._vehicle.get_world()
self._map = self._world.get_map()
self._dt = TIME_INTERVAL
# it works, but still has a little ossilation
args_lateral_dict = {
'K_P': 0.08,
'K_D': 0.008,
'K_I': 0.004,
'dt': self._dt
}
args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 0,
'dt': self._dt
}
self._vehicle_controller = VehiclePIDController(
self._vehicle,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
self._history = pd.DataFrame(columns=[
'timestamp',
'ego_vehicle_x',
'ego_vehicle_y',
'ego_vehicle_z',
'ego_vehicle_v',
'leader_vehicle_x',
'leader_vehicle_y',
'leader_vehicle_z',
'leader_vehicle_v',
])
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, opt_dict={}):
"""
:param vehicle: actor to apply to local planner logic onto
:param opt_dict: dictionary of arguments with different parameters:
dt: time between simulation steps
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 = self._vehicle.get_world()
self._map = self._world.get_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 = 1.0 / 20.0
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,
'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 opt_dict:
if 'dt' in opt_dict:
self._dt = opt_dict['dt']
if 'target_speed' in opt_dict:
self._target_speed = opt_dict['target_speed']
if 'sampling_radius' in opt_dict:
self._sampling_radius = opt_dict['sampling_radius']
if 'lateral_control_dict' in opt_dict:
self._args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_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:
self._max_brake = opt_dict['max_brake']
if 'max_steering' in opt_dict:
self._max_steer = opt_dict['max_steering']
if 'offset' in opt_dict:
self._offset = opt_dict['offset']
if 'base_min_distance' in opt_dict:
self._base_min_distance = opt_dict['base_min_distance']
if 'follow_speed_limits' in opt_dict:
self._follow_speed_limits = opt_dict['follow_speed_limits']
# initializing controller
self._init_controller()
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))
def set_speed(self, 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
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):
"""
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))
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
The 'clean_queue` parameter erases the previous plan if True, otherwise, it adds it to the old one
The 'stop_waypoint_creation' flag stops the automatic creation of random waypoints
:param current_plan: list of (carla.Waypoint, RoadOption)
:param stop_waypoint_creation: bool
:param clean_queue: bool
:return:
"""
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
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.
: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) < 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
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
def get_plan(self):
"""Returns the current plan of the local planner"""
return self._waypoints_queue
def done(self):
"""
Returns whether or not the planner has finished
:return: boolean
"""
return len(self._waypoints_queue) == 0
def run_step(self, target_speed=None, debug=False):
"""
Execute one step of local planning which involves
running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param target_speed: desired speed
:param debug: boolean flag to activate waypoints debugging
:return: control
"""
if target_speed is not None:
self._target_speed = target_speed
else:
self._target_speed = self._vehicle.get_speed_limit()
# Buffer waypoints
self._buffer_waypoints()
if len(self._waypoint_buffer) == 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
# Current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
speed = get_speed(self._vehicle) # kph
look_ahead = max(2, speed / 4.5)
# Target waypoint
self.target_waypoint, self.target_road_option = self._waypoint_buffer[
0]
look_ahead_loc = self._get_look_ahead_location(look_ahead)
if target_speed > 50:
args_lat = self.args_lat_hw_dict
args_long = self.args_long_hw_dict
else:
args_lat = self.args_lat_city_dict
args_long = self.args_long_city_dict
if not self._pid_controller:
self._pid_controller = VehiclePIDController(
self._vehicle,
args_lateral=args_lat,
args_longitudinal=args_long)
else:
self._pid_controller.set_lon_controller_params(**args_long)
self._pid_controller.set_lat_controller_params(**args_lat)
control = self._pid_controller.run_step(self._target_speed,
look_ahead_loc)
# Purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
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):
if i == max_index:
self._prev_waypoint = self._waypoint_buffer.popleft()[0]
else:
self._waypoint_buffer.popleft()
if debug:
carla_world = self._vehicle.get_world()
# Draw current buffered waypoint
buffered_waypts = [elem[0] for elem in self._waypoint_buffer]
draw_waypoints(carla_world, buffered_waypts)
# Draw current look ahead point
look_ahead_loc
carla_world.debug.draw_line(look_ahead_loc,
look_ahead_loc + carla.Location(z=0.2),
color=carla.Color(255, 255, 0),
thickness=0.2,
life_time=1.0)
return control
def _init_controller(self, opt_dict):
"""
Controller initialization.
: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._offset = 0
# parameters overload
if opt_dict:
if 'dt' in opt_dict:
self._dt = opt_dict['dt']
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
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
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:
self._max_brake = opt_dict['max_brake']
if 'max_steering' in opt_dict:
self._max_steer = opt_dict['max_steering']
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,
offset=self._offset,
max_throttle=self._max_throt,
max_brake=self._max_brake,
max_steering=self._max_steer)
self._global_plan = False
# compute initial waypoints
self._waypoints_queue.append(
(self._current_waypoint.next(self._sampling_radius)[0],
RoadOption.LANEFOLLOW))
self._target_road_option = RoadOption.LANEFOLLOW
# fill waypoint trajectory queue
self._compute_next_waypoints(k=200)
class PIDAgent(Agent):
def __init__(self, vehicle, opt_dict=None):
super(PIDAgent, self).__init__(vehicle)
# Default agent params:
self.target_speed = 20.0
# Default PID params:
self.lateral_pid_dict = {
'K_P': 1.0,
'K_D': 0.01,
'K_I': 0.6,
'dt': 0.05
}
self.longitudinal_pid_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 1,
'dt': 0.05
}
if opt_dict:
if 'target_speed' in opt_dict:
self.target_speed = opt_dict['target_speed']
self.lateral_pid_dict['dt'] = 1.0 / self.target_speed
self.longitudinal_pid_dict['dt'] = 1.0 / self.target_speed
if 'radius' in opt_dict:
self.radius = opt_dict['radius']
if 'min_dist' in opt_dict:
self.min_dist = opt_dict['min_dist']
if 'lateral_pid_dict' in opt_dict:
self.lateral_pid_dict = opt_dict['lateral_pid_dict']
if 'longitudinal_pid_dict' in opt_dict:
self.longitudinal_pid_dict = opt_dict['longitudinal_pid_dict']
self.controller = VehiclePIDController(
vehicle,
args_lateral=self.lateral_pid_dict,
args_longitudinal=self.longitudinal_pid_dict)
self.waypoints = []
self.max_waypoints = 200
self.radius = self.target_speed / 3.6 # Radius at which next waypoint is sampled
self.min_dist = 0.9 * self.radius # If min_dist away from waypoint[0], move on to next one.
def add_next_waypoints(self):
def d_angle(a, b):
return abs((a - b + 180) % 360 - 180)
if not self.waypoints:
current_w = self._map.get_waypoint(self._vehicle.get_location())
self.waypoints.append(current_w)
while len(self.waypoints) < self.max_waypoints:
last_w = self.waypoints[-1]
last_heading = last_w.transform.rotation.yaw
next_w_list = list(last_w.next(self.radius))
# Go straight if possible.
if next_w_list:
next_w = min(next_w_list,
key=lambda w: d_angle(w.transform.rotation.yaw,
last_heading))
else:
print('No more waypoints.')
return
self.waypoints.append(next_w)
def run_step(self):
transform = self._vehicle.get_transform()
if self.waypoints:
# If too far off course, reset waypoint queue.
if distance_vehicle(self.waypoints[0],
transform) > 5.0 * self.radius:
self.waypoints = []
# Get more waypoints.
if len(self.waypoints) < self.max_waypoints // 2:
self.add_next_waypoints()
# If no more waypoints, stop.
if not self.waypoints:
print('Ran out of waypoints; stopping.')
control = carla.VehicleControl()
control.throttle = 0.0
return control
# Remove waypoints we've reached.
while distance_vehicle(self.waypoints[0], transform) < self.min_dist:
self.waypoints = self.waypoints[1:]
# Draw next waypoint
draw_waypoints(self._vehicle.get_world(), self.waypoints[:1],
self._vehicle.get_location().z + 1.0)
return self.controller.run_step(self.target_speed, self.waypoints[0])
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.
"""
# minimum distance to target waypoint as a percentage (e.g. within 90% of
# total distance)
MIN_DISTANCE_PERCENTAGE = 0.9
def __init__(self, vehicle, dest, opt_dict=None):
"""
:param vehicle: actor to apply to local planner logic onto
:param dest: destination of the route (carla.Location Object)
:param opt_dict: dictionary of arguments with the following semantics:
dt -- time difference between physics control in seconds. This is typically fixed from server side
using the arguments -benchmark -fps=F . In this case dt = 1/F
target_speed -- desired cruise speed in Km/h
sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead
lateral_control_dict -- dictionary of arguments to setup the lateral PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
longitudinal_control_dict -- dictionary of arguments to setup the longitudinal PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
"""
self._vehicle = vehicle
self._map = self._vehicle.get_world().get_map()
# self._world = world
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._global_plan = 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)
# initializing controller
self._init_controller(dest, opt_dict)
def _init_controller(self, dest, opt_dict):
"""
Controller initialization.
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
self._dt = 1.0 / 10.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
args_lateral_dict = {
'K_P': 1.0,
'K_D': 0.0,
'K_I': 0.5,
'dt': self._dt
}
args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 1,
'dt': self._dt
}
# parameters overload
if opt_dict:
print("opt_dict is passed into route_planner")
if 'dt' in opt_dict:
self._dt = opt_dict['dt']
if 'target_speed' in opt_dict:
self._target_speed = opt_dict['target_speed']
print("target speed modified to " + str(self._target_speed))
if 'sampling_radius' in opt_dict:
self._sampling_radius = self._target_speed * \
opt_dict['sampling_radius'] / 3.6
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
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)
self._global_plan = False
# compute initial waypoints
self._waypoints_queue.append(
(self._current_waypoint.next(self._sampling_radius)[0],
RoadOption.LANEFOLLOW))
self._target_road_option = RoadOption.LANEFOLLOW
# fill waypoint trajectory queue
self._compute_next_waypoints(dest, k=200)
def set_speed(self, speed):
"""
Request new target speed.
:param speed: new target speed in Km/h
:return:
"""
self._target_speed = speed
def _compute_next_waypoints(self, dest, 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]
# print("the last waypoint is: " + str(last_waypoint) + "\n")
next_waypoints = list(last_waypoint.next(self._sampling_radius))
if len(next_waypoints) == 1:
#print("only one waypoint available")
# only one option available ==> lanefollowing
next_waypoint = next_waypoints[0]
road_option = RoadOption.LANEFOLLOW
else:
# choose a waypoint towards the destination
road_options_list = _retrieve_options(next_waypoints,
last_waypoint)
#location = self._current_waypoint.transform.location
#print("choosing waypoints at x={}, y={}".format(location.x, location.y))
min_dist = -1
road_option = RoadOption.LANEFOLLOW
next_waypoint = next_waypoints[0]
if (self._current_waypoint.is_junction):
#print("vehicle is at a junction")
for option in road_options_list:
# if option == RoadOption.LEFT:
# print("left")
# elif option == RoadOption.RIGHT:
# print("right")
# elif option == RoadOption.LANEFOLLOW:
# print("follow")
next_waypoint = next_waypoints[road_options_list.index(
option)]
dist = dest.distance(next_waypoint.transform.location)
if min_dist < 0 or dist < min_dist:
road_option = option
min_dist = dist
#road_option = random.choice(road_options_list)
#else:
#next_waypoint = next_waypoints[0] # default to lanefollow
#print("final road option: " + str(road_option))
next_waypoint = next_waypoints[road_options_list.index(
road_option)]
self._waypoints_queue.append((next_waypoint, road_option))
def set_global_plan(self, current_plan):
self._waypoints_queue.clear()
for elem in current_plan:
self._waypoints_queue.append(elem)
self._target_road_option = RoadOption.LANEFOLLOW
self._global_plan = True
def run_step(self, dest, lane_change, world, debug=False):
"""
Execute one step of local planning which involves running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param debug: boolean flag to activate waypoints debugging
:return:
"""
# if(lane_change == "right"):
# print('switch into the right lane')
# # change target waypoint to a point on the right lane
# right_lane = self._current_waypoint.get_right_lane()
# new_waypoint = right_lane.next(5)[0]
# # self.target_waypoint = new_waypoint
# self._waypoints_queue.clear()
# self._waypoint_buffer.clear()
# self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW))
# elif(lane_change == "left"):
# print('switch into the left lane')
# # change target waypoint to a point on the right lane
# left_lane = self._current_waypoint.get_left_lane()
# new_waypoint = left_lane.next(5)[0]
# # self.target_waypoint = new_waypoint
# self._waypoints_queue.clear()
# self._waypoint_buffer.clear()
# print(len(self._waypoints_queue))
# # print("new waypoint at " + str(new_waypoint))
# self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW))
# not enough waypoints in the horizon? => add more!
if not self._global_plan and len(self._waypoints_queue) < int(
self._waypoints_queue.maxlen * 0.5):
# print("current waypoint: " + str(self._current_waypoint))
# print("add 100 additional waypoints")
self._compute_next_waypoints(dest, k=100)
current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
if len(self._waypoints_queue) == 0:
if not current_waypoint.is_intersection:
self._target_road_option = RoadOption.LANEFOLLOW
print("way points queue is empty")
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 i in range(self._buffer_size):
if self._waypoints_queue:
self._waypoint_buffer.append(
self._waypoints_queue.popleft())
else:
break
# current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# target waypoint
self.target_waypoint, self._target_road_option = self._waypoint_buffer[
0]
# print("target waypoint at " + str(self.target_waypoint))
if (lane_change == "right"):
# print('switch into the right lane')
# change target waypoint to a point on the right lane
right_lane = self._current_waypoint.get_right_lane()
if (not right_lane):
print("cannot switch into the right lane")
else:
self.target_waypoint = right_lane.next(5)[-1]
# self.target_waypoint = new_waypoint
elif (lane_change == "left"):
# print('switch into the left lane')
# change target waypoint to a point on the right lane
left_lane = self._current_waypoint.get_left_lane()
if (not left_lane):
print("cannot switch into the right lane")
else:
self.target_waypoint = left_lane.next(5)[-1]
# # set the target speed
# speed_limit = self._vehicle.get_speed_limit()
# #set highway driving speed to 40 kmh
# if(speed_limit > 30):
# self.set_speed(30)
# # otherwise, set driving speed to 20 kmh
# else:
# self.set_speed(20)
# move using PID controllers
#print("target_speed: " + str(self._target_speed))
control = self._vehicle_controller.run_step(self._target_speed,
self.target_waypoint)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
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)
# if self._target_road_option != RoadOption.LANEFOLLOW not current_waypoint.is_intersection:
# self._target_road_option = RoadOption.LANEFOLLOW
return control
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.
"""
# 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={}):
"""
:param vehicle: actor to apply to local planner logic onto
:param opt_dict: dictionary of arguments with the following semantics:
dt -- time difference between physics control in seconds. This is typically fixed from server side
using the arguments -benchmark -fps=F . In this case dt = 1/F
target_speed -- desired cruise speed in Km/h
sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead
lateral_control_dict -- dictionary of arguments to setup the lateral PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
longitudinal_control_dict -- dictionary of arguments to setup the longitudinal PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
"""
self._vehicle = vehicle
self._map = self._vehicle.get_world().get_map()
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._global_plan = None
# queue with tuples of (waypoint, RoadOption)
self._waypoints_queue = deque(maxlen=600)
self._buffer_size = 10
self._waypoint_buffer = deque(maxlen=self._buffer_size)
# initializing controller
self.init_controller(opt_dict)
def __del__(self):
self._vehicle.destroy()
print("Destroying ego-vehicle!")
def init_controller(self, opt_dict):
"""
Controller initialization.
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
self._dt = 1.0 / 5.0
self._target_speed = 35.0 # Km/h
self._sampling_radius = 30 * 0.5 / 3.6 #self._target_speed * 0.5 / 3.6 # 0.5 seconds horizon
self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE
args_lateral_dict = {
'K_P': .5,
'K_D': 0.01,
'K_I': 0.4,
'dt': self._dt
}
args_longitudinal_dict = {
'K_P': 0.2,
'K_D': 0.014,
'K_I': 0.0258,
'dt': self._dt
}
# parameters overload
if 'dt' in opt_dict:
self._dt = opt_dict['dt']
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
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
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)
self._global_plan = False
# compute initial waypoints
self._waypoints_queue.append(
(self._current_waypoint.next(self._sampling_radius)[0],
RoadOption.LANEFOLLOW))
self._target_road_option = RoadOption.LANEFOLLOW
# fill waypoint trajectory queue
self._compute_next_waypoints(k=200)
def set_speed(self, speed):
"""
Request new target speed.
:param speed: new target speed in Km/h
:return:
"""
self._target_speed = speed
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) == 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))
def set_global_plan(self, current_plan):
self._waypoints_queue.clear()
for elem in current_plan:
self._waypoints_queue.append(elem)
self._target_road_option = RoadOption.LANEFOLLOW
self._global_plan = True
def get_filled_waypoint_buffer(self) -> List[carla.Waypoint]:
while len(self._waypoint_buffer) < self._buffer_size:
if self._waypoints_queue:
# [0] -> get only carla.Waypoint, ignore RoadOption
next_waypoint = self._waypoints_queue.popleft()[0]
self._waypoint_buffer.append(next_waypoint)
else:
break
return self._waypoint_buffer
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
def __init__(
self,
environment: oatomobile.Env,
*,
setpoint_index: int = 5,
replan_every_steps: int = 1,
lateral_control_dict: Mapping[str,
Any] = LATERAL_PID_CONTROLLER_CONFIG,
longitudinal_control_dict: Mapping[
str, Any] = LONGITUDINAL_PID_CONTROLLER_CONFIG,
fixed_delta_seconds_between_setpoints: Optional[int] = None
) -> None:
"""Constructs a setpoint-based agent.
Args:
environment: The navigation environment to spawn the agent.
setpoint_index: The index of the point to cut-off the plan.
replan_every_steps: The number of steps between subsequent call on the model.
lateral_control_dict: The lateral PID controller's config.
longitudinal_control_dict: The longitudinal PID controller's config.
fixed_delta_seconds_between_setpoints: The time difference (in seconds)
between the setpoints. It defaults to the fps of the simulator.
"""
try:
from agents.navigation.controller import VehiclePIDController # pylint: disable=import-error
except ImportError:
raise ImportError(
"Missing CARLA installation, "
"make sure the environment variable CARLA_ROOT is provided "
"and that the PythonAPI is `easy_install`ed")
super(SetPointAgent, self).__init__(environment=environment)
# References to the CARLA objects.
self._vehicle = self._environment.simulator.hero
self._world = self._vehicle.get_world()
self._map = self._world.get_map()
# Sets up PID controllers.
dt = self._vehicle.get_world().get_settings().fixed_delta_seconds
lateral_control_dict = lateral_control_dict.copy()
lateral_control_dict.update({"dt": dt})
logging.debug(
"Lateral PID controller config {}".format(lateral_control_dict))
longitudinal_control_dict = longitudinal_control_dict.copy()
longitudinal_control_dict.update({"dt": dt})
logging.debug("Longitudinal PID controller config {}".format(
longitudinal_control_dict))
self._vehicle_controller = VehiclePIDController(
vehicle=self._vehicle,
args_lateral=lateral_control_dict,
args_longitudinal=longitudinal_control_dict,
)
# Sets agent's hyperparameters.
self._setpoint_index = setpoint_index
self._replan_every_steps = replan_every_steps
self._fixed_delta_seconds_between_setpoints = fixed_delta_seconds_between_setpoints or dt
# Inits agent's buffer of setpoints.
self._setpoints_buffer = None
self._steps_counter = 0
class NaiveAgent:
def __init__(self, vehicle, route, target_speed=20):
self._vehicle = vehicle
self._route = route
self._target_speed = target_speed
self._driver = IDM(self._vehicle, self._target_speed, TIME_INTERVAL)
self._world = self._vehicle.get_world()
self._map = self._world.get_map()
self._dt = TIME_INTERVAL
# it works, but still has a little ossilation
args_lateral_dict = {
'K_P': 0.08,
'K_D': 0.008,
'K_I': 0.004,
'dt': self._dt
}
args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 0,
'dt': self._dt
}
self._vehicle_controller = VehiclePIDController(
self._vehicle,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
self._history = pd.DataFrame(columns=[
'timestamp',
'ego_vehicle_x',
'ego_vehicle_y',
'ego_vehicle_z',
'ego_vehicle_v',
'leader_vehicle_x',
'leader_vehicle_y',
'leader_vehicle_z',
'leader_vehicle_v',
])
def update_history(self, info_dict):
self._history = self._history.append(info_dict, ignore_index=True)
@staticmethod
def get_info_dict(timestamp, ego_vehicle_snapshot,
leader_vehicle_snapshot):
info_dict = {}
info_dict['timestamp'] = timestamp.elapsed_seconds
ego_vehicle_location = ego_vehicle_snapshot.get_transform().location
info_dict['ego_vehicle_x'] = ego_vehicle_location.x
info_dict['ego_vehicle_y'] = ego_vehicle_location.y
info_dict['ego_vehicle_z'] = ego_vehicle_location.z
info_dict['ego_vehicle_v'] = get_speed(ego_vehicle_snapshot)
if leader_vehicle_snapshot:
leader_vehicle_location = leader_vehicle_snapshot.get_transform(
).location
info_dict['leader_vehicle_x'] = leader_vehicle_location.x
info_dict['leader_vehicle_y'] = leader_vehicle_location.y
info_dict['leader_vehicle_z'] = leader_vehicle_location.z
info_dict['leader_vehicle_v'] = get_speed(leader_vehicle_snapshot)
return info_dict
def store_history(self):
self._history.to_csv('./out/acc{}.csv'.format(
time.strftime("%Y%m%d-%H%M%S")))
def set_target_speed(self, target_speed):
self._target_speed = target_speed
def is_close(self, other_transform, epsilon=0.1):
vehicle_transform = self._vehicle.get_transform()
loc = vehicle_transform.location
dx = other_transform.location.x - loc.x
dy = other_transform.location.y - loc.y
return np.linalg.norm(np.array([dx, dy])) < epsilon
def is_ahead(self, other_transform):
"""Check if ego vehicle is ahead of other"""
vehicle_transform = self._vehicle.get_transform()
loc = vehicle_transform.location
orientation = vehicle_transform.rotation.yaw
dx = other_transform.location.x - loc.x
dy = other_transform.location.y - loc.y
target_vector = np.array([dx, dy])
norm_target = np.linalg.norm(target_vector)
forward_vector = np.array([
math.cos(math.radians(orientation)),
math.sin(math.radians(orientation))
])
cos_vector = np.dot(forward_vector, target_vector) / norm_target
if cos_vector > 1:
cos_vector = 1
elif cos_vector < -1:
cos_vector = -1
d_angle = math.degrees(math.acos(cos_vector))
return d_angle > 90
def get_distance(self, other):
ego_vehicle_transform = self._vehicle.get_transform()
other_transform = other.get_transform()
sign = -1 if self.is_ahead(other_transform) else 1
ego_location = ego_vehicle_transform.location
other_location = other_transform.location
dx = ego_location.x - other_location.x
dy = ego_location.y - other_location.y
dz = ego_location.z - other_location.z
target_vector = np.array([dx, dy, dz])
norm_target = np.linalg.norm(target_vector)
return sign * norm_target
def get_front_vehicle_state(self):
actor_list = self._world.get_actors()
vehicle_list = actor_list.filter("*vehicle*")
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
front_vehicles_info = []
for target_vehicle in vehicle_list:
# do not account for the ego vehicle
if target_vehicle.id == self._vehicle.id:
continue
# if the object is not in our lane it's not an obstacle
target_vehicle_waypoint = self._map.get_waypoint(
target_vehicle.get_location())
# consider absolute value of lane_id is enough for the experiment
if abs(target_vehicle_waypoint.lane_id) != abs(
ego_vehicle_waypoint.lane_id):
continue
# get the state of front vehiles
target_distance = self.get_distance(target_vehicle)
target_speed = get_speed(target_vehicle)
target_id = target_vehicle.id
if target_distance > 0:
front_vehicles_info.append(
(target_distance, target_speed, target_id))
if front_vehicles_info:
front_vehicles_info = sorted(front_vehicles_info,
key=lambda x: x[0])
return True, front_vehicles_info[0]
else:
return False, None
def run_step(self, debug=False):
# stop if no route to follow
if not self._route:
return emergency_stop()
# purge obsolete waypoints in the route
index = 0
for i, waypoint in enumerate(self._route):
if self.is_close(waypoint.transform):
if debug:
print("Vehicle: {}".format(self._vehicle.get_transform()))
print("vehicle is close to obsolete waypoint: {}".format(
waypoint))
index += 1
elif self.is_ahead(waypoint.transform):
if debug:
print("Vehicle: {}".format(self._vehicle.get_transform()))
print("vehicle is ahead of obsolete waypoint: {}".format(
waypoint))
index += 1
else:
break
if index != 0:
self._route = self._route[index:]
if debug:
print("waypoint remain: {}".format(len(self._route)))
if not self._route:
return emergency_stop()
# follow next waypoint
target_waypoint = self._route[0]
if debug:
print("target waypoint: {}".format(target_waypoint))
print("waypoint lane id: {}".format(target_waypoint.lane_id))
# find leader vehicle
has_front_vehicle, front_vehicle_state = self.get_front_vehicle_state()
# prepare to add info to history
world_snapshot = self._world.get_snapshot()
timestamp = world_snapshot.timestamp
ego_vehicle_snapshot = world_snapshot.find(self._vehicle.id)
front_vehicle_snapshot = None
# get control based on front vehicle state
if has_front_vehicle:
front_vehicle_distance, front_vehicle_speed, front_vehicle_id = front_vehicle_state
front_vehicle_snapshot = world_snapshot.find(front_vehicle_id)
print("obstacle distance: {:3f}m, speed: {:1f}km/h".format(
front_vehicle_distance, front_vehicle_speed))
# stop directly if too close
if front_vehicle_distance < self._driver.minimum_distance:
control = emergency_stop()
else:
self._driver.set_leader_info(front_vehicle_distance,
front_vehicle_speed)
desired_speed = self._driver.calc_desired_speed()
print("target speed: {:1f}".format(desired_speed))
control = self._vehicle_controller.run_step(
desired_speed, target_waypoint)
else:
print("no obstacle, default speed: {}".format(self._target_speed))
control = self._vehicle_controller.run_step(
self._target_speed, target_waypoint)
self.update_history(
self.get_info_dict(timestamp, ego_vehicle_snapshot,
front_vehicle_snapshot))
return control
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.
"""
# Minimum distance to target waypoint as a percentage
# (e.g. within 80% of total distance)
# TODO: Set FPS in constructor
# FPS used for dt
FPS = 10
def __init__(self, agent):
"""
:param agent: agent that regulates the vehicle
:param vehicle: actor to apply to local planner logic onto
"""
self._vehicle = agent.vehicle
self._map = agent.vehicle.get_world().get_map()
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._global_plan = None
self._pid_controller = None
# queue with tuples of (waypoint, RoadOption)
self.waypoints_queue = deque(maxlen=20000)
self._buffer_size = 10
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self._prev_waypoint = None
self._init_controller() # initializing controller
def reset_vehicle(self):
"""Reset the ego-vehicle"""
self._vehicle = None
print("Resetting ego-vehicle!")
def _init_controller(self):
"""
Controller initialization.
dt -- time difference between physics control in seconds.
This is can be fixed from server side
using the arguments -benchmark -fps=F, since dt = 1/F
target_speed -- desired cruise speed in km/h
min_distance -- minimum distance to remove waypoint from queue
lateral_dict -- dictionary of arguments to setup the lateral PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
"""
# Default parameters
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._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
self._prev_waypoint = self._current_waypoint
self._global_plan = False
self._target_speed = self._vehicle.get_speed_limit()
self._min_distance = 4.5
def set_speed(self, speed):
"""
Request new target speed.
:param speed: new target speed in km/h
"""
self._target_speed = speed
def set_global_plan(self, current_plan, clean=False):
"""
Sets new global plan.
:param current_plan: list of waypoints in the actual plan
"""
for elem in current_plan:
self.waypoints_queue.append(elem)
if clean:
self._waypoint_buffer.clear()
for _ in range(self._buffer_size):
if self.waypoints_queue:
self._waypoint_buffer.append(
self.waypoints_queue.popleft())
else:
break
# Put waypoints from global queue to buffer
self._buffer_waypoints()
self._global_plan = True
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._waypoint_buffer) > steps:
return self._waypoint_buffer[steps]
else:
try:
wpt, direction = self._waypoint_buffer[-1]
return wpt, direction
except IndexError as i:
print(i)
return None, RoadOption.VOID
return None, RoadOption.VOID
def run_step(self, target_speed=None, debug=False):
"""
Execute one step of local planning which involves
running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param target_speed: desired speed
:param debug: boolean flag to activate waypoints debugging
:return: control
"""
if target_speed is not None:
self._target_speed = target_speed
else:
self._target_speed = self._vehicle.get_speed_limit()
# Buffer waypoints
self._buffer_waypoints()
if len(self._waypoint_buffer) == 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
# Current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
speed = get_speed(self._vehicle) # kph
look_ahead = max(2, speed / 4.5)
# Target waypoint
self.target_waypoint, self.target_road_option = self._waypoint_buffer[
0]
look_ahead_loc = self._get_look_ahead_location(look_ahead)
if target_speed > 50:
args_lat = self.args_lat_hw_dict
args_long = self.args_long_hw_dict
else:
args_lat = self.args_lat_city_dict
args_long = self.args_long_city_dict
if not self._pid_controller:
self._pid_controller = VehiclePIDController(
self._vehicle,
args_lateral=args_lat,
args_longitudinal=args_long)
else:
self._pid_controller.set_lon_controller_params(**args_long)
self._pid_controller.set_lat_controller_params(**args_lat)
control = self._pid_controller.run_step(self._target_speed,
look_ahead_loc)
# Purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
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):
if i == max_index:
self._prev_waypoint = self._waypoint_buffer.popleft()[0]
else:
self._waypoint_buffer.popleft()
if debug:
carla_world = self._vehicle.get_world()
# Draw current buffered waypoint
buffered_waypts = [elem[0] for elem in self._waypoint_buffer]
draw_waypoints(carla_world, buffered_waypts)
# Draw current look ahead point
look_ahead_loc
carla_world.debug.draw_line(look_ahead_loc,
look_ahead_loc + carla.Location(z=0.2),
color=carla.Color(255, 255, 0),
thickness=0.2,
life_time=1.0)
return control
def _buffer_waypoints(self):
"""Put waypoints from global queue into the buffer."""
num_waypoints_to_add = self._buffer_size - len(self._waypoint_buffer)
for _ in range(num_waypoints_to_add):
if self.waypoints_queue:
next_waypoint = self.waypoints_queue.popleft()
self._waypoint_buffer.append(next_waypoint)
else:
break
def waypoints_in_buffer(self):
"""True if waypoints exist in buffer."""
return len(self._waypoint_buffer) > 0
def _get_projection(self):
"""Get the projection of current vehicle position between prev and target waypoints.
Returns:
carla.Location: Location of the current projection point.
numpy.ndarray: 3D vector formed by the prev and incoming waypoints.
"""
# Vector between prev and target waypoints
waypt_location_diff = self.target_waypoint.transform.location - \
self._prev_waypoint.transform.location
vec_waypoints = np.array([
waypt_location_diff.x, waypt_location_diff.y, waypt_location_diff.z
])
# Vector between prev waypoint and current vehicle's location
veh_location_diff = self._vehicle.get_location(
) - self._prev_waypoint.transform.location
vec_vehicle = np.array(
[veh_location_diff.x, veh_location_diff.y, veh_location_diff.z])
proj_pt = vec_vehicle @ vec_waypoints * \
vec_waypoints / np.linalg.norm(vec_waypoints)**2
prev_waypt_loc = self._prev_waypoint.transform.location
proj_loc = carla.Location(proj_pt[0] + prev_waypt_loc.x,
proj_pt[1] + prev_waypt_loc.y,
proj_pt[2] + prev_waypt_loc.z)
return proj_loc, vec_waypoints
def _get_look_ahead_location(self, look_ahead):
"""Get location of look ahead point along path formed by waypoints."""
proj_loc, vec_waypoints = self._get_projection()
target_loc = self.target_waypoint.transform.location
# Distance between current projection point and the incoming target waypoint
dist_to_next_waypoint = proj_loc.distance(target_loc)
if look_ahead <= dist_to_next_waypoint:
# Vector scaled by look ahead distance
vec_look_ahead = vec_waypoints / \
np.linalg.norm(vec_waypoints) * look_ahead
look_ahead_location = carla.Location(
proj_loc.x + vec_look_ahead[0], proj_loc.y + vec_look_ahead[1],
proj_loc.z + vec_look_ahead[2])
else:
# Loop over buffered waypoints to find the section where the look ahead point
# lies in, then compute the location of the look ahead point
idx = 0
while True:
# If out of waypoints in buffer, use the last waypoint in buffer as look ahead point
if idx + 2 > len(self._waypoint_buffer):
look_ahead_location = self._waypoint_buffer[-1][
0].transform.location
break
# Comput look ahead distacne in the current section
look_ahead -= dist_to_next_waypoint
# Vector formed by start and end waypoints of the current section
vec = self._waypoint_buffer[idx+1][0].transform.location - \
self._waypoint_buffer[idx][0].transform.location
vec = np.array([vec.x, vec.y, vec.z])
dist_to_next_waypoint = np.linalg.norm(vec)
# If look ahead distance exceeds length of current section, go to next one
if look_ahead > dist_to_next_waypoint:
idx += 1
continue
# Section found, compute look ahead location
else:
# Vector scaled by remaining look ahead distance
vec_look_ahead = vec / dist_to_next_waypoint * look_ahead
base_waypt_loc = self._waypoint_buffer[idx][
0].transform.location
look_ahead_location = carla.Location(
base_waypt_loc.x + vec_look_ahead[0],
base_waypt_loc.y + vec_look_ahead[1],
base_waypt_loc.z + vec_look_ahead[2])
break
return look_ahead_location
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.
"""
# 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={}):
"""
:param vehicle: actor to apply to local planner logic onto
:param opt_dict: dictionary of arguments with the following semantics:
dt -- time difference between physics control in seconds. This is typically fixed from server side
using the arguments -benchmark -fps=F . In this case dt = 1/F
target_speed -- desired cruise speed in Km/h
sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead
lateral_control_dict -- dictionary of arguments to setup the lateral PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
longitudinal_control_dict -- dictionary of arguments to setup the longitudinal PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
"""
self._vehicle = vehicle
self._map = self._vehicle.get_world().get_map()
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._global_plan = None
# queue with tuples of (waypoint, RoadOption)
self._waypoints_queue = deque(maxlen=600)
self._buffer_size = 5
self.message_waypoints = 3
# 当前阶段完成的waypoints个数
self.finished_waypoints = 0
self._waypoint_buffer = deque(maxlen=self._buffer_size)
# initializing controller
self.init_controller(opt_dict)
def __del__(self):
if self._vehicle:
self._vehicle.destroy()
print("Destroying ego-vehicle!")
def init_controller(self, opt_dict):
"""
Controller initialization.
: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 * 0.5 / 3.6 # 0.5 seconds horizon
self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE
args_lateral_dict = {
'K_P': 1.95,
'K_D': 0.01,
'K_I': 1.4,
'dt': self._dt
}
args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 1,
'dt': self._dt
}
# parameters overload
if 'dt' in opt_dict:
self._dt = opt_dict['dt']
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
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# print("current waypoint: ", self._current_waypoint)
self._vehicle_controller = VehiclePIDController(
self._vehicle,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict)
self._global_plan = False
# compute initial waypoints
# print("before append len: ", len(self._waypoints_queue))
# self._waypoints_queue.append()
# self._waypoints_queue.append( (self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW))
# self._target_road_option = RoadOption.LANEFOLLOW
# fill waypoint trajectory queue
# # 计算初始的waypoints
# self._compute_next_waypoints(k=10)
# print("after append len: ", len(self._waypoints_queue))
def set_speed(self, speed):
"""
Request new target speed.
:param speed: new target speed in Km/h
:return:
"""
self._target_speed = speed
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) == 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))
def add_waypoint(self, waypoint):
"""
Add a new specific waypoint to the waypoints buffer
from the SUMO simulation server.
:param waypoint: the specific waypoint passed here
"""
# print("initial waypoint: ", waypoint)
# new_point = self._map.get_waypoint(waypoint)
new_point = self._map.get_waypoint(waypoint.location)
# print("point lane type:", type(new_point.lane_type))
# new_point = new_point.get_left_lane().get_left_lane()
# new_point.
# print("new f*****g waypoint: ", new_point)
# print("type of new point: ", type(new_point))
# new_point = carla.Waypoint()
#print("self.current_waypoint: ", self._current_waypoint)
# print("waypoint in add_waypoint: ", waypoint)
# new_point.transform = waypoint
# new_point.transform.location = waypoint.location
new_point.transform.rotation = waypoint.rotation
# # new_point.transform.location.y = waypoint.location.y
# # new_point.transform.location.z = waypoint.location.z
# new_point.transform.rotation.pitch = waypoint.rotation.pitch
# new_point.transform.rotation.yaw = waypoint.rotation.yaw
# new_point.transform.rotation.roll = waypoint.rotation.roll
road_option = compute_connection(self._current_waypoint, new_point)
# print("f*****g road option: ", road_option)
# print("new point: ", new_point)
self._waypoints_queue.append((new_point, road_option))
# wp, _ = self._waypoints_queue[-1]
# wp2, _ = self._waypoints_queue[0]
# print("element in queue[0]: ", wp2)
# print("element in queue[-1]: ", wp)
# print("queue length: ", len(self._waypoints_queue))
# print("waypoint queue add done in local_planner! current length: ", len(self._waypoints_queue))
def set_global_plan(self, current_plan):
self._waypoints_queue.clear()
for elem in current_plan:
self._waypoints_queue.append(elem)
self._target_road_option = RoadOption.LANEFOLLOW
self._global_plan = True
def get_finished_waypoints(self):
ret = self.finished_waypoints
if self.finished_waypoints >= self.message_waypoints:
self.finished_waypoints = 0
return ret
# return True if vehicle is close to the 10th waypoint
def reached_final_waypoint(self):
pass
'''
收到新的action package后丢弃现有的路点缓冲
'''
def drop_waypoint_buffer(self):
self._waypoints_queue.clear()
self._waypoint_buffer.clear()
self.finished_waypoints = 0
def run_step(self, debug=True):
"""
Execute one step of local planning which involves running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param debug: boolean flag to activate waypoints debugging
:return:
"""
# print("current queue len: ", len(self._waypoints_queue))
# print("maxlen: ", int(self._waypoints_queue.maxlen * 0.5))
# not enough waypoints in the horizon? => add more!
# if len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5):
# print("no points here!")
# 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
# Buffering the waypoints
# print("queue length inside run_step: ", len(self._waypoints_queue))
if not self._waypoint_buffer:
for i in range(self._buffer_size):
if self._waypoints_queue:
# print("queue length: ", len(self._waypoints_queue))
left_point, left_road = self._waypoints_queue.popleft()
self._waypoint_buffer.append((left_point, left_road))
# right_point, _ = self._waypoint_buffer.popleft()
# print("right point is ", right_point)
else:
break
# target_waypoint, _ = self._waypoint_buffer[0]
# print("target_waypoint:", target_waypoint)
# for i, (waypoint, _) in enumerate(self._waypoint_buffer):
# print("waypoint is ", waypoint)
# time.sleep(2)
# current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
if not self._waypoint_buffer:
# control = self._vehicle_controller.run_step(0, self._current_waypoint)
# return control
return None
self._target_waypoint, self._target_road_option = self._waypoint_buffer[
0]
# for i in range(len(self._waypoint_buffer)):
# wp, _ = self._waypoint_buffer[i]
# print("waypoint in buffer is ", wp)
# move using PID controllers
# print("target_waypoint: ", self._target_waypoint)
control = self._vehicle_controller.run_step(self._target_speed,
self._target_waypoint)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
# print("len of buffer: ", len(self._waypoint_buffer))
# for i in range(len(self._waypoint_buffer)):
# waypoint = self
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
distance = distance_vehicle(waypoint, vehicle_transform)
# print("min distance: ", self._min_distance, "distance: ", distance)
# time.sleep(2)
# 当前车辆和路点的距离小于最小距离，认为已经行驶完成
if distance < self._min_distance:
# print("waypoint in enumerate is ", waypoint)
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
self.finished_waypoints += 1
print("current finished waypoints is ",
self.finished_waypoints)
if debug:
draw_waypoints(self._vehicle.get_world(),
[self._target_waypoint],
self._vehicle.get_location().z + 1.0)
# if len(self._waypoint_buffer) == 0:
# self.finished_waypoints = self.message_waypoints
return control
def run_step(self, target_speed=None, debug=False):
"""
Execute one step of local planning which involves
running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param target_speed: desired speed
:param debug: boolean flag to activate waypoints debugging
:return: control
"""
if target_speed is not None:
self._target_speed = target_speed
else:
self._target_speed = self._vehicle.get_speed_limit()
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 i in range(self._buffer_size):
if self.waypoints_queue:
self._waypoint_buffer.append(
self.waypoints_queue.popleft())
else:
break
# Current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# Target waypoint
self.target_waypoint, self.target_road_option = self._waypoint_buffer[
0]
if target_speed > 50:
args_lat = self.args_lat_hw_dict
args_long = self.args_long_hw_dict
else:
args_lat = self.args_lat_city_dict
args_long = self.args_long_city_dict
self._pid_controller = VehiclePIDController(
self._vehicle, args_lateral=args_lat, args_longitudinal=args_long)
control = self._pid_controller.run_step(self._target_speed,
self.target_waypoint)
# Purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
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],
1.0)
return control
class SetPointAgent(oatomobile.Agent):
"""An agent that predicts setpoints and consumes them with a PID
controller."""
def __init__(
self,
environment: oatomobile.Env,
*,
setpoint_index: int = 5,
replan_every_steps: int = 1,
lateral_control_dict: Mapping[str,
Any] = LATERAL_PID_CONTROLLER_CONFIG,
longitudinal_control_dict: Mapping[
str, Any] = LONGITUDINAL_PID_CONTROLLER_CONFIG,
fixed_delta_seconds_between_setpoints: Optional[int] = None
) -> None:
"""Constructs a setpoint-based agent.
Args:
environment: The navigation environment to spawn the agent.
setpoint_index: The index of the point to cut-off the plan.
replan_every_steps: The number of steps between subsequent call on the model.
lateral_control_dict: The lateral PID controller's config.
longitudinal_control_dict: The longitudinal PID controller's config.
fixed_delta_seconds_between_setpoints: The time difference (in seconds)
between the setpoints. It defaults to the fps of the simulator.
"""
try:
from agents.navigation.controller import VehiclePIDController # pylint: disable=import-error
except ImportError:
raise ImportError(
"Missing CARLA installation, "
"make sure the environment variable CARLA_ROOT is provided "
"and that the PythonAPI is `easy_install`ed")
super(SetPointAgent, self).__init__(environment=environment)
# References to the CARLA objects.
self._vehicle = self._environment.simulator.hero
self._world = self._vehicle.get_world()
self._map = self._world.get_map()
# Sets up PID controllers.
dt = self._vehicle.get_world().get_settings().fixed_delta_seconds
lateral_control_dict = lateral_control_dict.copy()
lateral_control_dict.update({"dt": dt})
logging.debug(
"Lateral PID controller config {}".format(lateral_control_dict))
longitudinal_control_dict = longitudinal_control_dict.copy()
longitudinal_control_dict.update({"dt": dt})
logging.debug("Longitudinal PID controller config {}".format(
longitudinal_control_dict))
self._vehicle_controller = VehiclePIDController(
vehicle=self._vehicle,
args_lateral=lateral_control_dict,
args_longitudinal=longitudinal_control_dict,
)
# Sets agent's hyperparameters.
self._setpoint_index = setpoint_index
self._replan_every_steps = replan_every_steps
self._fixed_delta_seconds_between_setpoints = fixed_delta_seconds_between_setpoints or dt
# Inits agent's buffer of setpoints.
self._setpoints_buffer = None
self._steps_counter = 0
@abc.abstractmethod
def __call__(self, observation: oatomobile.Observations, *args,
**kwargs) -> np.ndarray:
"""Returns the predicted plan in ego-coordinates, based on a model."""
def act(self, observation: oatomobile.Observations, *args,
**kwargs) -> oatomobile.Action:
"""Takes in an observation, samples from agent's policy, returns an
action."""
from oatomobile.utils import carla as cutil
# Current measurements used for local2world2local transformations.
current_location = observation["location"]
current_rotation = observation["rotation"]
if self._setpoints_buffer is None or self._steps_counter % self._replan_every_steps == 0:
# Get agent predictions.
predicted_plan_ego = self(copy.deepcopy(observation), *args,
**kwargs) # [T, 3]
# Transform plan to world coordinates
predicted_plan_world = cutil.local2world(
current_location=current_location,
current_rotation=current_rotation,
local_locations=predicted_plan_ego,
)
# Refreshes buffer.
self._setpoints_buffer = predicted_plan_world
else:
# Pops first setpoint from the buffer.
self._setpoints_buffer = self._setpoints_buffer[1:]
# Registers setpoints for rendering.
self._environment.unwrapped.simulator.sensor_suite.get(
"predictions").predictions = cutil.world2local(
current_location=current_location,
current_rotation=current_rotation,
world_locations=self._setpoints_buffer,
)
# Increments counter.
self._steps_counter += 1
# Calculates target speed by averaging speed in the `setpoint_index` window.
target_speed = np.linalg.norm(
np.diff(self._setpoints_buffer[:self._setpoint_index], axis=0),
axis=1,
).mean() / (self._fixed_delta_seconds_between_setpoints)
# Converts plan to PID controller setpoint.
setpoint = self._map.get_waypoint(
cutil.ndarray_to_location(
self._setpoints_buffer[self._setpoint_index]))
# Avoids getting stuck when spawned.
if self._steps_counter <= 100:
target_speed = 20.0 / 3.6
# Run PID step.
control = self._vehicle_controller.run_step(
target_speed=target_speed *
3.6, # PID controller expects speed in km/h!
waypoint=setpoint,
)
return control
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.
"""
# Minimum distance to target waypoint as a percentage
# (e.g. within 80% of total distance)
# FPS used for dt
FPS = 20
def __init__(self, agent):
"""
:param agent: agent that regulates the vehicle
:param vehicle: actor to apply to local planner logic onto
"""
self._vehicle = agent.vehicle
self._map = agent.vehicle.get_world().get_map()
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._global_plan = None
self._pid_controller = None
self.waypoints_queue = deque(
maxlen=20000) # queue with tuples of (waypoint, RoadOption)
self._buffer_size = 5
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self._init_controller() # initializing controller
def reset_vehicle(self):
"""Reset the ego-vehicle"""
self._vehicle = None
print("Resetting ego-vehicle!")
def _init_controller(self):
"""
Controller initialization.
dt -- time difference between physics control in seconds.
This is can be fixed from server side
using the arguments -benchmark -fps=F, since dt = 1/F
target_speed -- desired cruise speed in km/h
min_distance -- minimum distance to remove waypoint from queue
lateral_dict -- dictionary of arguments to setup the lateral PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
"""
# Default parameters
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._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
self._global_plan = False
self._target_speed = self._vehicle.get_speed_limit()
self._min_distance = 3
def set_speed(self, speed):
"""
Request new target speed.
:param speed: new target speed in km/h
"""
self._target_speed = speed
def set_global_plan(self, current_plan):
"""
Sets new global plan.
:param current_plan: list of waypoints in the actual plan
"""
for elem in current_plan:
self.waypoints_queue.append(elem)
self._global_plan = True
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:
print(i)
return None, RoadOption.VOID
return None, RoadOption.VOID
def run_step(self, target_speed=None, debug=False):
"""
Execute one step of local planning which involves
running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param target_speed: desired speed
:param debug: boolean flag to activate waypoints debugging
:return: control
"""
if target_speed is not None:
self._target_speed = target_speed
else:
self._target_speed = self._vehicle.get_speed_limit()
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 i in range(self._buffer_size):
if self.waypoints_queue:
self._waypoint_buffer.append(
self.waypoints_queue.popleft())
else:
break
# Current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# Target waypoint
self.target_waypoint, self.target_road_option = self._waypoint_buffer[
0]
if target_speed > 50:
args_lat = self.args_lat_hw_dict
args_long = self.args_long_hw_dict
else:
args_lat = self.args_lat_city_dict
args_long = self.args_long_city_dict
self._pid_controller = VehiclePIDController(
self._vehicle, args_lateral=args_lat, args_longitudinal=args_long)
control = self._pid_controller.run_step(self._target_speed,
self.target_waypoint)
# Purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
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],
1.0)
return control
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.
"""
# 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={}):
"""
:param vehicle: actor to apply to local planner logic onto
:param opt_dict: dictionary of arguments with the following semantics:
dt -- time difference between physics control in seconds. This is typically fixed from server side
using the arguments -benchmark -fps=F . In this case dt = 1/F
target_speed -- desired cruise speed in Km/h
sampling_radius -- search radius for next waypoints in seconds: e.g. 0.5 seconds ahead
lateral_control_dict -- dictionary of arguments to setup the lateral PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
longitudinal_control_dict -- dictionary of arguments to setup the longitudinal PID controller
{'K_P':, 'K_D':, 'K_I':, 'dt'}
"""
self._vehicle = vehicle
self._map = self._vehicle.get_world().get_map()
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._global_plan = None
# queue with tuples of (waypoint, RoadOption)
self._waypoints_queue = deque(maxlen=200)
#
self.init_controller(opt_dict)
def __del__(self):
self._vehicle.destroy()
print("Destroying ego-vehicle!")
def init_controller(self, opt_dict):
"""
Controller initialization.
: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 * 0.5 / 3.6 # 0.5 seconds horizon
self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE
args_lateral_dict = {
'K_P': 1.95,
'K_D': 0.01,
'K_I': 1.4,
'dt': self._dt
}
args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 1,
'dt': self._dt
}
# parameters overload
if 'dt' in opt_dict:
self._dt = opt_dict['dt']
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
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
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)
self._global_plan = False
# compute initial waypoints
self._waypoints_queue.append(
(self._current_waypoint.next(self._sampling_radius)[0],
RoadOption.LANEFOLLOW))
self._target_road_option = RoadOption.LANEFOLLOW
# fill waypoint trajectory queue
self._compute_next_waypoints(k=200)
def set_speed(self, speed):
"""
Request new target speed.
:param speed: new target speed in Km/h
:return:
"""
self._target_speed = speed
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) == 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))
def set_global_plan(self, current_plan):
self._waypoints_queue.clear()
for elem in current_plan:
self._waypoints_queue.append(elem)
self._target_road_option = RoadOption.LANEFOLLOW
self._global_plan = True
def run_step(self, debug=True):
"""
Execute one step of local planning which involves running the longitudinal and lateral PID controllers to
follow the waypoints trajectory.
:param debug: boolean flag to activate waypoints debugging
:return:
"""
# 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
# current vehicle waypoint
self._current_waypoint = self._map.get_waypoint(
self._vehicle.get_location())
# target waypoint
self._target_waypoint, self._target_road_option = self._waypoints_queue[
0]
# move using PID controllers
control = self._vehicle_controller.run_step(self._target_speed,
self._target_waypoint)
# purge the queue of obsolete waypoints
vehicle_transform = self._vehicle.get_transform()
max_index = -1
for i, (waypoint, _) in enumerate(self._waypoints_queue):
if distance_vehicle(waypoint,
vehicle_transform) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoints_queue.popleft()
if debug:
draw_waypoints(self._vehicle.get_world(), [self._target_waypoint],
self._vehicle.get_location().z + 1.0)
return control
class Colliding_Agent:
def __init__(self, world, main_agent):
self.world = world
self.map = self.world.get_map()
self.vehicle = None
self.main_agent = main_agent
self.target_speed = 20 #Km/Hr # add gaussian
dt = 0.05
self.args_lateral_dict = {
'K_P': 1.95,
'K_D': 0.01,
'K_I': 1.4,
'dt': dt
}
self.args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 1,
'dt': dt
}
self.vehicle_controller = None
self.waypoint = None
def create_agent(self, spawn_point):
try:
blueprints = self.world.get_blueprint_library().filter('vehicle.*')
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
# spawn_points = list(self.world.get_map().get_spawn_points())
# print('found %d spawn points.' % len(spawn_points))
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
blueprint.set_attribute('role_name', 'autopilot')
self.vehicle = self.world.try_spawn_actor(blueprint, spawn_point)
if self.vehicle is not None:
self.vehicle.set_autopilot()
print('*Collision* Spawned %r at %s' %
(self.vehicle.type_id, spawn_point.location))
else:
print("No vehicles to spawn")
return False
self.vehicle_controller = VehiclePIDController(self.vehicle, args_lateral=self.args_lateral_dict, \
args_longitudinal=self.args_longitudinal_dict)
except:
print("Unable to import vehicle")
def destroy(self):
if self.vehicle is not None:
self.vehicle.destroy()
def control_agent(self):
# Control the oncoming vehicle to hit into ego vehicle using Carla's PID
if self.find_distance(self.vehicle.get_transform(),
self.main_agent.get_transform()) < 35:
if self.waypoint is None:
# Store the waypoint at the first timestep of lane change
self.waypoint = self.map.get_waypoint(
self.vehicle.get_transform().location, True)
self.vehicle.set_autopilot(False)
# print(self.main_agent.get_transform())
# print(waypoint.next(50)[0].get_left_lane())
# Use the waypoint 20 m ahead to come and hit the vehicle ()
target_waypoint = self.waypoint.next(20)[0].get_left_lane()
# print(target_waypoint)
control = self.vehicle_controller.run_step(self.target_speed,
target_waypoint)
self.vehicle.apply_control(control)
def find_distance(self, main_agent_T, vehicle_T):
distance = math.sqrt((main_agent_T.location.x - vehicle_T.location.x)**2 + (main_agent_T.location.y - vehicle_T.location.y)**2 \
+ (main_agent_T.location.z - vehicle_T.location.z)**2)
return distance
