class BasicAgent(Agent):
"""
BasicAgent implements a basic agent that navigates scenes to reach a given
target destination. This agent respects traffic lights and other vehicles.
"""
def __init__(self, vehicle, target_speed=20):
"""
:param vehicle: actor to apply to local planner logic onto
"""
super(BasicAgent, self).__init__(vehicle)
self._proximity_threshold = 10.0 # meters
self._state = AgentState.NAVIGATING
self._local_planner = LocalPlanner(
self._vehicle, opt_dict={'target_speed': target_speed})
self._hop_resolution = 2.0
# setting up global router
self._current_plan = None
def add_waypoint(self, waypoint):
self._local_planner.add_waypoint(waypoint)
def get_finished_waypoints(self):
return self._local_planner.get_finished_waypoints()
def drop_waypoint_buffer(self):
self._local_planner.drop_waypoint_buffer()
def set_destination(self, location):
start_waypoint = self._map.get_waypoint(self._vehicle.get_location())
end_waypoint = self._map.get_waypoint(
carla.Location(location[0], location[1], location[2]))
solution = []
# Setting up global router
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map())
grp = GlobalRoutePlanner(dao)
grp.setup()
# Obtain route plan
x1 = start_waypoint.transform.location.x
y1 = start_waypoint.transform.location.y
x2 = end_waypoint.transform.location.x
y2 = end_waypoint.transform.location.y
route = grp.plan_route((x1, y1), (x2, y2))
current_waypoint = start_waypoint
route.append(RoadOption.VOID)
for action in route:
# Generate waypoints to next junction
wp_choice = current_waypoint.next(self._hop_resolution)
while len(wp_choice) == 1:
current_waypoint = wp_choice[0]
solution.append((current_waypoint, RoadOption.LANEFOLLOW))
wp_choice = current_waypoint.next(self._hop_resolution)
# Stop at destination
if current_waypoint.transform.location.distance(
end_waypoint.transform.location
) < self._hop_resolution:
break
if action == RoadOption.VOID: break
# Select appropriate path at the junction
if len(wp_choice) > 1:
# Current heading vector
current_transform = current_waypoint.transform
current_location = current_transform.location
projected_location = current_location + \
carla.Location(
x=math.cos(math.radians(current_transform.rotation.yaw)),
y=math.sin(math.radians(current_transform.rotation.yaw)))
v_current = vector(current_location, projected_location)
direction = 0
if action == RoadOption.LEFT:
direction = 1
elif action == RoadOption.RIGHT:
direction = -1
elif action == RoadOption.STRAIGHT:
direction = 0
select_criteria = float('inf')
# Choose correct path
for wp_select in wp_choice:
v_select = vector(current_location,
wp_select.transform.location)
cross = float('inf')
if direction == 0:
cross = abs(np.cross(v_current, v_select)[-1])
else:
cross = direction * np.cross(v_current, v_select)[-1]
if cross < select_criteria:
select_criteria = cross
current_waypoint = wp_select
# Generate all waypoints within the junction
# along selected path
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(
self._hop_resolution)[0]
while current_waypoint.is_intersection:
solution.append((current_waypoint, action))
current_waypoint = current_waypoint.next(
self._hop_resolution)[0]
assert solution
self._current_plan = solution
self._local_planner.set_global_plan(self._current_plan)
def run_step(self, debug=False):
"""
Execute one step of navigation.
:return: carla.VehicleControl
"""
# is there an obstacle in front of us?
hazard_detected = False
# retrieve relevant elements for safe navigation, i.e.: traffic lights
# and other vehicles
actor_list = self._world.get_actors()
vehicle_list = actor_list.filter("*vehicle*")
lights_list = actor_list.filter("*traffic_light*")
# check possible obstacles
vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list)
if vehicle_state:
if debug:
print('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id))
self._state = AgentState.BLOCKED_BY_VEHICLE
hazard_detected = True
# check for the state of the traffic lights
light_state, traffic_light = self._is_light_red(lights_list)
if light_state:
if debug:
print('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id))
self._state = AgentState.BLOCKED_RED_LIGHT
hazard_detected = True
if hazard_detected:
control = self.emergency_stop()
else:
self._state = AgentState.NAVIGATING
# standard local planner behavior
control = self._local_planner.run_step()
return control
class BasicAgent(Agent):
"""
BasicAgent implements a basic agent that navigates scenes to reach a given
target destination. This agent respects traffic lights and other vehicles.
"""
def __init__(self, vehicle, target_speed=20):
"""
:param vehicle: actor to apply to local planner logic onto
"""
super(BasicAgent, self).__init__(vehicle)
self._proximity_threshold = 10.0 # meters
self._state = AgentState.NAVIGATING
args_lateral_dict = {'K_P': 1, 'K_D': 0.02, 'K_I': 0, 'dt': 1.0 / 20.0}
self._local_planner = LocalPlanner(self._vehicle,
opt_dict={
'target_speed':
target_speed,
'lateral_control_dict':
args_lateral_dict
})
self._hop_resolution = 2.0
self._path_seperation_hop = 2
self._path_seperation_threshold = 0.5
self._target_speed = target_speed
self._grp = None
self._stopped = False
def set_destination(self, location):
"""
This method creates a list of waypoints from agent's position to destination location
based on the route returned by the global router
"""
start_waypoint = self._map.get_waypoint(self._vehicle.get_location())
end_waypoint = self._map.get_waypoint(
carla.Location(location[0], location[1], location[2]))
route_trace = self._trace_route(start_waypoint, end_waypoint)
assert route_trace
self._local_planner.set_global_plan(route_trace)
def set_sumo_drive(self, sumo_drive=False):
self._stopped = True
self._local_planner.set_sumo_drive(sumo_drive)
def compute_next_waypoints(self, k=3):
waypoint = self._map.get_waypoint(self._vehicle.get_location())
self._local_planner.add_carla_waypoint(waypoint)
self._local_planner._compute_next_waypoints(k)
def add_waypoint(self, waypoint):
self._local_planner.add_waypoint(waypoint)
def set_target_speed(self, target_speed):
self._target_speed = target_speed
def get_finished_waypoints(self):
return self._local_planner.get_finished_waypoints()
def drop_waypoint_buffer(self):
self._local_planner.drop_waypoint_buffer()
def _trace_route(self, start_waypoint, end_waypoint):
"""
This method sets up a global router and returns the optimal route
from start_waypoint to end_waypoint
"""
# Setting up global router
if self._grp is None:
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(),
self._hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
self._grp = grp
# Obtain route plan
route = self._grp.trace_route(start_waypoint.transform.location,
end_waypoint.transform.location)
return route
def run_step(self, debug=True):
"""
Execute one step of navigation.
:return: carla.VehicleControl
"""
# is there an obstacle in front of us?
hazard_detected = False
# retrieve relevant elements for safe navigation, i.e.: traffic lights
# and other vehicles
actor_list = self._world.get_actors()
vehicle_list = actor_list.filter("*vehicle*")
lights_list = actor_list.filter("*traffic_light*")
# check possible obstacles
vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list)
if vehicle_state:
if debug:
print('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id))
self._state = AgentState.BLOCKED_BY_VEHICLE
hazard_detected = True
# check for the state of the traffic lights
light_state, traffic_light = self._is_light_red(lights_list)
if light_state:
if debug:
print('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id))
self._state = AgentState.BLOCKED_RED_LIGHT
hazard_detected = True
if hazard_detected or self._stopped:
control = self.emergency_stop()
else:
self._state = AgentState.NAVIGATING
# standard local planner behavior
control = self._local_planner.run_step(debug=debug)
return control
