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_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_orientation = self._vehicle.get_transform().rotation.yaw
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
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 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