class RoamingAgent(Agent):
"""
RoamingAgent implements a basic agent that navigates scenes making random
choices when facing an intersection.
This agent respects traffic lights and other vehicles.
"""
def __init__(self, vehicle):
"""
:param vehicle: actor to apply to local planner logic onto
"""
super(RoamingAgent, self).__init__(vehicle)
self._proximity_threshold = 10.0 # meters
self._state = AgentState.NAVIGATING
self._local_planner = LocalPlanner(self._vehicle)
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 Planner(object):
def __init__(self, vehicle = None):
self._vehicle = None
self.global_planner = None
self.local_planner = None
self.resolution = 20.0
self.route_trace = None
if vehicle is not None:
self.initialize(vehicle)
def initialize(self, vehicle):
self._vehicle = vehicle
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self.resolution)
self.global_planner = GlobalRoutePlanner(dao)
self.global_planner.setup()
self.local_planner = LocalPlanner(self._vehicle)
def set_destination(self, location):
start_waypoint = self._vehicle.get_world().get_map().get_waypoint(self._vehicle.get_location())
end_waypoint = self._vehicle.get_world().get_map().get_waypoint(carla.Location(location[0], location[1], location[2]))
self.route_trace = self.global_planner.trace_route(start_waypoint.transform.location, end_waypoint.transform.location)
#self.route_trace.pop(0)
#assert self.route_trace
self.local_planner.set_global_plan(self.route_trace)
def run_step(self):
return self.local_planner.run_step(False)
def view_plan(self):
for w in self.route_trace:
self._vehicle.get_world().debug.draw_string(w[0].transform.location, 'o', draw_shadow=False,
color=carla.Color(r=255, g=0, b=0), life_time=120.0,
persistent_lines=True)
def done(self):
return self.local_planner.done()
class AutopilotAgent(oatomobile.Agent):
"""An autopilot agent, based on the official implementation of
`carla.PythonAPI.agents.navigation.basic_agent.BasicAgent`"""
def __init__(self,
environment: oatomobile.envs.CARLAEnv,
*,
proximity_tlight_threshold: float = 9.5,
proximity_vehicle_threshold: float = 9.5,
noise: float = 0.1) -> None:
"""Constructs an autopilot agent.
Args:
environment: The navigation environment to spawn the agent.
proximity_tlight_threshold: The threshold (in metres) to
the traffic light.
proximity_vehicle_threshold: The threshold (in metres) to
the front vehicle.
noise: The percentage of random actions.
"""
super(AutopilotAgent, 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()
self._road_map = imread(osp.join(osp.dirname(__file__), '%s.png' % self._map.name))
# Agent hyperparametres.
self._proximity_tlight_threshold = proximity_tlight_threshold
self._proximity_vehicle_threshold = proximity_vehicle_threshold
self._hop_resolution = 2.0
self._path_seperation_hop = 2
self._path_seperation_threshold = 0.5
self._target_speed = defaults.TARGET_SPEED
self._noise = noise
# The internal state of the agent.
self._last_traffic_light = None
# Local planner, including the PID controllers.
dt = self._vehicle.get_world().get_settings().fixed_delta_seconds
# lateral_control_dict = defaults.LATERAL_PID_CONTROLLER_CONFIG.copy()
# lateral_control_dict.update({"dt": dt})
# TODO(filangel): tune the parameters for FPS != 20
self._local_planner = LocalPlanner(
self._vehicle,
opt_dict=dict(
target_speed=self._target_speed,
dt=dt,
),
)
# Set agent's dsestination.
if hasattr(self._environment.unwrapped.simulator, "destination"):
self._set_destination(
self._environment.unwrapped.simulator.destination.location)
def act(
self,
observation: oatomobile.Observations,
) -> oatomobile.Action:
"""Takes in an observation, samples from agent's policy, returns an
action."""
# Remove unused arguments.
del observation
# Random action branch.
if random.random() < self._noise:
return carla.VehicleControl( # pylint: disable=no-member
**{
k: float(v)
for (k, v) in self._environment.action_space.sample().items()
})
# Normal autopilot action.
else:
return self._run_step()
def _run_step(
self,
debug: bool = False,
) -> carla.VehicleControl: # pylint: disable=no-member
"""Executes one step of navigation."""
# 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*")
walkers_list = actor_list.filter('*walker*')
# check possible obstacles
vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list)
if vehicle_state:
if debug:
logging.warning('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id))
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:
logging.warning('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id))
hazard_detected = True
# check for the state of the walkers
walker_state, walker = self._is_walker_hazard(walkers_list)
if walker_state:
if debug:
logging.warning('=== WALKER AHEAD [{}])'.format(walker.id))
hazard_detected = True
if hazard_detected:
control = carla.VehicleControl() # pylint: disable=no-member
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
else:
# standard local planner behavior
control = self._local_planner.run_step(debug=debug)
return control
def _set_destination(
self,
destination: carla.Location, # pylint: disable=no-member
) -> None:
"""Generates the global plan for the agent.
Args:
destination: The location of the new destination.
"""
# Set vehicle's current location as start for the plan.
origin = self._vehicle.get_location()
start_waypoint = self._map.get_waypoint(origin).transform.location
end_waypoint = self._map.get_waypoint(destination).transform.location
# Calculate the plan.
waypoints, roadoptions, _ = cutil.global_plan(
world=self._world,
origin=start_waypoint,
destination=end_waypoint,
)
# Mutate the local planner's global plan.
self._local_planner.set_global_plan(list(zip(waypoints, roadoptions)))
def _is_walker_hazard(self, walkers_list):
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
for walker in walkers_list:
loc = walker.get_location()
dist = loc.distance(ego_vehicle_location)
degree = 162 / (np.clip(dist, 1.5, 10.5)+0.3)
if self._is_point_on_sidewalk(loc):
continue
if is_within_distance_ahead(loc, ego_vehicle_location,
self._vehicle.get_transform().rotation.yaw,
self._proximity_vehicle_threshold, degree=degree):
return (True, walker)
return (False, None)
def _is_vehicle_hazard(
self,
vehicle_list,
) -> Tuple[bool, Optional[carla.Vehicle]]: # pylint: disable=no-member
"""It detects if a vehicle in the scene can be dangerous for the ego
vehicle's current plan.
Args:
vehicle_list: List of potential vehicles (obstancles) to check.
Returns:
vehicle_ahead: If True a vehicle ahead blocking us and False otherwise.
vehicle: The blocker vehicle itself.
"""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_orientation = self._vehicle.get_transform().rotation.yaw
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
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())
if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \
target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
loc = target_vehicle.get_location()
ori = target_vehicle.get_transform().rotation.yaw
if is_within_distance_ahead(
loc,
ego_vehicle_location,
self._vehicle.get_transform().rotation.yaw,
self._proximity_vehicle_threshold,
) and compute_yaw_difference(ego_vehicle_orientation, ori) <= 150:
return (True, target_vehicle)
return (False, None)
def _is_light_red(
self,
lights_list,
) -> Tuple[bool, Any]: # pylint: disable=no-member
"""It detects if the light in the scene is red.
Args:
lights_list: The list containing TrafficLight objects.
Returns:
light_ahead: If True a traffic light ahead is read and False otherwise.
traffic_light: The traffic light object ahead itself.
"""
if self._map.name == 'Town01' or self._map.name == 'Town02':
return self._is_light_red_europe_style(lights_list)
else:
return self._is_light_red_us_style(lights_list)
def _is_light_red_europe_style(self, lights_list):
"""This method is specialized to check European style traffic lights."""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
for traffic_light in lights_list:
object_waypoint = self._map.get_waypoint(traffic_light.get_location())
if object_waypoint.road_id != ego_vehicle_waypoint.road_id or \
object_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
loc = traffic_light.get_location()
if is_within_distance_ahead(
loc,
ego_vehicle_location,
self._vehicle.get_transform().rotation.yaw,
self._proximity_tlight_threshold,
):
if traffic_light.state == carla.TrafficLightState.Red: # pylint: disable=no-member
return (True, traffic_light)
return (False, None)
def _is_light_red_us_style(self, lights_list, debug=False):
"""This method is specialized to check US style traffic lights."""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
if ego_vehicle_waypoint.is_junction:
# It is too late. Do not block the intersection! Keep going!
return (False, None)
if self._local_planner.target_waypoint is not None:
if self._local_planner.target_waypoint.is_junction:
min_angle = 180.0
sel_magnitude = 0.0
sel_traffic_light = None
for traffic_light in lights_list:
loc = traffic_light.get_location()
magnitude, angle = compute_magnitude_angle(
loc, ego_vehicle_location,
self._vehicle.get_transform().rotation.yaw)
if magnitude < 60.0 and angle < min(25.0, min_angle):
sel_magnitude = magnitude
sel_traffic_light = traffic_light
min_angle = angle
if sel_traffic_light is not None:
if debug:
logging.debug('=== Magnitude = {} | Angle = {} | ID = {}'.format(
sel_magnitude, min_angle, sel_traffic_light.id))
if self._last_traffic_light is None:
self._last_traffic_light = sel_traffic_light
if self._last_traffic_light.state == carla.TrafficLightState.Red: # pylint: disable=no-member
return (True, self._last_traffic_light)
else:
self._last_traffic_light = None
return (False, None)
def _get_trafficlight_trigger_location(
self,
traffic_light,
) -> carla.Location: # pylint: disable=no-member
"""Calculates the yaw of the waypoint that represents the trigger volume of
the traffic light."""
def rotate_point(point, radians):
"""Rotates a given point by a given angle."""
rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y
rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y
return carla.Vector3D(rotated_x, rotated_y, point.z) # pylint: disable=no-member
base_transform = traffic_light.get_transform()
base_rot = base_transform.rotation.yaw
area_loc = base_transform.transform(traffic_light.trigger_volume.location)
area_ext = traffic_light.trigger_volume.extent
point = rotate_point(
carla.Vector3D(0, 0, area_ext.z), # pylint: disable=no-member
math.radians(base_rot),
)
point_location = area_loc + carla.Location(x=point.x, y=point.y) # pylint: disable=no-member
return carla.Location(point_location.x, point_location.y, point_location.z) # pylint: disable=no-member
def _world_to_pixel(self, location, offset=(0, 0)):
world_offset = WORLD_OFFSETS[self._map.name]
x = PIXELS_PER_METER * (location.x - world_offset[0])
y = PIXELS_PER_METER * (location.y - world_offset[1])
return [int(x - offset[0]), int(y - offset[1])]
def _is_point_on_sidewalk(self, loc):
# Convert to pixel coordinate
pixel_x, pixel_y = self._world_to_pixel(loc)
pixel_y = np.clip(pixel_y, 0, self._road_map.shape[0]-1)
pixel_x = np.clip(pixel_x, 0, self._road_map.shape[1]-1)
point = self._road_map[pixel_y, pixel_x, 0]
return point == 0
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.stopping_for_traffic_light = False
self._proximity_threshold = 10.0 # meters
self._state = AgentState.NAVIGATING
args_lateral_dict = {
'K_P': 0.75,
'K_D': 0.001,
'K_I': 1,
'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.drawn_lights = False
self.is_affected_by_traffic_light = 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 _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=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() # type: ActorList
vehicle_list = actor_list.filter("*vehicle*") # type: List[Actor]
pedestrians_list = actor_list.filter("*walker.pedestrian*")
lights_list = actor_list.filter(
"*traffic_light*") # type: List[carla.TrafficLight]
if not self.drawn_lights and debug:
for light in lights_list:
self._world.debug.draw_box(
carla.BoundingBox(
light.trigger_volume.location +
light.get_transform().location,
light.trigger_volume.extent * 2),
carla.Rotation(0, 0, 0), 0.05, carla.Color(255, 128, 0, 0),
0)
self.drawn_lights = True
# 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 pedestrians
pedestrian_state, pedestrian = self._is_pedestrian_hazard(
pedestrians_list)
if pedestrian_state:
if debug:
print('!!! PEDESTRIAN BLOCKING AHEAD [{}])'.format(
pedestrian.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
new_target_speed = self._update_target_speed(hazard_detected, debug)
# if hazard_detected:
# control = self.emergency_stop()
# else:
# self._state = AgentState.NAVIGATING
# self.braking_intial_speed = None
# # standard local planner behavior
# control = self._local_planner.run_step(debug=debug)
# if self.stopping_for_traffic_light:
# control.steer = 0.0
self._state = AgentState.NAVIGATING
self.braking_intial_speed = None
# standard local planner behavior
control = self._local_planner.run_step(debug=debug)
if self.stopping_for_traffic_light:
control.steer = 0.0
# Prevent from steering randomly when stopped
if math.fabs(get_speed(self._vehicle)) < 0.1:
control.steer = 0
return control
def done(self):
"""
Check whether the agent has reached its destination.
:return bool
"""
return self._local_planner.done()
def _update_target_speed(self, hazard_detected, debug):
if hazard_detected:
self._set_target_speed(0)
return 0
MAX_PERCENTAGE_OF_SPEED_LIMIT = 0.75
speed_limit = self._vehicle.get_speed_limit() # km/h
current_speed = get_speed(self._vehicle)
new_target_speed = speed_limit * MAX_PERCENTAGE_OF_SPEED_LIMIT
use_custom_traffic_light_speed = False
if use_custom_traffic_light_speed:
TRAFFIC_LIGHT_SECONDS_AWAY = 3
METERS_TO_STOP_BEFORE_TRAFFIC_LIGHT = 8
get_traffic_light = self._vehicle.get_traffic_light(
) # type: carla.TrafficLight
nearest_traffic_light, distance = get_nearest_traffic_light(
self._vehicle) # type: carla.TrafficLight, float
distance_to_light = distance
distance -= METERS_TO_STOP_BEFORE_TRAFFIC_LIGHT
if nearest_traffic_light is None:
nearest_traffic_light = get_traffic_light
# Draw debug info
if debug and nearest_traffic_light is not None:
self._world.debug.draw_point(
nearest_traffic_light.get_transform().location,
size=1,
life_time=0.1,
color=carla.Color(255, 15, 15))
"""
if get_traffic_light is not None:
print("get_traffic_light: ", get_traffic_light.get_location() if get_traffic_light is not None else "None", " ", get_traffic_light.state if get_traffic_light is not None else "None")
if nearest_traffic_light is not None:
print("nearest_traffic_light: ", nearest_traffic_light.get_location() if nearest_traffic_light is not None else "None", " ", nearest_traffic_light.state if nearest_traffic_light is not None else "None")
"""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
self.is_affected_by_traffic_light = False
self.stopping_for_traffic_light = False
if ego_vehicle_waypoint.is_junction:
# It is too late. Do not block the intersection! Keep going!
pass
# Check if we should start braking
elif distance_to_light <= TRAFFIC_LIGHT_SECONDS_AWAY * new_target_speed / 3.6 and nearest_traffic_light is not None and nearest_traffic_light.state != carla.TrafficLightState.Green:
self.is_affected_by_traffic_light = True
brake_distance = current_speed / 3.6 * TRAFFIC_LIGHT_SECONDS_AWAY
print("TL distance: ", distance_to_light,
", distance (to stop): ", distance,
", distance travel 4 secs: ", brake_distance)
new_target_speed = self._target_speed
if distance <= 0:
new_target_speed = 0
self.stopping_for_traffic_light = True
print("Stopping before traffic light, distance ",
distance, "m")
elif brake_distance >= distance and brake_distance != 0:
percent_before_light = (brake_distance -
distance) / brake_distance
new_target_speed = speed_limit - max(
0, percent_before_light) * speed_limit
print("Slowing down before traffic light ",
percent_before_light * 100, "% ", new_target_speed,
" km/h")
self._set_target_speed(max(0, new_target_speed))
return new_target_speed
def _set_target_speed(self, target_speed: int):
"""
This function updates all the needed values required to actually set a new target speed
"""
self._target_speed = target_speed
self._local_planner.set_speed(target_speed)
class LearningAgent(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, world):
"""
:param vehicle: actor to apply to local planner logic onto
"""
super(LearningAgent, self).__init__(world.player)
self._world_obj = world
# Learning Model
self._model = Model()
self._THW = None
self._target_speed = None
self._sin_param = None
self._poly_param = None
# Local plannar
self._local_planner = LocalPlanner(world.player)
self.update_parameters()
# Global plannar
self._proximity_threshold = 10.0 # meter
self._state = AgentState.NAVIGATING
self._hop_resolution = 0.2
self._path_seperation_hop = 2
self._path_seperation_threshold = 0.5
self._grp = None # global route planar
# Behavior planning
self._hazard_detected = False
self._blocked_time = None
self._perform_lane_change = False
self._front_r = []
self._left_front_r = []
self._left_back_r = []
# Update personalized parameters from model
def update_parameters(self):
self._THW = self._model.get_parameter("safe_distance")["THW"]
self._target_speed = self._model.get_parameter("target_speed") * 3.6
self._sin_param = self._model.get_parameter("sin_param")
self._poly_param = self._model.get_parameter("poly_param")
CONTROLLER_TYPE = 'PID' # options:MPC, PID, STANLEY
args_lateral_dict = {'K_P': 1.0, 'K_I': 0.4, 'K_D': 0.01, 'control_type': CONTROLLER_TYPE}
args_longitudinal_dict = {'K_P': 0.3, 'K_I': 0.2, 'K_D': 0.002}
self._local_planner.init_controller(opt_dict={'target_speed': self._target_speed,
'lateral_control_dict': args_lateral_dict,
'longitudinal_control_dict': args_longitudinal_dict})
# Start learning by collecting data
def collect(self):
# State for each step
personalization_param = []
# Time stamp
personalization_param.extend([pygame.time.get_ticks()])
# Collect vehicle position
t = self._vehicle.get_transform()
personalization_param.extend([t.location.x,
t.location.y,
t.location.z,
t.rotation.yaw])
# Collect vehicle velocity and speed
v = self._vehicle.get_velocity()
personalization_param.extend([v.x, v.y, v.z, self._get_speed()])
# Collect radar information
front_dis = 100
front_vel = 50
left_front_dis = 100
left_front_vel = 50
left_back_dis = -100
left_back_vel = 0
if self._front_r:
front_dis = self._front_r[1][0]
front_vel = self._front_r[2][0]
if self._left_front_r:
left_front_dis = self._left_front_r[1][0]
left_front_vel = self._left_front_r[2][0]
if self._left_back_r:
left_back_dis = self._left_back_r[1][0]
left_back_vel = self._left_back_r[2][0]
personalization_param.extend([front_dis, left_front_dis, left_back_dis,
front_vel, left_front_vel, left_back_vel])
self._model.collect(personalization_param)
# End collection
def end_collect(self):
self._model.end_collect()
# Train model
def train_model(self):
self._model.train_new_model()
# Set global destination and get global waypoints
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)
# Get global waypoints
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
# Get vehicle speed
def _get_speed(self):
v = self._vehicle.get_velocity()
ego_speed = math.sqrt(v.x**2 + v.y**2 + v.z**2)
return ego_speed
# Run step
def run_step(self, debug=False):
"""
Execute one step of navigation.
:return: carla.VehicleControl
"""
## Update Environment ##
# Check all the radars
if self._world_obj.front_radar.detected:
if abs(self._world_obj.front_radar.rel_pos[1]) < 1:
self._front_r = [pygame.time.get_ticks(), self._world_obj.front_radar.rel_pos,
self._world_obj.front_radar.rel_vel]
self._world_obj.front_radar.detected = False
if self._world_obj.left_front_radar.detected:
if self._world_obj.left_front_radar.rel_pos[1] < -1:
self._left_front_r =[pygame.time.get_ticks(), self._world_obj.left_front_radar.rel_pos,
self._world_obj.left_front_radar.rel_vel]
self._world_obj.left_front_radar.detected = False
if self._world_obj.left_back_radar.detected:
if self._world_obj.left_back_radar.rel_pos[1] < -1:
self._left_back_r = [pygame.time.get_ticks(), self._world_obj.left_back_radar.rel_pos,
self._world_obj.left_back_radar.rel_vel]
self._world_obj.left_back_radar.detected = False
# Remove radar data if not detected again in 0.5 second
if self._front_r and (pygame.time.get_ticks() - self._front_r[0] > 5000):
self._front_r = []
if self._left_front_r and (pygame.time.get_ticks() - self._left_front_r[0] > 5000):
self._left_front_r = []
if self._left_back_r and (pygame.time.get_ticks() - self._left_back_r[0] > 5000):
self._left_back_r = []
# Detect vehicles in front
self._hazard_detected = False
if self._front_r and (self._front_r[1][0] < 20.0):
self._hazard_detected = True
# update hazard existing time
if self._hazard_detected:
if self._blocked_time is None:
self._blocked_time = pygame.time.get_ticks()
hazard_time = 0
else:
hazard_time = pygame.time.get_ticks() - self._blocked_time
else:
self._blocked_time = None
# Get a safe_distance
safe_distance = self._THW * self._get_speed()
'''
# retrieve relevant elements for safe navigation, i.e.: traffic lights
actor_list = self._world.get_actors()
lights_list = actor_list.filter("*traffic_light*")
# 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
self._hazard_detected = True
'''
#print(self._state)
# Finite State Machine
# 1, Navigating
if self._state == AgentState.NAVIGATING:
if self._hazard_detected:
self._state = AgentState.BLOCKED_BY_VEHICLE
# 2, Blocked by Vehicle
elif self._state == AgentState.BLOCKED_BY_VEHICLE:
if not self._hazard_detected:
self._state = AgentState.NAVIGATING
# The vehicle is driving at a certain speed
# There is enough space
else:
if hazard_time > 5000 and \
190 > self._vehicle.get_location().x > 10 and \
10 > self._vehicle.get_location().y > 7:
self._state = AgentState.PREPARE_LANE_CHANGING
# 4, Prepare Lane Change
elif self._state == AgentState.PREPARE_LANE_CHANGING:
if not (self._front_r and self._front_r[1][0] < safe_distance) and \
not (self._left_front_r and self._left_front_r[1][0] < safe_distance) and \
not (self._left_back_r and self._left_back_r[1][0] > -10):
print(self._front_r)
print(self._left_front_r)
print(self._left_back_r)
self._state = AgentState.LANE_CHANGING
self._perform_lane_change = True
# 5, Lane Change
elif self._state == AgentState.LANE_CHANGING:
if abs(self._vehicle.get_velocity().y) < 0.5 and \
self._vehicle.get_location().y < 7.0:
self._state = AgentState.NAVIGATING
# 6, Emergency Brake
emergency_distance = safe_distance *3/5
emergency_front_speed = 1.0
if self._front_r and (self._front_r[1][0] < emergency_distance or
self._front_r[2][0] < emergency_front_speed):
self._state = AgentState.EMERGENCY_BRAKE
# Local Planner Behavior according to states
if self._state == AgentState.NAVIGATING or self._state == AgentState.LANE_CHANGING:
control = self._local_planner.run_step(debug=debug)
elif self._state == AgentState.PREPARE_LANE_CHANGING:
if self._left_front_r and self._left_front_r[1][0] < safe_distance or \
self._front_r and self._front_r[1][0] < safe_distance:
control = self._local_planner.empty_control(debug=debug)
else:
control = self._local_planner.run_step(debug=debug)
elif self._state == AgentState.BLOCKED_BY_VEHICLE:
# ACC
front_dis = self._front_r[1][0]
front_vel = self._front_r[2][0]
ego_speed = self._get_speed()
desired_speed = front_vel - (ego_speed-front_vel)/front_dis
if ego_speed > 1:
desired_speed += 2*(front_dis/ego_speed - self._THW)
control = self._local_planner.run_step(debug=debug, target_speed=desired_speed*3.6)
elif self._state == AgentState.EMERGENCY_BRAKE:
control = self._local_planner.brake()
if self._front_r:
if self._front_r[1][0] >= emergency_distance and \
self._front_r[2][0] > emergency_front_speed:
self._state = AgentState.NAVIGATING
elif self._state == AgentState.BLOCKED_RED_LIGHT:
control = self._local_planner.empty_control(debug=debug)
# When performing a lane change
if self._perform_lane_change:
# Record original destination
destination = self._local_planner.get_global_destination()
# Get lane change start location
ref_location = self._world_obj.player.get_location()
ref_yaw = self._world_obj.player.get_transform().rotation.yaw
if self._local_planner.waypoint_buffer:
waypoint = self._local_planner.waypoint_buffer[-1][0]
ref_location = waypoint.transform.location
wait_dist = 0.0 # need some time to plan
ref = [ref_location.x + wait_dist, ref_location.y, ref_yaw]
# Replace current plan with a lane change plan
DL = self._left_front_r[1][0] if self._left_front_r else 100
DH = self._left_back_r[1][0] if self._left_back_r else 100
GMM_v = [[self._vehicle.get_velocity().x, self._sin_param["lat_dis"], DL, DH]]
lane_changer = SinLaneChange(self._world_obj, self._sin_param, np.array(GMM_v))
lane_change_plan = lane_changer.get_waypoints(ref)
self._local_planner.set_local_plan(lane_change_plan)
'''
lane_changer = PolyLaneChange(self._world_obj, self._poly_param)
lane_change_plan = lane_changer.get_waypoints(ref)
self._local_planner.set_local_plan(lane_change_plan)
'''
# replan globally with new vehicle position after lane changing
new_start = self._map.get_waypoint(lane_change_plan[-1][0].transform.location)
route_trace = self._trace_route(new_start, destination)
assert route_trace
self._local_planner.add_global_plan(route_trace)
self._perform_lane_change = False
print("perform lane change")
return control
def done(self):
"""
Check whether the agent has reached its destination.
:return bool
"""
return self._local_planner.done()
class BasicAgent(object):
"""
BasicAgent implements an agent that navigates the scene.
This agent respects traffic lights and other vehicles, but ignores stop signs.
It has several functions available to specify the route that the agent must follow,
as well as to change its parameters in case a different driving mode is desired.
"""
def __init__(self, vehicle, target_speed=20, opt_dict={}):
"""
Initialization the agent paramters, the local and the global planner.
:param vehicle: actor to apply to agent logic onto
:param target_speed: speed (in Km/h) at which the vehicle will move
:param opt_dict: dictionary in case some of its parameters want to be changed.
This also applies to parameters related to the LocalPlanner.
"""
self._vehicle = vehicle
self._world = self._vehicle.get_world()
self._map = self._world.get_map()
self._last_traffic_light = None
# Base parameters
self._ignore_traffic_lights = False
self._ignore_stop_signs = False
self._ignore_vehicles = False
self._target_speed = target_speed
self._sampling_resolution = 2.0
self._base_tlight_threshold = 5.0 # meters
self._base_vehicle_threshold = 5.0 # meters
self._max_brake = 0.5
# Change parameters according to the dictionary
opt_dict['target_speed'] = target_speed
if 'ignore_traffic_lights' in opt_dict:
self._ignore_traffic_lights = opt_dict['ignore_traffic_lights']
if 'ignore_stop_signs' in opt_dict:
self._ignore_stop_signs = opt_dict['ignore_stop_signs']
if 'ignore_vehicles' in opt_dict:
self._ignore_vehicles = opt_dict['ignore_vehicles']
if 'sampling_resolution' in opt_dict:
self._sampling_resolution = opt_dict['sampling_resolution']
if 'base_tlight_threshold' in opt_dict:
self._base_tlight_threshold = opt_dict['base_tlight_threshold']
if 'base_vehicle_threshold' in opt_dict:
self._base_vehicle_threshold = opt_dict['base_vehicle_threshold']
if 'max_brake' in opt_dict:
self._max_steering = opt_dict['max_brake']
# Initialize the planners
self._local_planner = LocalPlanner(self._vehicle, opt_dict=opt_dict)
self._global_planner = GlobalRoutePlanner(self._map,
self._sampling_resolution)
def add_emergency_stop(self, control):
"""
Overwrites the throttle a brake values of a control to perform an emergency stop.
The steering is kept the same to avoid going out of the lane when stopping during turns
:param speed (carl.VehicleControl): control to be modified
"""
control.throttle = 0.0
control.brake = self._max_brake
control.hand_brake = False
return control
def set_target_speed(self, speed):
"""
Changes the target speed of the agent
:param speed (float): target speed in Km/h
"""
self._local_planner.set_speed(speed)
def follow_speed_limits(self, value=True):
"""
If active, the agent will dynamically change the target speed according to the speed limits
:param value (bool): whether or not to activate this behavior
"""
self._local_planner.follow_speed_limits(value)
def get_local_planner(self):
"""Get method for protected member local planner"""
return self._local_planner
def get_global_planner(self):
"""Get method for protected member local planner"""
return self._global_planner
def set_destination(self, end_location, start_location=None):
"""
This method creates a list of waypoints between a starting and ending location,
based on the route returned by the global router, and adds it to the local planner.
If no starting location is passed, the vehicle local planner's target location is chosen,
which corresponds (by default), to a location about 5 meters in front of the vehicle.
:param end_location (carla.Location): final location of the route
:param start_location (carla.Location): starting location of the route
"""
if not start_location:
start_location = self._local_planner.target_waypoint.transform.location
clean_queue = True
else:
start_location = self._vehicle.get_location()
clean_queue = False
start_waypoint = self._map.get_waypoint(start_location)
end_waypoint = self._map.get_waypoint(end_location)
route_trace = self.trace_route(start_waypoint, end_waypoint)
self._local_planner.set_global_plan(route_trace,
clean_queue=clean_queue)
def set_global_plan(self,
plan,
stop_waypoint_creation=True,
clean_queue=True):
"""
Adds a specific plan to the agent.
:param plan: list of [carla.Waypoint, RoadOption] representing the route to be followed
:param stop_waypoint_creation: stops the automatic random creation of waypoints
:param clean_queue: resets the current agent's plan
"""
self._local_planner.set_global_plan(
plan,
stop_waypoint_creation=stop_waypoint_creation,
clean_queue=clean_queue)
def trace_route(self, start_waypoint, end_waypoint):
"""
Calculates the shortest route between a starting and ending waypoint.
:param start_waypoint (carla.Waypoint): initial waypoint
:param end_waypoint (carla.Waypoint): final waypoint
"""
start_location = start_waypoint.transform.location
end_location = end_waypoint.transform.location
return self._global_planner.trace_route(start_location, end_location)
def run_step(self):
"""Execute one step of navigation."""
hazard_detected = False
# Retrieve all relevant actors
actor_list = self._world.get_actors()
vehicle_list = actor_list.filter("*vehicle*")
lights_list = actor_list.filter("*traffic_light*")
vehicle_speed = get_speed(self._vehicle) / 3.6
# Check for possible vehicle obstacles
max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed
affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(
vehicle_list, max_vehicle_distance)
if affected_by_vehicle:
hazard_detected = True
# Check if the vehicle is affected by a red traffic light
max_tlight_distance = self._base_tlight_threshold + vehicle_speed
affected_by_tlight, _ = self._affected_by_traffic_light(
lights_list, max_tlight_distance)
if affected_by_tlight:
hazard_detected = True
control = self._local_planner.run_step()
if hazard_detected:
control = self.add_emergency_stop(control)
return control
def done(self):
"""Check whether the agent has reached its destination."""
return self._local_planner.done()
def ignore_traffic_lights(self, active=True):
"""(De)activates the checks for traffic lights"""
self._ignore_traffic_lights = active
def ignore_stop_signs(self, active=True):
"""(De)activates the checks for stop signs"""
self._ignore_stop_signs = active
def ignore_vehicles(self, active=True):
"""(De)activates the checks for stop signs"""
self._ignore_vehicles = active
def _affected_by_traffic_light(self, lights_list=None, max_distance=None):
"""
Method to check if there is a red light affecting the vehicle.
:param lights_list (list of carla.TrafficLight): list containing TrafficLight objects.
If None, all traffic lights in the scene are used
:param max_distance (float): max distance for traffic lights to be considered relevant.
If None, the base threshold value is used
"""
if self._ignore_traffic_lights:
return (False, None)
if not lights_list:
lights_list = self._world.get_actors().filter("*traffic_light*")
if not max_distance:
max_distance = self._base_tlight_threshold
if self._last_traffic_light:
if self._last_traffic_light.state != carla.TrafficLightState.Red:
self._last_traffic_light = None
else:
return (True, self._last_traffic_light)
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
for traffic_light in lights_list:
object_location = get_trafficlight_trigger_location(traffic_light)
object_waypoint = self._map.get_waypoint(object_location)
if object_waypoint.road_id != ego_vehicle_waypoint.road_id:
continue
ve_dir = ego_vehicle_waypoint.transform.get_forward_vector()
wp_dir = object_waypoint.transform.get_forward_vector()
dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z
if dot_ve_wp < 0:
continue
if traffic_light.state != carla.TrafficLightState.Red:
continue
if is_within_distance(object_waypoint.transform,
self._vehicle.get_transform(), max_distance,
[0, 90]):
self._last_traffic_light = traffic_light
return (True, traffic_light)
return (False, None)
def _vehicle_obstacle_detected(self,
vehicle_list=None,
max_distance=None,
up_angle_th=90,
low_angle_th=0,
lane_offset=0):
"""
Method to check if there is a vehicle in front of the agent blocking its path.
:param vehicle_list (list of carla.Vehicle): list contatining vehicle objects.
If None, all vehicle in the scene are used
:param max_distance: max freespace to check for obstacles.
If None, the base threshold value is used
"""
if self._ignore_vehicles:
return (False, None, -1)
if not vehicle_list:
vehicle_list = self._world.get_actors().filter("*vehicle*")
if not max_distance:
max_distance = self._base_vehicle_threshold
ego_transform = self._vehicle.get_transform()
ego_wpt = self._map.get_waypoint(self._vehicle.get_location())
# Get the right offset
if ego_wpt.lane_id < 0 and lane_offset != 0:
lane_offset *= -1
# Get the transform of the front of the ego
ego_forward_vector = ego_transform.get_forward_vector()
ego_extent = self._vehicle.bounding_box.extent.x
ego_front_transform = ego_transform
ego_front_transform.location += carla.Location(
x=ego_extent * ego_forward_vector.x,
y=ego_extent * ego_forward_vector.y,
)
for target_vehicle in vehicle_list:
target_transform = target_vehicle.get_transform()
target_wpt = self._map.get_waypoint(target_transform.location,
lane_type=carla.LaneType.Any)
# Simplified version for outside junctions
if not ego_wpt.is_junction or not target_wpt.is_junction:
if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id + lane_offset:
next_wpt = self._local_planner.get_incoming_waypoint_and_direction(
steps=3)[0]
if not next_wpt:
continue
if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id + lane_offset:
continue
target_forward_vector = target_transform.get_forward_vector()
target_extent = target_vehicle.bounding_box.extent.x
target_rear_transform = target_transform
target_rear_transform.location -= carla.Location(
x=target_extent * target_forward_vector.x,
y=target_extent * target_forward_vector.y,
)
if is_within_distance(target_rear_transform,
ego_front_transform, max_distance,
[low_angle_th, up_angle_th]):
return (True, target_vehicle,
compute_distance(target_transform.location,
ego_transform.location))
# Waypoints aren't reliable, check the proximity of the vehicle to the route
else:
route_bb = []
ego_location = ego_transform.location
extent_y = self._vehicle.bounding_box.extent.y
r_vec = ego_transform.get_right_vector()
p1 = ego_location + carla.Location(extent_y * r_vec.x,
extent_y * r_vec.y)
p2 = ego_location + carla.Location(-extent_y * r_vec.x,
-extent_y * r_vec.y)
route_bb.append([p1.x, p1.y, p1.z])
route_bb.append([p2.x, p2.y, p2.z])
for wp, _ in self._local_planner.get_plan():
if ego_location.distance(
wp.transform.location) > max_distance:
break
r_vec = wp.transform.get_right_vector()
p1 = wp.transform.location + carla.Location(
extent_y * r_vec.x, extent_y * r_vec.y)
p2 = wp.transform.location + carla.Location(
-extent_y * r_vec.x, -extent_y * r_vec.y)
route_bb.append([p1.x, p1.y, p1.z])
route_bb.append([p2.x, p2.y, p2.z])
if len(route_bb) < 3:
# 2 points don't create a polygon, nothing to check
return (False, None, -1)
ego_polygon = Polygon(route_bb)
# Compare the two polygons
for target_vehicle in vehicle_list:
target_extent = target_vehicle.bounding_box.extent.x
if target_vehicle.id == self._vehicle.id:
continue
if ego_location.distance(
target_vehicle.get_location()) > max_distance:
continue
target_bb = target_vehicle.bounding_box
target_vertices = target_bb.get_world_vertices(
target_vehicle.get_transform())
target_list = [[v.x, v.y, v.z] for v in target_vertices]
target_polygon = Polygon(target_list)
if ego_polygon.intersects(target_polygon):
return (True, target_vehicle,
compute_distance(target_vehicle.get_location(),
ego_location))
return (False, None, -1)
return (False, None, -1)
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 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 PIDAgent(MapAgent):
def setup(self, path_to_conf_file):
super().setup(path_to_conf_file)
self.save_path = None
parent_folder = os.environ['SAVE_FOLDER']
string = pathlib.Path(os.environ['ROUTES']).stem
self.save_path = pathlib.Path(parent_folder) / string
def _init(self):
super()._init()
self._proximity_tlight_threshold = 5.0 # meters
self._proximity_vehicle_threshold = 10.0 # meters
self._local_planner = None
try:
self._map = self._world.get_map()
except RuntimeError as error:
print('RuntimeError: {}'.format(error))
print(' The server could not send the OpenDRIVE (.xodr) file:')
print(' Make sure it exists, has the same name of your town, and is correct.')
sys.exit(1)
self._last_traffic_light = None
self._proximity_tlight_threshold = 5.0 # meters
self._proximity_vehicle_threshold = 10.0 # meters
self._state = AgentState.NAVIGATING
# TBD: subject to change
self.target_speed = 7
args_lateral_dict = {
'K_P': 1,
'K_D': 0.4,
'K_I': 0,
'dt': 1.0/20.0}
self._local_planner = LocalPlanner(
self._vehicle, opt_dict={'target_speed' : self.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._grp = None
self._map = CarlaDataProvider.get_map()
route = [(self._map.get_waypoint(x[0].location), x[1]) for x in self._global_plan_world_coord]
self._local_planner.set_global_plan(route)
def _get_control(self, tick_data, _draw):
pos = self._get_position(tick_data)
theta = tick_data['compass']
speed = tick_data['speed']
# 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:
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:
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(debug=DEBUG)
_draw.text((5, 90), 'Speed: %.3f' % speed)
_draw.text((5, 110), 'Target: %.3f' % self.target_speed)
return control
def run_step(self, input_data, timestamp):
if not self.initialized:
self._init()
data = self.tick(input_data)
rgb_with_car = cv2.cvtColor(input_data['rgb_with_car'][1][:, :, :3], cv2.COLOR_BGR2RGB)
data['rgb_with_car'] = rgb_with_car
topdown = data['topdown']
rgb = np.hstack((data['rgb_left'], data['rgb'], data['rgb_right']))
gps = self._get_position(data)
_topdown = Image.fromarray(COLOR[CONVERTER[topdown]])
_rgb = Image.fromarray(rgb)
_draw = ImageDraw.Draw(_topdown)
_topdown.thumbnail((256, 256))
_rgb = _rgb.resize((int(256 / _rgb.size[1] * _rgb.size[0]), 256))
_combined = Image.fromarray(np.hstack((_rgb, _topdown)))
_draw = ImageDraw.Draw(_combined)
# change
control = self._get_control(data, _draw)
steer = control.steer
throttle = control.throttle
brake = control.brake
_draw.text((5, 10), 'FPS: %.3f' % (self.step / (time.time() - self.wall_start)))
_draw.text((5, 30), 'Steer: %.3f' % steer)
_draw.text((5, 50), 'Throttle: %.3f' % throttle)
_draw.text((5, 70), 'Brake: %s' % brake)
if HAS_DISPLAY:
cv2.imshow('map', cv2.cvtColor(np.array(_combined), cv2.COLOR_BGR2RGB))
cv2.waitKey(1)
if self.step % 2 == 0:
self.gather_info()
record_every_n_steps = 3
if self.step % record_every_n_steps == 0:
self.save('', steer, throttle, brake, self.target_speed, data)
return control
def save(self, far_command, steer, throttle, brake, target_speed, tick_data):
# frame = self.step // 10
frame = self.step
speed = tick_data['speed']
string = os.environ['SAVE_FOLDER'] + '/' + pathlib.Path(os.environ['ROUTES']).stem
center_str = string + '/' + 'rgb' + '/' + ('%04d.png' % frame)
left_str = string + '/' + 'rgb_left' + '/' + ('%04d.png' % frame)
right_str = string + '/' + 'rgb_right' + '/' + ('%04d.png' % frame)
# topdown_str = string + '/' + 'topdown' + '/' + ('%04d.png' % frame)
center = self.save_path / 'rgb' / ('%04d.png' % frame)
left = self.save_path / 'rgb_left' / ('%04d.png' % frame)
right = self.save_path / 'rgb_right' / ('%04d.png' % frame)
topdown = self.save_path / 'topdown' / ('%04d.png' % frame)
rgb_with_car = self.save_path / 'rgb_with_car' / ('%04d.png' % frame)
data_row = ','.join([str(i) for i in [frame, far_command, speed, steer, throttle, brake, center_str, left_str, right_str]])
with (self.save_path / 'measurements.csv').open("a") as f_out:
f_out.write(data_row+'\n')
Image.fromarray(tick_data['rgb']).save(center)
Image.fromarray(tick_data['rgb_left']).save(left)
Image.fromarray(tick_data['rgb_right']).save(right)
# modification
Image.fromarray(COLOR[CONVERTER[tick_data['topdown']]]).save(topdown)
Image.fromarray(tick_data['rgb_with_car']).save(rgb_with_car)
def _draw_line(self, p, v, z, color=(255, 0, 0)):
if not DEBUG:
return
p1 = _location(p[0], p[1], z)
p2 = _location(p[0]+v[0], p[1]+v[1], z)
color = carla.Color(*color)
self._world.debug.draw_line(p1, p2, 0.25, color, 0.01)
def _is_light_red(self, lights_list):
"""
Method to check if there is a red light affecting us. This version of
the method is compatible with both European and US style traffic lights.
:param lights_list: list containing TrafficLight objects
:return: a tuple given by (bool_flag, traffic_light), where
- bool_flag is True if there is a traffic light in RED
affecting us and False otherwise
- traffic_light is the object itself or None if there is no
red traffic light affecting us
"""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
for traffic_light in lights_list:
object_location = self._get_trafficlight_trigger_location(traffic_light)
object_waypoint = self._map.get_waypoint(object_location)
if object_waypoint.road_id != ego_vehicle_waypoint.road_id:
continue
ve_dir = ego_vehicle_waypoint.transform.get_forward_vector()
wp_dir = object_waypoint.transform.get_forward_vector()
dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z
if dot_ve_wp < 0:
continue
if is_within_distance_ahead(object_waypoint.transform,
self._vehicle.get_transform(),
self._proximity_tlight_threshold):
if traffic_light.state == carla.TrafficLightState.Red:
return (True, traffic_light)
return (False, None)
def _get_trafficlight_trigger_location(self, traffic_light): # pylint: disable=no-self-use
"""
Calculates the yaw of the waypoint that represents the trigger volume of the traffic light
"""
def rotate_point(point, radians):
"""
rotate a given point by a given angle
"""
rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y
rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y
return carla.Vector3D(rotated_x, rotated_y, point.z)
base_transform = traffic_light.get_transform()
base_rot = base_transform.rotation.yaw
area_loc = base_transform.transform(traffic_light.trigger_volume.location)
area_ext = traffic_light.trigger_volume.extent
point = rotate_point(carla.Vector3D(0, 0, area_ext.z), math.radians(base_rot))
point_location = area_loc + carla.Location(x=point.x, y=point.y)
return carla.Location(point_location.x, point_location.y, point_location.z)
def _bh_is_vehicle_hazard(self, ego_wpt, ego_loc, vehicle_list,
proximity_th, up_angle_th, low_angle_th=0, lane_offset=0):
"""
Check if a given vehicle is an obstacle in our way. To this end we take
into account the road and lane the target vehicle is on and run a
geometry test to check if the target vehicle is under a certain distance
in front of our ego vehicle. We also check the next waypoint, just to be
sure there's not a sudden road id change.
WARNING: This method is an approximation that could fail for very large
vehicles, which center is actually on a different lane but their
extension falls within the ego vehicle lane. Also, make sure to remove
the ego vehicle from the list. Lane offset is set to +1 for right lanes
and -1 for left lanes, but this has to be inverted if lane values are
negative.
:param ego_wpt: waypoint of ego-vehicle
:param ego_log: location of ego-vehicle
:param vehicle_list: list of potential obstacle to check
:param proximity_th: threshold for the agent to be alerted of
a possible collision
:param up_angle_th: upper threshold for angle
:param low_angle_th: lower threshold for angle
:param lane_offset: for right and left lane changes
:return: a tuple given by (bool_flag, vehicle, distance), where:
- bool_flag is True if there is a vehicle ahead blocking us
and False otherwise
- vehicle is the blocker object itself
- distance is the meters separating the two vehicles
"""
# Get the right offset
if ego_wpt.lane_id < 0 and lane_offset != 0:
lane_offset *= -1
for target_vehicle in vehicle_list:
target_vehicle_loc = target_vehicle.get_location()
# If the object is not in our next or current lane it's not an obstacle
target_wpt = self._map.get_waypoint(target_vehicle_loc)
if target_wpt.road_id != ego_wpt.road_id or \
target_wpt.lane_id != ego_wpt.lane_id + lane_offset:
next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=5)[0]
if target_wpt.road_id != next_wpt.road_id or \
target_wpt.lane_id != next_wpt.lane_id + lane_offset:
continue
if is_within_distance(target_vehicle_loc, ego_loc,
self._vehicle.get_transform().rotation.yaw,
proximity_th, up_angle_th, low_angle_th):
return (True, target_vehicle, compute_distance(target_vehicle_loc, ego_loc))
return (False, None, -1)
def _is_vehicle_hazard(self, vehicle_list):
"""
:param vehicle_list: list of potential obstacle to check
:return: a tuple given by (bool_flag, vehicle), where
- bool_flag is True if there is a vehicle ahead blocking us
and False otherwise
- vehicle is the blocker object itself
"""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
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())
if target_vehicle_waypoint.road_id != ego_vehicle_waypoint.road_id or \
target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
if is_within_distance_ahead(target_vehicle.get_transform(),
self._vehicle.get_transform(),
self._proximity_vehicle_threshold):
return (True, target_vehicle)
return (False, None)
@staticmethod
def emergency_stop():
"""
Send an emergency stop command to the vehicle
:return: control for braking
"""
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
return control
class NavigationAssistant:
def __init__(self, vehicle):
self._vehicle = vehicle
self._map = vehicle.get_world().get_map()
args_lateral_dict = {
'K_P': 1,
'K_D': 0.4,
'K_I': 0,
'dt': 1.0/20.0}
self._local_planner = LocalPlanner(
self._vehicle, opt_dict={
'target_speed' : 0,
'lateral_control_dict' :args_lateral_dict
}
)
self._hop_resolution = 2.0
self._grp = self._initialize_global_route_planner()
# Initializes the global route planner.
def _initialize_global_route_planner(self):
dao = GlobalRoutePlannerDAO(self._vehicle.get_world().get_map(), self._hop_resolution)
grp = GlobalRoutePlanner(dao)
grp.setup()
return grp
# Sets vehicle's speed.
def set_speed(self, target_speed):
self._local_planner.set_speed(target_speed)
# Sets vehicle's destination & computes the optimal route towards the destination.
def set_destination(self, location):
start_waypoint = self._map.get_waypoint( self._vehicle.get_location() )
end_waypoint = self._map.get_waypoint(location)
# Computes the optimal route for the starting location.
route_trace = self._grp.trace_route(start_waypoint.transform.location, end_waypoint.transform.location)
self._local_planner.set_global_plan(route_trace)
# Executes one step of navigation.
def run_step(self, hazard_detected, debug=False):
if hazard_detected:
control = self._emergency_stop()
else:
control = self._local_planner.run_step(debug=debug)
return control
# Checks whether the vehicle reached its destination.
def done(self):
return self._local_planner.done()
# Returns a control that forces the vehicle to stop.
@staticmethod
def _emergency_stop():
control = carla.VehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
return control
class RainDrivingAgent(Agent):
def __init__(self, vehicle, target_speed=20):
"""
:param vehicle: actor to apply to local planner logic onto
"""
super(RainDrivingAgent, self).__init__(vehicle)
self._proximity_tlight_threshold = 5.0 # meters
self._proximity_vehicle_threshold = 10.0 # meters
self._state = AgentState.NAVIGATING
args_lateral_dict = {'K_P': 1, 'K_D': 0.4, '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
# create the camera
camera_bp = self._world.get_blueprint_library().find(
'sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', str(1920 // 2))
camera_bp.set_attribute('image_size_y', str(1080 // 2))
camera_bp.set_attribute('fov', str(90))
camera_transform = carla.Transform(carla.Location(x=-5.5, z=2.8),
carla.Rotation(pitch=-15))
self._camera = self._world.spawn_actor(camera_bp,
camera_transform,
attach_to=self._vehicle)
self._camera.listen(lambda image: self._process_image(image))
self._curr_image = None
self._save_count = 0
def _process_image(self, image):
self._curr_image = image
"""
image_array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
image_array = np.reshape(image_array, (image.height, image.width, 4))
image_array = image_array[:, :, :3]
image_array = image_array[:, :, ::-1]
"""
file_name = 'curr.jpg'
image.save_to_disk(file_name)
def parse_file(filename):
image_string = tf.io.read_file(filename)
image_decoded = tf.image.decode_jpeg(image_string, channels=3)
return tf.cast(image_decoded, tf.float32) / 255.0
whole_path = [file_name]
filename_tensor = tf.convert_to_tensor(value=whole_path,
dtype=tf.string)
dataset = tf.data.Dataset.from_tensor_slices((filename_tensor))
dataset = dataset.map(parse_file)
dataset = dataset.prefetch(buffer_size=1)
dataset = dataset.batch(batch_size=1).repeat()
iterator = tf.compat.v1.data.make_one_shot_iterator(dataset)
image_array = iterator.get_next()
output = Network.inference(image_array,
is_training=False,
middle_layers=12)
output = tf.clip_by_value(output, 0., 1.)
output = output[0, :, :, :]
config = tf.compat.v1.ConfigProto()
config.gpu_options.allow_growth = True
saver = tf.compat.v1.train.Saver()
with tf.compat.v1.Session(config=config) as sess:
with tf.device('/gpu:0'):
saver.restore(sess, pre_trained_model_path)
derained, ori = sess.run([output, image_array])
derained = np.uint8(derained * 255.)
skimage.io.imsave('curr_derained.png', derained)
if self._save_count % 6 == 0:
image.save_to_disk('_out/%08d_orig' % image.frame)
skimage.io.imsave('_out/%08d_derained.png' % image.frame,
derained)
self._save_count += 1
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)
self._local_planner.set_global_plan(route_trace)
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=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(debug=debug)
return control
def done(self):
"""
Check whether the agent has reached its destination.
:return bool
"""
return self._local_planner.done()
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=50):
"""
: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
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 _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=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 = False, None #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(debug)
return control
class TestAgent(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=50):
"""
:param vehicle: actor to apply to local planner logic onto
"""
super(TestAgent, 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._world = self._vehicle.get_world()
self._map = self._vehicle.get_world().get_map()
def set_destination(self, start_waypoint, end_waypoint, time=False):
"""
This method creates a list of waypoints from agent's position to destination location
based on the route returned by the global router
"""
self.create_samples(start_waypoint, end_waypoint)
route_trace = self._trace_route(time=time)
assert route_trace
self._local_planner.set_global_plan(route_trace)
def create_samples(self,
start,
goal,
waypoint_dist=4,
disk_radius=4 * math.sqrt(2),
num_yaw=8):
print(
f'Creating samples {waypoint_dist}m apart with {num_yaw} yaw vaules and neighbors within {disk_radius}m.'
)
wp = []
for mp in self._map.generate_waypoints(waypoint_dist):
wp.append(mp.transform)
wp.append(goal)
wp.append(start)
states = []
neighbors = []
num_neighbors = []
# for each waypoint wp
for i, wi in enumerate(wp):
li = wi.location
ni = []
num = 0
# find other waypoints within disk radius
for j, wj in enumerate(wp):
lj = wj.location
if li == lj:
continue
elif li.distance(lj) <= disk_radius:
# account for index shifts with adding in orientation
for k in range(num_yaw):
if k == (num_yaw) / 2:
continue
elif k > (num_yaw) / 2:
ni.append(j * (num_yaw - 1) + k - 1)
else:
ni.append(j * (num_yaw - 1) + k)
num += 1
if wj == start or wj == goal:
break
# add in number of yaw orientations to waypoint list
ri = wi.rotation
for k in range(num_yaw):
if k == (num_yaw) / 2:
continue
num_neighbors.append(num)
neighbors += ni
theta = ri.yaw + k * 360 / (num_yaw)
if theta >= 180:
theta = theta - 360
elif theta <= -180:
theta = 360 - theta
states.append([li.x, li.y, theta * np.pi / 180])
if wi == start or wi == goal:
break
self.states = np.array(states).astype(np.float32)
self.neighbors = np.array(neighbors).astype(np.int32)
self.num_neighbors = np.array(num_neighbors).astype(np.int32)
init_parameters = {
'states': self.states,
'neighbors': self.neighbors,
'num_neighbors': self.num_neighbors,
'threadsPerBlock': 128
}
self.start = self.states.shape[0] - 1
self.goal = self.states.shape[0] - 2
print(
f'start: {self.start} goal: {self.goal} total states: {self.states.shape[0]}'
)
print(
f'start location: {self.states[self.start]}, goal location: {self.states[self.goal]}'
)
self.gmt_planner = GMT(init_parameters, debug=False)
# self.gmt_planner = GMTmem(init_parameters, debug=False)
# self.gmt_planner = GMTstream(init_parameters, debug=False)
# self.gmt_planner = GMTmemStream(init_parameters, debug=False)
@staticmethod
def _create_bb_points(vehicle):
cords = np.zeros((3, 4))
extent = vehicle.bounding_box.extent
cords[0, :] = np.array([extent.x + 2.2, extent.y + 2.2, extent.z, 1])
cords[1, :] = np.array([-extent.x - 2.2, -extent.y - 2.2, extent.z, 1])
cords[2, :] = np.array([0, 0, 0, 1]) # center
return cords
@staticmethod
def _vehicle_to_world(cords, vehicle):
bb_transform = carla.Transform(vehicle.bounding_box.location)
bb_vehicle_matrix = TestAgent.get_matrix(bb_transform)
vehicle_world_matrix = TestAgent.get_matrix(vehicle.get_transform())
bb_world_matrix = np.dot(vehicle_world_matrix, bb_vehicle_matrix)
world_cords = np.dot(bb_world_matrix, np.transpose(cords))
return world_cords
@staticmethod
def get_matrix(transform):
rotation = transform.rotation
location = transform.location
c_y = np.cos(np.radians(rotation.yaw))
s_y = np.sin(np.radians(rotation.yaw))
c_r = np.cos(np.radians(rotation.roll))
s_r = np.sin(np.radians(rotation.roll))
c_p = np.cos(np.radians(rotation.pitch))
s_p = np.sin(np.radians(rotation.pitch))
matrix = np.matrix(np.identity(4))
matrix[0, 3] = location.x
matrix[1, 3] = location.y
matrix[2, 3] = location.z
matrix[0, 0] = c_p * c_y
matrix[0, 1] = c_y * s_p * s_r - s_y * c_r
matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r
matrix[1, 0] = s_y * c_p
matrix[1, 1] = s_y * s_p * s_r + c_y * c_r
matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r
matrix[2, 0] = s_p
matrix[2, 1] = -c_p * s_r
matrix[2, 2] = c_p * c_r
return matrix
def _trace_route(self, debug=False, time=False):
"""
This method sets up a global router and returns the optimal route
from start_waypoint to end_waypoint
"""
self.radius = 2
self.threshold = 1
obstacles = []
for vehicle in self._world.get_actors().filter('vehicle.*'):
#print(vehicle.bounding_box)
# draw Box
bb_points = TestAgent._create_bb_points(vehicle)
global_points = TestAgent._vehicle_to_world(bb_points, vehicle)
global_points /= global_points[3, :]
my_bb_points = TestAgent._create_bb_points(self._vehicle)
my_global_points = TestAgent._vehicle_to_world(
my_bb_points, self._vehicle)
my_global_points /= my_global_points[3, :]
dist = np.sqrt((my_global_points[0, 2] - global_points[0, 2])**2 +
(my_global_points[1, 2] - global_points[1, 2])**2 +
(my_global_points[2, 2] - global_points[2, 2])**2)
if 0 < dist:
vehicle_box = [
global_points[0, 0], global_points[1, 0],
global_points[0, 1], global_points[1, 1]
]
obstacles.append(vehicle_box)
print(f'vehicle box: {vehicle_box}')
print('number of near obstacles: ', len(obstacles))
if len(obstacles) == 0:
self.obstacles = np.array([[-1, -1, -1, -1]]).astype(np.float32)
self.num_obs = self.num_obs = np.array([0]).astype(np.int32)
else:
self.obstacles = np.array(obstacles).astype(np.float32)
self.num_obs = self.num_obs = np.array([self.obstacles.shape[0]
]).astype(np.int32)
iter_parameters = {
'start': self.start,
'goal': self.goal,
'radius': self.radius,
'threshold': self.threshold,
'obstacles': self.obstacles,
'num_obs': self.num_obs
}
start_timer = timer()
route = self.gmt_planner.run_step(iter_parameters,
iter_limit=1000,
debug=debug,
time=time)
end_timer = timer()
print("elapsed time: ", end_timer - start_timer)
if time:
self.time_df = pd.DataFrame(self.gmt_planner.time_data)
# trace_route = []
# for r in route:
# wp = carla.Transform(carla.Location(self.states[r][0].item(), self.states[r][1].item(), 1.2), carla.Rotation(roll=0,pitch=0, yaw=(self.states[r][2]*180/np.pi).item()))
# trace_route.append(wp)
# draw_route(self._vehicle.get_world(), trace_route)
index = len(route) - 1
trace_route = []
for i in range(len(route) - 1):
wp = self._map.get_waypoint(
carla.Location(self.states[route[index]][0].item(),
self.states[route[index]][1].item(), 1.2)
) # , carla.Rotation(roll=0,pitch=0, yaw=(self.states[r][2]*180/np.pi).item()
trace_route.append((wp, -1))
index -= 1
return trace_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 = False, None #
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(debug)
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):
"""
: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)
# setting up global router
self._current_plan = None
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]))
current_waypoint = start_waypoint
active_list = [ [(current_waypoint, RoadOption.LANEFOLLOW)] ]
solution = []
while not solution:
for _ in range(len(active_list)):
trajectory = active_list.pop()
if len(trajectory) > 1000:
continue
# expand this trajectory
current_waypoint, _ = trajectory[-1]
next_waypoints = current_waypoint.next(5.0)
while len(next_waypoints) == 1:
next_option = compute_connection(current_waypoint, next_waypoints[0])
current_distance = next_waypoints[0].transform.location.distance(end_waypoint.transform.location)
if current_distance < 5.0:
solution = trajectory + [(end_waypoint, RoadOption.LANEFOLLOW)]
break
# keep adding nodes
trajectory.append((next_waypoints[0], next_option))
current_waypoint, _ = trajectory[-1]
next_waypoints = current_waypoint.next(5.0)
if not solution:
# multiple choices
for waypoint in next_waypoints:
next_option = compute_connection(current_waypoint, waypoint)
active_list.append(trajectory + [(waypoint, next_option)])
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 AutonomousAgent(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, world):
"""
:param vehicle: actor to apply to local planner logic onto
"""
super(AutonomousAgent, self).__init__(world.player)
self._world_obj = world
self._THW = 2
self._target_speed = None
# Local plannar
self._local_planner = LocalPlanner(world.player)
self.update_parameters()
# Global plannar
self._proximity_threshold = 10.0 # meter # Distance between waypoints
self._state = AgentState.NAVIGATING
self._hop_resolution = 0.2
self._path_seperation_hop = 2
self._path_seperation_threshold = 0.5
self._grp = None # global route planar
# Behavior planning
self._hazard_detected = False
self._blocked_time = None
self._perform_lane_change = False
self._front_r = []
self._left_front_r = []
self._left_back_r = []
# Turns positions
self.right_positions = None
self.left_positions = None
# Turn flags
self.right_turn = False
self.left_turn = False
self.temp_flag = True
self.left_positions = None
def update_parameters(self):
self._THW = 2
self._target_speed = 30
CONTROLLER_TYPE = 'PID' # options:MPC, PID, STANLEY
args_lateral_dict = {
'K_P': 1.0,
'K_I': 0.4,
'K_D': 0.01,
'control_type': CONTROLLER_TYPE
}
args_longitudinal_dict = {'K_P': 0.3, 'K_I': 0.2, 'K_D': 0.002}
self._local_planner.init_controller(
opt_dict={
'target_speed': self._target_speed,
'lateral_control_dict': args_lateral_dict,
'longitudinal_control_dict': args_longitudinal_dict
})
# Set global destination and get global waypoints
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)
# Get global waypoints
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)
self.turn_positions_getter(route, RoadOption.RIGHT)
self.turn_positions_getter(route, RoadOption.LEFT)
return route
def turn_positions_getter(self, route, state):
"""
Returns list of all Left and right turns waypoints
"""
count_flag = False
temp_list = []
list_of_turn_waypoints = []
for i, j in route:
if j == state:
count_flag = True
temp_list.append(i)
continue
if count_flag:
start_waypoint = temp_list[0]
end_waypoint = temp_list[-1]
list_of_turn_waypoints.append((start_waypoint, end_waypoint))
temp_list = []
count_flag = False
if state == RoadOption.RIGHT:
self.right_positions = list_of_turn_waypoints
else:
self.left_positions = list_of_turn_waypoints
# Get vehicle speed
def _get_speed(self):
v = self._vehicle.get_velocity()
ego_speed = math.sqrt(v.x**2 + v.y**2 + v.z**2)
return ego_speed
# Run step
def run_step(self, debug=False):
"""
Execute one step of navigation.
:return: carla.VehicleControl
"""
## Update Environment ##
# Check all the radars
try:
if self._state == AgentState.EMERGENCY_BRAKE:
pass
pass
except:
pass
if self._world_obj.front_radar.detected:
if abs(self._world_obj.front_radar.rel_pos[1]) < 1:
self._front_r = [
pygame.time.get_ticks(),
self._world_obj.front_radar.rel_pos,
self._world_obj.front_radar.rel_vel
]
self._world_obj.front_radar.detected = False
if self._world_obj.left_front_radar.detected:
if self._world_obj.left_front_radar.rel_pos[1] < -1:
self._left_front_r = [
pygame.time.get_ticks(),
self._world_obj.left_front_radar.rel_pos,
self._world_obj.left_front_radar.rel_vel
]
self._world_obj.left_front_radar.detected = False
if self._world_obj.left_back_radar.detected:
if self._world_obj.left_back_radar.rel_pos[1] < -1:
self._left_back_r = [
pygame.time.get_ticks(),
self._world_obj.left_back_radar.rel_pos,
self._world_obj.left_back_radar.rel_vel
]
self._world_obj.left_back_radar.detected = False
# Remove radar data if not detected again in 0.5 second
if self._front_r and (pygame.time.get_ticks() - self._front_r[0] >
5000):
self._front_r = []
if self._left_front_r and (
pygame.time.get_ticks() - self._left_front_r[0] > 5000):
self._left_front_r = []
if self._left_back_r and (
pygame.time.get_ticks() - self._left_back_r[0] > 5000):
self._left_back_r = []
# Detect vehicles in front
self._hazard_detected = False
if self._front_r and (self._front_r[1][0] < 20.0):
self._hazard_detected = True
# update hazard existing time
if self._hazard_detected:
if self._blocked_time is None:
self._blocked_time = pygame.time.get_ticks()
hazard_time = 0
else:
hazard_time = pygame.time.get_ticks() - self._blocked_time
else:
self._blocked_time = None
# Get a safe_distance
safe_distance = self._THW * self._get_speed()
try:
i = self.right_positions[0][0]
j = self.right_positions[0][1]
loc_start = i.transform.location
loc_start_yaw = i.transform.rotation.yaw
loc = loc_start
loc_end_yaw = j.transform.rotation.yaw
loc_end = j.transform.location
if (abs(loc.x-self._vehicle.get_location().x)+\
abs(loc.y-self._vehicle.get_location().y)+\
abs(loc.z-self._vehicle.get_location().z))<=10:
self.right_turn = True
self.temp_flag = False
except:
pass
try:
i = self.left_positions[0][0]
j = self.left_positions[0][1]
loc2_start = i.transform.location
loc2_start_yaw = i.transform.rotation.yaw
loc2 = loc2_start
loc2_end = j.transform.location
loc2_end_yaw = j.transform.rotation.yaw
if (abs(loc2.x-self._vehicle.get_location().x)+\
abs(loc2.y-self._vehicle.get_location().y)+\
abs(loc2.z-self._vehicle.get_location().z))<=10:
self.left_turn = True
self.temp_flag = False
except:
pass
# Finite State Machine
# 1, Navigating
if self._state == AgentState.NAVIGATING:
if self._hazard_detected:
self._state = AgentState.BLOCKED_BY_VEHICLE
# 2, Blocked by Vehicle
elif self._state == AgentState.BLOCKED_BY_VEHICLE:
if not self._hazard_detected:
self._state = AgentState.NAVIGATING
# The vehicle is driving at a certain speed
# There is enough space
else:
if hazard_time > 5000 and \
190 > self._vehicle.get_location().x > 10 and \
10 > self._vehicle.get_location().y > 7:
self._state = AgentState.PREPARE_LANE_CHANGING
# 4, Prepare Lane Change
elif self._state == AgentState.PREPARE_LANE_CHANGING:
if not (self._front_r and self._front_r[1][0] < safe_distance) and \
not (self._left_front_r and self._left_front_r[1][0] < safe_distance) and \
not (self._left_back_r and self._left_back_r[1][0] > -10):
self._state = AgentState.LANE_CHANGING
self._perform_lane_change = True
# 5, Lane Change
elif self._state == AgentState.LANE_CHANGING:
if abs(self._vehicle.get_velocity().y) < 0.5 and \
self._vehicle.get_location().y < 7.0:
self._state = AgentState.NAVIGATING
# 6, Emergency Brake
emergency_distance = safe_distance * 3 / 5
emergency_front_speed = 1.0
if self._front_r and (self._front_r[1][0] < emergency_distance
or self._front_r[2][0] < emergency_front_speed):
self._state = AgentState.EMERGENCY_BRAKE
# Local Planner Behavior according to states
if self._state == AgentState.NAVIGATING or self._state == AgentState.LANE_CHANGING:
control = self._local_planner.run_step(debug=debug)
elif self._state == AgentState.PREPARE_LANE_CHANGING:
if self._left_front_r and self._left_front_r[1][0] < safe_distance or \
self._front_r and self._front_r[1][0] < safe_distance:
control = self._local_planner.empty_control(debug=debug)
else:
control = self._local_planner.run_step(debug=debug)
elif self._state == AgentState.BLOCKED_BY_VEHICLE:
# ACC
front_dis = self._front_r[1][0]
front_vel = self._front_r[2][0]
ego_speed = self._get_speed()
desired_speed = front_vel - (ego_speed - front_vel) / front_dis
if ego_speed > 1:
desired_speed += 2 * (front_dis / ego_speed - self._THW)
control = self._local_planner.run_step(debug=debug,
target_speed=desired_speed *
3.6)
elif self._state == AgentState.EMERGENCY_BRAKE:
control = self._local_planner.brake()
if self._front_r:
if self._front_r[1][0] >= emergency_distance and \
self._front_r[2][0] > emergency_front_speed:
self._state = AgentState.NAVIGATING
elif self._state == AgentState.BLOCKED_RED_LIGHT:
control = self._local_planner.empty_control(debug=debug)
# When performing a lane change
if self._perform_lane_change:
# Record original destination
destination = self._local_planner.get_global_destination()
# Get lane change start location
ref_location = self._world_obj.player.get_location()
ref_yaw = self._world_obj.player.get_transform().rotation.yaw
if self._local_planner.waypoint_buffer:
waypoint = self._local_planner.waypoint_buffer[-1][0]
ref_location = waypoint.transform.location
wait_dist = 0.0 # need some time to plan
ref = [ref_location.x + wait_dist, ref_location.y, ref_yaw]
# Replace current plan with a lane change plan
overtake = BezierOverTake(self._world_obj)
overtake_plan = overtake.get_waypoints(ref)
self._local_planner.set_local_plan(overtake_plan)
# replan globally with new vehicle position after lane changing
new_start = self._map.get_waypoint(
overtake_plan[-1][0].transform.location)
route_trace = self._trace_route(new_start, destination)
assert route_trace
self._local_planner.add_global_plan(route_trace)
self._perform_lane_change = False
print("overtake")
if self.right_turn or self.left_turn:
# Record original destination
destination = self._local_planner.get_global_destination()
# Get lane change start location
ref_location = self._world_obj.player.get_location()
ref_yaw = self._world_obj.player.get_transform().rotation.yaw
if self._local_planner.waypoint_buffer:
waypoint = self._local_planner.waypoint_buffer[-1][0]
ref_location = waypoint.transform.location
if self.right_turn:
ref1 = [loc_start.x, loc_start.y, loc_start_yaw]
ref2 = [loc_end.x, loc_end.y, loc_end_yaw]
turner = BezierTurn(self._world_obj, True)
turn_plan = turner.get_waypoints(ref1, ref2)
self.right_turn = False
print('Right Turn')
elif self.left_turn:
ref1 = [loc2_start.x, loc2_start.y, loc2_start_yaw]
ref2 = [loc2_end.x, loc2_end.y, loc2_end_yaw]
turner = BezierTurn(self._world_obj, False)
turn_plan = turner.get_waypoints(ref1, ref2)
self.left_turn = False
print('Left turn')
self._local_planner.set_local_plan(turn_plan)
# replan globally with new vehicle position after lane changing
new_start = self._map.get_waypoint(
turn_plan[-1][0].transform.location)
route_trace = self._trace_route(new_start, destination)
assert route_trace
self._local_planner.add_global_plan(route_trace)
return control
def done(self):
"""
Check whether the agent has reached its destination.
:return bool
"""
return self._local_planner.done()