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