p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Improved local planner

This commit is contained in:
Guillermo 2021-06-21 11:07:05 +02:00 committed by bernat
parent f94afe9630
commit 0207ec6f72
4 changed files with 133 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
PythonAPI/carla/agents/navigation/basic_agent.py
View File

@ -54,7 +54,6 @@ class BasicAgent(Agent):
end_waypoint = self._map.get_waypoint(location)
route_trace = self.trace_route(start_waypoint, end_waypoint)
self._local_planner.set_global_plan(route_trace)
def trace_route(self, start_waypoint, end_waypoint):

56
PythonAPI/carla/agents/navigation/behavior_agent.py
View File

@ -112,8 +112,7 @@ class BehaviorAgent(Agent):
self.end_waypoint = self._map.get_waypoint(end_location)
route_trace = self.trace_route(self.start_waypoint, self.end_waypoint)
self._local_planner.set_global_plan(route_trace)
self._local_planner.set_global_plan(route_trace, stop_waypoint_creation=False)
def reroute(self, spawn_points):
"""
@ -337,21 +336,29 @@ class BehaviorAgent(Agent):
# Under safety time distance, slow down.
if self.behavior.safety_time > ttc > 0.0:
target_speed = min([
positive(vehicle_speed - self.behavior.speed_decrease),
self.behavior.max_speed,
self.speed_limit - self.behavior.speed_lim_dist])
self._local_planner.set_speed(target_speed)
control = self._local_planner.run_step(debug=debug)
# control = self._local_planner.run_step(
# target_speed=min(positive(vehicle_speed - self.behavior.speed_decrease),
# min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist)), debug=debug)
# Actual safety distance area, try to follow the speed of the vehicle in front.
elif 2 * self.behavior.safety_time > ttc >= self.behavior.safety_time:
target_speed = min([
max(self.min_speed, vehicle_speed),
self.behavior.max_speed,
self.speed_limit - self.behavior.speed_lim_dist])
self._local_planner.set_speed(target_speed)
control = self._local_planner.run_step(debug=debug)
# control = self._local_planner.run_step(
# target_speed=min(max(self.min_speed, vehicle_speed),
# min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist)), debug=debug)
# Normal behavior.
else:
target_speed = min([
self.behavior.max_speed,
self.speed_limit - self.behavior.speed_lim_dist])
self._local_planner.set_speed(target_speed)
control = self._local_planner.run_step(debug=debug)
# control = self._local_planner.run_step(
# target_speed= min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug)
return control
@ -362,7 +369,6 @@ class BehaviorAgent(Agent):
:param debug: boolean for debugging
:return control: carla.VehicleControl
"""
# TODO: Everytime the local_planner run step is called, reset the speed
control = None
if self.behavior.tailgate_counter > 0:
self.behavior.tailgate_counter -= 1
@ -373,12 +379,10 @@ class BehaviorAgent(Agent):
ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc)
# 1: Red lights and stops behavior
if self.traffic_light_manager(ego_vehicle_wp) != 0:
return self.emergency_stop()
# 2.1: Pedestrian avoidancd behaviors
# 2.1: Pedestrian avoidance behaviors
walker_state, walker, w_distance = self.pedestrian_avoid_manager(
ego_vehicle_loc, ego_vehicle_wp)
@ -410,20 +414,20 @@ class BehaviorAgent(Agent):
else:
control = self.car_following_manager(vehicle, distance)
# 4: Intersection behavior
# Checking if there's a junction nearby to slow down
elif self.incoming_waypoint.is_junction and (self.incoming_direction == RoadOption.LEFT or self.incoming_direction == RoadOption.RIGHT):
# 3: Intersection behavior
elif self.incoming_waypoint.is_junction and (self.incoming_direction in [RoadOption.LEFT, RoadOption.RIGHT]):
target_speed = min([
self.behavior.max_speed,
self.speed_limit - 5])
self._local_planner.set_speed(target_speed)
control = self._local_planner.run_step(debug=debug)
# control = self._local_planner.run_step(
# target_speed=min(self.behavior.max_speed, self.speed_limit - 5), debug=debug)
# 5: Normal behavior
# Calculate controller based on no turn, traffic light or vehicle in front
# 4: Normal behavior
else:
control = self._local_planner.run_step(debug=debug)
# control = self._local_planner.run_step(
# target_speed= min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug)
target_speed = min([
self.behavior.max_speed,
self.speed_limit - self.behavior.speed_lim_dist])
self._local_planner.set_speed(target_speed)
control = self._local_planner.run_step(debug=debug)
return control

206
PythonAPI/carla/agents/navigation/local_planner.py
View File

@ -29,17 +29,16 @@ class RoadOption(Enum):
class LocalPlanner(object):
"""
LocalPlanner implements the basic behavior of following a trajectory of waypoints that is generated on-the-fly.
The low-level motion of the vehicle is computed by using two PID controllers, one is used for the lateral control
and the other for the longitudinal control (cruise speed).
LocalPlanner implements the basic behavior of following a
trajectory of waypoints that is generated on-the-fly.
When multiple paths are available (intersections) this local planner makes a random choice.
The low-level motion of the vehicle is computed by using two PID controllers,
one is used for the lateral control and the other for the longitudinal control (cruise speed).
When multiple paths are available (intersections) this local planner makes a random choice,
unless a given global plan has already been specified.
"""
# minimum distance to target waypoint as a percentage (e.g. within 90% of
# total distance)
MIN_DISTANCE_PERCENTAGE = 0.9
def __init__(self, vehicle, opt_dict=None):
"""
:param vehicle: actor to apply to local planner logic onto
@ -58,22 +57,34 @@ class LocalPlanner(object):
{'K_P':, 'K_D':, 'K_I':, 'dt'}
"""
self._vehicle = vehicle
self._map = self._vehicle.get_world().get_map()
self._world = self._vehicle.get_world()
self._map = self._world.get_map()
self._target_speed = 20.0 # Km/h
self._sampling_radius = 1.0
self._min_distance = 5.0
self._dt = 1.0 / 20.0
self._max_brake = 0.3
self._max_throt = 0.75
self._max_steer = 0.8
self._offset = 0
self._args_lateral_dict = {'K_P': 1.95, 'K_D': 0.2, 'K_I': 0.07, 'dt': self._dt}
self._args_longitudinal_dict = {'K_P': 1.0, 'K_D': 0, 'K_I': 0.05, 'dt': self._dt}
# self.args_lat_hw_dict = {'K_P': 0.75, 'K_D': 0.02, 'K_I': 0.4, 'dt': 1.0 / self.FPS}
# self.args_lat_city_dict = {'K_P': 0.58, 'K_D': 0.02, 'K_I': 0.5, 'dt': 1.0 / self.FPS}
# self.args_long_hw_dict = {'K_P': 0.37, 'K_D': 0.024, 'K_I': 0.032, 'dt': 1.0 / self.FPS}
# self.args_long_city_dict = {'K_P': 0.15, 'K_D': 0.05, 'K_I': 0.07, 'dt': 1.0 / self.FPS}
self._dt = None
self._target_speed = None
self._sampling_radius = None
self._min_distance = None
self._current_waypoint = None
self.target_road_option = None
self._next_waypoints = None
self.target_waypoint = None
self._vehicle_controller = None
self._stop_waypoint_creation = None
# queue with tuples of (waypoint, RoadOption)
self._waypoints_queue = deque(maxlen=20000)
self._buffer_size = 5
self._waypoint_buffer = deque(maxlen=self._buffer_size)
self.target_waypoint = None
self.target_road_option = None
self._waypoints_queue = deque(maxlen=10000)
self._min_waypoint_queue_length = 100
self._stop_waypoint_creation = False
self._follow_speed_limits = False
# initializing controller
self._init_controller(opt_dict)
@ -81,7 +92,6 @@ class LocalPlanner(object):
def __del__(self):
if self._vehicle:
self._vehicle.destroy()
print("Destroying ego-vehicle!")
def reset_vehicle(self):
"""Reset the ego-vehicle"""
@ -94,46 +104,6 @@ class LocalPlanner(object):
:param opt_dict: dictionary of arguments.
:return:
"""
# default params
self._dt = 1.0 / 20.0
self._target_speed = 20.0 # Km/h
self._sampling_radius = self._target_speed * 1 / 3.6 # 1 seconds horizon
self._min_distance = self._sampling_radius * self.MIN_DISTANCE_PERCENTAGE
self._max_brake = 0.3
self._max_throt = 0.75
self._max_steer = 0.8
args_lateral_dict = {
'K_P': 1.95,
'K_D': 0.2,
'K_I': 0.07,
'dt': self._dt}
args_longitudinal_dict = {
'K_P': 1.0,
'K_D': 0,
'K_I': 0.05,
'dt': self._dt}
# self.args_lat_hw_dict = {
# 'K_P': 0.75,
# 'K_D': 0.02,
# 'K_I': 0.4,
# 'dt': 1.0 / self.FPS}
# self.args_lat_city_dict = {
# 'K_P': 0.58,
# 'K_D': 0.02,
# 'K_I': 0.5,
# 'dt': 1.0 / self.FPS}
# self.args_long_hw_dict = {
# 'K_P': 0.37,
# 'K_D': 0.024,
# 'K_I': 0.032,
# 'dt': 1.0 / self.FPS}
# self.args_long_city_dict = {
# 'K_P': 0.15,
# 'K_D': 0.05,
# 'K_I': 0.07,
# 'dt': 1.0 / self.FPS}
self._offset = 0
# parameters overload
if opt_dict:
if 'dt' in opt_dict:
@ -141,12 +111,11 @@ class LocalPlanner(object):
if 'target_speed' in opt_dict:
self._target_speed = opt_dict['target_speed']
if 'sampling_radius' in opt_dict:
self._sampling_radius = self._target_speed * \
opt_dict['sampling_radius'] / 3.6
self._sampling_radius = opt_dict['sampling_radius']
if 'lateral_control_dict' in opt_dict:
args_lateral_dict = opt_dict['lateral_control_dict']
self._args_lateral_dict = opt_dict['lateral_control_dict']
if 'longitudinal_control_dict' in opt_dict:
args_longitudinal_dict = opt_dict['longitudinal_control_dict']
self._args_longitudinal_dict = opt_dict['longitudinal_control_dict']
if 'max_throttle' in opt_dict:
self._max_throt = opt_dict['max_throttle']
if 'max_brake' in opt_dict:
@ -156,31 +125,46 @@ class LocalPlanner(object):
if 'offset' in opt_dict:
self._offset = opt_dict['offset']
self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
self._vehicle_controller = VehiclePIDController(self._vehicle,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict,
args_lateral=self._args_lateral_dict,
args_longitudinal=self._args_longitudinal_dict,
offset=self._offset,
max_throttle=self._max_throt,
max_brake=self._max_brake,
max_steering=self._max_steer)
self._stop_waypoint_creation = False
# Compute the current vehicle waypoint
current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
self.target_waypoint, self.target_road_option = (current_waypoint, RoadOption.LANEFOLLOW)
print("Starting position: {}".format(self.target_waypoint))
# compute initial waypoints
self._waypoints_queue.append((self._current_waypoint.next(self._sampling_radius)[0], RoadOption.LANEFOLLOW))
# Fill the waypoint queue
self._waypoints_queue.append((self.target_waypoint, self.target_road_option))
self._compute_next_waypoints(k=self._min_waypoint_queue_length)
# fill waypoint trajectory queue
self._compute_next_waypoints(k=200)
self._world.debug.draw_point(self.target_waypoint.transform.location + carla.Location(z=1), life_time=20, size=0.5)
def set_speed(self, speed):
"""
Request new target speed.
Changes the target speed
:param speed: new target speed in Km/h
:return:
"""
if self._follow_speed_limits:
print("WARNING: The max speed is currently set to follow the speed limits. "
"Use 'follow_speed_limits' to deactivate this")
self._target_speed = speed
# TODO: Change the sampling distance too?
def follow_speed_limits(self, value=True):
"""
Activates a flag that makes the max speed dynamically vary according to the spped limits
:param value: bool
:return:
"""
self._follow_speed_limits = value
def _compute_next_waypoints(self, k=1):
"""
@ -213,30 +197,23 @@ class LocalPlanner(object):
self._waypoints_queue.append((next_waypoint, road_option))
def set_global_plan(self, current_plan, stop_waypoint_creation=True):
def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True):
"""
Resets the waypoint queue and buffer to match the new plan. Also
handles the stop_waypoint_creation flag to avoid creating more waypoints
Adds a new plan to the local planner.
If 'clean_queue`, erases the previous plan, and if note, it is added to the old one
The 'stop_waypoint_creation' flag avoids creating more random waypoints
:param current_plan: list of (carla.Waypoint, RoadOption)
:param stop_waypoint_creation: bool
:param ceal_queue: bool
:return:
"""
if clean_queue:
self._waypoints_queue.clear()
# Reset the queue
self._waypoints_queue.clear()
for elem in current_plan:
self._waypoints_queue.append(elem)
# and the buffer
self._waypoint_buffer.clear()
for _ in range(self._buffer_size):
if self._waypoints_queue:
self._waypoint_buffer.append(
self._waypoints_queue.popleft())
else:
break
self._stop_waypoint_creation = stop_waypoint_creation
def run_step(self, debug=False):
@ -247,48 +224,38 @@ class LocalPlanner(object):
:param debug: boolean flag to activate waypoints debugging
:return: control to be applied
"""
if self._follow_speed_limits:
self._target_speed = self._vehicle.get_speed_limit()
# Add more waypoints too few in the horizon
if not self._stop_waypoint_creation and len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5):
self._compute_next_waypoints(k=100)
if not self._stop_waypoint_creation and len(self._waypoints_queue) < self._min_waypoint_queue_length:
self._compute_next_waypoints(k=self._min_waypoint_queue_length)
if len(self._waypoints_queue) == 0 and len(self._waypoint_buffer) == 0:
if len(self._waypoints_queue) == 0:
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
return control
# Buffering the waypoints
if not self._waypoint_buffer:
for _ in range(self._buffer_size):
if self._waypoints_queue:
self._waypoint_buffer.append(
self._waypoints_queue.popleft())
else:
break
# Current vehicle waypoint
vehicle_transform = self._vehicle.get_transform()
self._current_waypoint = self._map.get_waypoint(vehicle_transform.location)
# Target waypoint
self.target_waypoint, self.target_road_option = self._waypoint_buffer[0]
# Move using PID controllers
# Get the target waypoint and move using the PID controllers
self.target_waypoint, self.target_road_option = self._waypoints_queue[0]
control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint)
# Purge the queue of obsolete waypoints
max_index = -1
# # Purge the queue of obsolete waypoints
veh_location = self._vehicle.get_location()
num_waypoint_removed = 0
for waypoint, _ in self._waypoints_queue:
if veh_location.distance(waypoint.transform.location) < self._min_distance:
num_waypoint_removed += 1
else:
break
for i, (waypoint, _) in enumerate(self._waypoint_buffer):
if waypoint.transform.location.distance(vehicle_transform.location) < self._min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
if num_waypoint_removed > 0:
for _ in range(num_waypoint_removed):
self._waypoints_queue.popleft()
if debug:
draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0)
@ -311,14 +278,13 @@ class LocalPlanner(object):
except IndexError as i:
return None, RoadOption.VOID
def done(self):
"""
Returns whether or not the planner has finished
:return: boolean
"""
return len(self._waypoints_queue) == 0 and len(self._waypoint_buffer) == 0
return len(self._waypoints_queue) == 0
def _retrieve_options(list_waypoints, current_waypoint):
"""

50
PythonAPI/examples/automatic_control.py
View File

@ -173,6 +173,7 @@ class World(object):
actor.apply_physics_control(physics_control)
except Exception:
pass
def tick(self, clock):
"""Method for every tick"""
self.hud.tick(self, clock)
@ -695,22 +696,18 @@ def game_loop(args):
world = World(client.get_world(), hud, args)
controller = KeyboardControl(world)
spawn_points = world.map.get_spawn_points()
random.shuffle(spawn_points)
if spawn_points[0].location != world.player.get_location():
destination = spawn_points[0].location
else:
destination = spawn_points[1].location
if args.agent == "Basic":
agent = BasicAgent(world.player)
spawn_point = world.map.get_spawn_points()[0]
agent.set_destination(spawn_point.location)
agent.set_destination(destination)
else:
# TODO: Change PIDS and target speed to make it on spped limti
agent = BehaviorAgent(world.player, behavior=args.behavior)
spawn_points = world.map.get_spawn_points()
random.shuffle(spawn_points)
if spawn_points[0].location != agent.vehicle.get_location():
destination = spawn_points[0].location
else:
destination = spawn_points[1].location
agent.set_destination(agent.vehicle.get_location(), destination)
clock = pygame.time.Clock()
@ -719,31 +716,17 @@ def game_loop(args):
clock.tick_busy_loop(60)
if controller.parse_events():
return
# As soon as the server is ready continue!
if not world.world.wait_for_tick(10.0):
continue
if args.agent == "Basic":
if controller.parse_events():
return
world.world.wait_for_tick(10.0)
world.tick(clock)
world.render(display)
pygame.display.flip()
# as soon as the server is ready continue!
world.world.wait_for_tick(10.0)
world.tick(clock)
world.render(display)
pygame.display.flip()
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
else:
if args.agent == "Behavior":
agent.update_information()
world.tick(clock)
world.render(display)
pygame.display.flip()
# Set new destination when target has been reached
if len(agent.get_local_planner()._waypoints_queue) < num_min_waypoints and args.loop:
agent.reroute(spawn_points)
@ -758,8 +741,9 @@ def game_loop(args):
speed_limit = world.player.get_speed_limit()
agent.get_local_planner().set_speed(speed_limit)
control = agent.run_step()
world.player.apply_control(control)
control = agent.run_step()
control.manual_gear_shift = False
world.player.apply_control(control)
finally:
if world is not None: