def is_collision(self, entity_list):
ego_vehicle_location = self.ego_vehicle.get_transform().location
ego_vehicle_waypoint = self.world.get_map().get_waypoint(
ego_vehicle_location)
for target in entity_list:
# if the object is not in our lane it's not an obstacle
target_waypoint = self.world.get_map().get_waypoint(
target.get_location(),
project_to_road=True,
lane_type=(carla.LaneType.Driving | carla.LaneType.Sidewalk))
# if target_waypoint.road_id == ego_vehicle_waypoint.road_id and \
# target_waypoint.lane_id == ego_vehicle_waypoint.lane_id:
#target_waypoint.lane_type == ego_vehicle_waypoint.lane_type:
if target_waypoint.lane_type == carla.LaneType.Driving and target_waypoint.lane_id == ego_vehicle_waypoint.lane_id:
if is_within_distance_ahead(target.get_transform(),
self.ego_vehicle.get_transform(),
4.0):
#if self.distance(self.ego_vehicle.get_transform(), target.get_transform()) < 10.0:
return (True, True, target)
# if target_waypoint.road_id == ego_vehicle_waypoint.road_id and \
# target_waypoint.lane_type == ego_vehicle_waypoint.lane_type:
elif is_within_distance_ahead(target.get_transform(),
self.ego_vehicle.get_transform(),
8.0):
return (False, True, target)
return (False, False, None)
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)
# print ("Checking for collision")
return (False, None)
def _is_light_red_europe_style(self, lights_list):
"""
This method is specialized to check European style traffic lights.
Only suitable for Towns 03 -- 07.
"""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
traffic_light_color = "NONE" # default, if no traffic lights are seen
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
if is_within_distance_ahead(traffic_light.get_transform(),
self._vehicle.get_transform(),
self._proximity_threshold):
if traffic_light.state == carla.TrafficLightState.Red:
return "RED"
elif traffic_light.state == carla.TrafficLightState.Yellow:
traffic_light_color = "YELLOW"
elif traffic_light.state == carla.TrafficLightState.Green:
if traffic_light_color is not "YELLOW": # (more severe)
traffic_light_color = "GREEN"
else:
import pdb
pdb.set_trace()
# investigate https://carla.readthedocs.io/en/latest/python_api/#carlatrafficlightstate
return traffic_light_color
def _is_light_red_europe_style(self, lights_list):
"""
This method is specialized to check European 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_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_threshold):
if traffic_light.state == carla.TrafficLightState.Red:
return (True, traffic_light)
return (False, None)
def _is_light_red_europe_style(self, lights_list):
"""
This method is specialized to check European 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_waypoint = self._map.get_waypoint(
traffic_light.get_location())
waypoint_list = ego_vehicle_waypoint.next(10)
distance = False
for waypoint in waypoint_list:
if (waypoint.road_id == object_waypoint.road_id):
norm_target = target_magnitude(
(waypoint.transform.location.x,
waypoint.transform.location.y),
(traffic_light.get_transform().location.x,
traffic_light.get_transform().location.y))
distance = is_within_distance_ahead(
traffic_light.get_transform(), waypoint.transform, 5)
#print(f'norm_target =========== {norm_target}')
if distance and traffic_light.state == carla.TrafficLightState.Red:
return (True, traffic_light)
return (False, None)
def detect_stationary_vehicle_ahead(self):
# Retrieving location of the agent.
vehicle_location = self._vehicle.get_location()
vehicle_waypoint = self._world_map.get_waypoint(vehicle_location)
for npc_vehicle in self._npc_vehicle_list:
# Checking if the npc vehicle is stationary.
npc_vehicle_control = npc_vehicle.get_control()
if npc_vehicle_control.throttle == 0.0:
# Retrieving the location of the candidate front vehicle.
npc_vehicle_location = npc_vehicle.get_location()
npc_vehicle_waypoint = self._world_map.get_waypoint(
npc_vehicle_location)
# Checking if the npc vehicle is located on the lane with the agent's vehicle.
if npc_vehicle_waypoint.road_id == vehicle_waypoint.road_id and \
npc_vehicle_waypoint.lane_id == vehicle_waypoint.lane_id:
# Checking if the npc vehicle is close to the agent's vehicle.
if is_within_distance_ahead(
npc_vehicle.get_transform(),
self._vehicle.get_transform(),
self._proximity_vehicle_threshold):
return True
return False
def _is_light_red(self, vehicle):
"""
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 = vehicle.get_location()
ego_vehicle_waypoint = self.map.get_waypoint(ego_vehicle_location)
for traffic_light in self.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,
vehicle.get_transform(),
self._traffic_light_threshold):
if traffic_light.state == carla.TrafficLightState.Red:
return (True, -0.1, traffic_light)
return (False, 0.0, None)
def _is_vehicle_hazard(self, vehicle_list):
"""
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.
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.
: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_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
loc = target_vehicle.get_location()
ori = target_vehicle.get_transform().rotation.yaw
target_vehicle_waypoint = self._map.get_waypoint(
target_vehicle.get_location())
# waiting = ego_vehicle_waypoint.is_intersection and target_vehicle.get_traffic_light_state() == carla.TrafficLightState.Red
# print ("Not our lane: ", other_lane)
# print ("Waiting", waiting)
# if the object is not in our lane it's not an obstacle
# if not ego_vehicle_waypoint.is_intersection and target_vehicle_waypoint.lane_id != ego_vehicle_waypoint.lane_id
# continue
# if the object is waiting for it's not an obstacle
# if waiting:
# continue
if compute_yaw_difference(
ego_vehicle_orientation,
ori) <= 150 and is_within_distance_ahead(
loc,
ego_vehicle_location,
self._vehicle.get_transform().rotation.yaw,
self._proximity_threshold,
degree=45):
return (True, target_vehicle)
return (False, None)
def _is_static_obstacle_ahead(self, prop_list):
"""
Check if a static obstacle is in the ego vehicles way.
"""
ego_vehicle_location = self._vehicle.get_location()
ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location)
for static_obstacle in prop_list:
static_obstacle_waypoint = self._map.get_waypoint(
static_obstacle.get_location())
if static_obstacle_waypoint.road_id != ego_vehicle_waypoint.road_id or \
static_obstacle_waypoint.lane_id != ego_vehicle_waypoint.lane_id:
continue
if is_within_distance_ahead(static_obstacle.get_transform(),
self._vehicle.get_transform(), 15):
return (True, static_obstacle)
return (False, None)
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_vehicle_hazard(self, vehicle_list):
"""
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.
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.
: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
loc = target_vehicle.get_location()
if is_within_distance_ahead(
loc, ego_vehicle_location,
self._vehicle.get_transform().rotation.yaw,
self._proximity_threshold):
return (True, target_vehicle)
return (False, None)
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 run_single(env,
weather,
start,
target,
agent_maker,
seed,
autopilot,
show=False,
model_path=None,
suite_name=None):
# addition from agent.py
from skimage.io import imread
_road_map = imread('PythonAPI/agents/navigation/%s.png' % env._map.name)
WORLD_OFFSETS = {
'Town01': (-52.059906005859375, -52.04995942115784),
'Town02': (-57.459808349609375, 55.3907470703125)
}
PIXELS_PER_METER = 5
def _world_to_pixel(vehicle, location, offset=(0, 0)):
world_offset = WORLD_OFFSETS[env._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(vehicle, loc):
# Convert to pixel coordinate
pixel_x, pixel_y = _world_to_pixel(vehicle, loc)
pixel_y = np.clip(pixel_y, 0, _road_map.shape[0] - 1)
pixel_x = np.clip(pixel_x, 0, _road_map.shape[1] - 1)
point = _road_map[pixel_y, pixel_x, 0]
return point == 0
# ------------------------------------------------
# HACK: deterministic vehicle spawns.
env.seed = seed
# modifications
env.init(start=start, target=target, weather=cu.PRESET_WEATHERS[weather])
if not autopilot:
agent = agent_maker()
else:
agent = agent_maker(env._player, resolution=1, threshold=7.5)
agent.set_route(env._start_pose.location, env._target_pose.location)
diagnostics = list()
result = {
'weather': weather,
'start': start,
'target': target,
'success': None,
't': None,
'total_lights_ran': None,
'total_lights': None,
'collided': None,
}
# modifications
all_data_folder = 'collected_data'
if not os.path.exists(all_data_folder):
os.mkdir(all_data_folder)
data_folder = all_data_folder + '/' + suite_name
if not os.path.exists(data_folder):
os.mkdir(data_folder)
trial_folder = data_folder + '/' + '0'
counter = 0
while os.path.exists(trial_folder):
counter += 1
trial_folder = data_folder + '/' + str(counter)
os.mkdir(trial_folder)
image_folder = trial_folder + '/' + 'images'
if not os.path.exists(image_folder):
os.mkdir(image_folder)
all_misbehavior_logfile_path = data_folder + '/' + 'all_misbehavior_driving_log.csv'
logfile_path = trial_folder + '/' + 'driving_log.csv'
misbehavior_logfile_path = trial_folder + '/' + 'misbehavior_driving_log.csv'
title = ','.join([
'FrameId', 'center', 'steering', 'throttle', 'brake', 'speed',
'command', 'Self Driving Model', 'Suit Name', 'Weather', 'Crashed',
'Crashed Type', 'Tot Crashes', 'Tot Lights Ran', 'Tot Lights',
'dist_ped', 'dist_vehicle', 'offroad', 'road_type', 'status'
])
with open(logfile_path, 'a+') as f_out:
f_out.write(title + '\n')
with open(misbehavior_logfile_path, 'a+') as f_out:
f_out.write(title + ',' + 'problem_type' + '\n')
with open(all_misbehavior_logfile_path, 'a+') as f_out:
f_out.write('counter' + ',' + title + ',' + 'problem_type' + '\n')
total_lights = 0
total_lights_ran = 0
collided = False
total_crashes = 0
frame_id = 0
LAST_ROUND = False
while env.tick() and not LAST_ROUND:
observations = env.get_observations()
control = agent.run_step(observations)
diagnostic = env.apply_control(control)
_paint(observations, control, diagnostic, agent.debug, env, show=show)
diagnostic.pop('viz_img')
diagnostics.append(diagnostic)
# modification
status = 'progress'
if env.is_failure() or env.is_success():
result['success'] = env.is_success()
result['total_lights_ran'] = env.traffic_tracker.total_lights_ran
result['total_lights'] = env.traffic_tracker.total_lights
result['collided'] = env.collided
result['t'] = env._tick
LAST_ROUND = True
if env.is_failure():
status = 'failure'
elif env.is_success():
status = 'success'
# additions
from agents.tools.misc import is_within_distance_ahead, compute_yaw_difference
total_lights = env.traffic_tracker.total_lights
total_lights_ran = env.traffic_tracker.total_lights_ran
collided = env.collided
_proximity_threshold_vehicle = 5.5 # 9.5 for autopilot to avoid crash
_proximity_threshold_ped = 2.5 # 9.5 for autopilot to avoid crash
actor_list = env._world.get_actors()
vehicle_list = actor_list.filter('*vehicle*')
# lights_list = actor_list.filter('*traffic_light*')
walkers_list = actor_list.filter('*walker*')
ego_vehicle_location = env._player.get_location()
ego_vehicle_orientation = env._player.get_transform().rotation.yaw
ego_vehicle_waypoint = env._map.get_waypoint(ego_vehicle_location,
project_to_road=False,
lane_type=LaneType.Any)
offroad = False
if not ego_vehicle_waypoint:
print('-' * 100, 'no lane', '-' * 100)
offroad = True
elif ego_vehicle_waypoint.lane_type != LaneType.Driving:
print('-' * 100, ego_vehicle_waypoint.lane_type, '-' * 100)
offroad = True
dist_ped = 10000
dist_vehicle = 10000
# crash_type
crash_type = 'none'
if collided:
crash_type = 'other'
total_crashes += 1
for walker in walkers_list:
loc = walker.get_location()
cur_dist_ped = loc.distance(ego_vehicle_location)
degree = 162 / (np.clip(dist_ped, 1.5, 10.5) + 0.3)
if _is_point_on_sidewalk(env._player, loc):
continue
if is_within_distance_ahead(
loc,
ego_vehicle_location,
env._player.get_transform().rotation.yaw,
_proximity_threshold_vehicle,
degree=cur_dist_ped):
crash_type = 'pedestrian'
if dist_ped > cur_dist_ped:
dist_ped = cur_dist_ped
for target_vehicle in vehicle_list:
# do not account for the ego vehicle
if target_vehicle.id == env._player.id:
continue
loc = target_vehicle.get_location()
ori = target_vehicle.get_transform().rotation.yaw
target_vehicle_waypoint = env._map.get_waypoint(
target_vehicle.get_location())
if compute_yaw_difference(
ego_vehicle_orientation,
ori) <= 150 and is_within_distance_ahead(
loc,
ego_vehicle_location,
env._player.get_transform().rotation.yaw,
_proximity_threshold_vehicle,
degree=45):
crash_type = 'vehicle'
cur_dist_vehicle = np.linalg.norm(
np.array([
loc.x - ego_vehicle_location.x,
loc.y - ego_vehicle_location.y
]))
if dist_vehicle > cur_dist_vehicle:
dist_vehicle = cur_dist_vehicle
prev_total_lights_ran = 0
with open(logfile_path, 'a+') as f_out:
dt = str(datetime.datetime.now())
m = re.search("(\d+)-(\d+)-(\d+) (\d+):(\d+):(\d+).\d+", dt)
if m:
time_info = '_'.join(m.groups())
else:
time_info = ''
center_address = image_folder + '/' + 'center_' + str(
frame_id) + '_' + time_info + '.jpg'
scipy.misc.toimage(observations['rgb'], cmin=0.0,
cmax=...).save(center_address)
log_text = ','.join([
str(frame_id), center_address,
str(control.steer),
str(control.throttle),
str(control.brake),
str(diagnostic['speed']),
str(observations['command']),
str(model_path), suite_name,
str(weather),
str(collided), crash_type,
str(total_crashes),
str(total_lights_ran),
str(total_lights),
str(dist_ped),
str(dist_vehicle),
str(offroad),
str(ego_vehicle_waypoint.lane_type), status
])
f_out.write(log_text + '\n')
if collided or offroad or total_lights_ran > prev_total_lights_ran:
if collided:
problem_type = 'collision'
elif offroad:
problem_type = 'offroad'
elif total_lights_ran > prev_total_lights_ran:
problem_type = 'light_ran'
with open(misbehavior_logfile_path, 'a+') as f_out:
f_out.write(log_text + ',' + problem_type + '\n')
with open(all_misbehavior_logfile_path, 'a+') as f_out:
f_out.write(
str(counter) + ',' + log_text + ',' + problem_type + '\n')
frame_id += 1
prev_total_lights_ran = total_lights_ran
if env.is_failure():
print('+' * 100, 'collision:', str(result['collided']), '+' * 100)
# -------------------------------------------------------------
return result, diagnostics
