def is_one_turn_ahead(point_a, point_b, distance): """ Check if the ROUTE distance between two points is bigger than "distance" :param point_a: :param point_b: :param distance: :return: """ _, route_initial = interpolate_trajectory( world, [point_a.location, point_b.location]) if estimate_route_distance(route_initial) < distance or \ estimate_route_distance(route_initial) > 3*distance: print("Rejected because it is too small") return False route = clean_route(route_initial) print(" One curve test ") if len(route) != 1: print(" reject because of size") return False for point in route: # Check if there are any curve if point[2] == RoadOption.STRAIGHT: print(" reject due to straight") return False return True
def is_distance_ahead(point_a, point_b, distance): """ Check if the ROUTE distance between two points is bigger than "distance" :param point_a: :param point_b: :param distance: :return: """ global world _, route = interpolate_trajectory(world, [point_a.location, point_b.location]) return distance < estimate_route_distance(route)
def test_route_parser(self): args = Arguments() client = carla.Client(args.host, int(args.port)) client.set_timeout(25.0) challenge = ChallengeEvaluator(args) filename = os.path.join(self.root_route_file_position, 'all_towns_traffic_scenarios.json') world_annotations = parser.parse_annotations_file(filename) # retrieve routes # Which type of file is expected ???? filename = os.path.join(self.root_route_file_position, 'routes_training.xml') list_route_descriptions = parser.parse_routes_file(filename) # For each of the routes to be evaluated. for route_description in list_route_descriptions: if route_description['town_name'] == 'Town03' or route_description[ 'town_name'] == 'Town01': continue print(" TOWN: ", route_description['town_name']) challenge.world = client.load_world(route_description['town_name']) # Set the actor pool so the scenarios can prepare themselves when needed CarlaActorPool.set_world(challenge.world) CarlaDataProvider.set_world(challenge.world) # find and filter potential scenarios # Returns the interpolation in a different format challenge.world.wait_for_tick() gps_route, route_description[ 'trajectory'] = interpolate_trajectory( challenge.world, route_description['trajectory']) potential_scenarios_definitions, existent_triggers = parser.scan_route_for_scenarios( route_description, world_annotations) for trigger_id, possible_scenarios in potential_scenarios_definitions.items( ): print(" For trigger ", trigger_id, " -- ", possible_scenarios[0]['trigger_position']) for scenario in possible_scenarios: print(" ", scenario['name']) challenge.cleanup(ego=True)
def draw_route(route_pair, count): initial_point, end_point = route_pair[0], route_pair[1] _, trajectory = interpolate_trajectory( world, [initial_point.location, end_point.location]) prev_point = trajectory[0] print(" Draw line for trajectoryyy of len ", len(trajectory)) for point in trajectory[1:]: draw_line(prev_point[0].location, point[0].location, color_palet[count], 4, alpha=0.6) prev_point = point draw_text(str(count), end_point.location, (1, 0, 0), 5)
def get_scenario_3(world, route, number_scenario3=1): _, route_interpolated = interpolate_trajectory(world, route['trajectory']) curves_positions = clean_route(route_interpolated) print("CURVES ", curves_positions) number_added_scenarios = 0 scenario_vec = [] if not curves_positions or curves_positions[0][0] > 20.0: transform = route_interpolated[0][0] scenario_vec.append({ "pitch": transform.rotation.pitch, "x": transform.location.x, "y": transform.location.y, "yaw": transform.rotation.yaw, "z": transform.location.z }) number_added_scenarios += 1 print(" ROUTE SIZE ", len(route_interpolated)) for curve_start_end_type in curves_positions: if number_added_scenarios == number_scenario3: break end_curve = curve_start_end_type[1] print(curve_start_end_type) # we get a position for scenario 3 just after a curve happens # position_scenario_inroute = end_curve + 1 print(" position ", position_scenario_inroute) transform = route_interpolated[position_scenario_inroute][0] scenario_vec.append({ "pitch": transform.rotation.pitch, "x": transform.location.x, "y": transform.location.y, "yaw": transform.rotation.yaw, "z": transform.location.z }) number_added_scenarios += 1 return scenario_vec
def get_positions_further_thresh(filename, world, thresh): spawn_points = world.get_map().get_spawn_points() routes_vector = [] count_a = 0 for point_a in spawn_points: count_b = 0 for point_b in spawn_points: # print (point_a, point_b) _, route_ab = interpolate_trajectory(world, [point_a, point_b]) distance = estimate_route_distance(route_ab) print(" Distance ", distance) print() if point_a != point_b and distance > thresh: routes_vector.append([point_a, point_b]) count_b += 1 count_a += 1 write_routes(filename, routes_vector, world.get_map().name)
def calculate_route(self): """ This function calculate a route for giving starting_point and ending_point :return: route (includeing Waypoint.transform & RoadOption) """ starting_location = carla.Location( x=float(self.starting_point.split("_")[0]), y=float(self.starting_point.split("_")[1]), z=float(self.starting_point.split("_")[2])) ending_location = carla.Location( x=float(self.ending_point.split("_")[0]), y=float(self.ending_point.split("_")[1]), z=float(self.ending_point.split("_")[2])) # returns list of (carla.Waypoint.transform, RoadOption) from origin to destination coarse_route = [] coarse_route.append(starting_location) coarse_route.append(ending_location) return interpolate_trajectory(self.world, coarse_route)
def test_possible_spawns(self): args = Arguments() client = carla.Client(args.host, int(args.port)) client.set_timeout(25.0) challenge = ChallengeEvaluator(args) filename = os.path.join(self.root_route_file_position, 'routes_training.xml') list_route_descriptions = parser.parse_routes_file(filename) # Which type of file is expected ???? # For each of the routes to be evaluated. for route_description in list_route_descriptions: challenge.world = client.load_world(route_description['town_name']) # Set the actor pool so the scenarios can prepare themselves when needed CarlaActorPool.set_world(challenge.world) CarlaDataProvider.set_world(challenge.world) # find and filter potential scenarios # Returns the iterpolation in a different format challenge.world.wait_for_tick() gps_route, route_description[ 'trajectory'] = interpolate_trajectory( challenge.world, route_description['trajectory']) #print (gps_route) #print (route_description['trajectory']) elevate_transform = route_description['trajectory'][0][0].transform elevate_transform.location.z += 0.5 #print (elevate_transform) challenge.prepare_ego_car(elevate_transform) challenge.cleanup(ego=True)
def get_scenario_list(world, scenarios_json_path, routes_path, routes_id): world_annotations = parse_annotations_file(scenarios_json_path) route_descriptions_list = parse_routes_file(routes_path) per_route_possible_scenarios = [] for id in routes_id: route = route_descriptions_list[id] _, route_interpolated = interpolate_trajectory(world, route['trajectory']) position_less_10_percent = int(0.1 * len(route_interpolated)) possible_scenarios, existent_triggers = scan_route_for_scenarios( route_interpolated[:-position_less_10_percent], world_annotations, world.get_map().name) #if not possible_scenarios: # continue per_route_possible_scenarios.append(possible_scenarios) return route_descriptions_list, per_route_possible_scenarios
def is_straight_ahead(point_a, point_b, distance): """ Check if the ROUTE distance between two points is bigger than "distance" :param point_a: :param point_b: :param distance: :return: """ _, route_initial = interpolate_trajectory( world, [point_a.location, point_b.location]) if estimate_route_distance(route_initial) < distance or \ estimate_route_distance(route_initial) > 3*distance: print("Rejected because it is too small") return False route = clean_route(route_initial) print(" Straight test ") # TODO analize the size of the straight if len(route) > 3: print("Rejected because of size") return False for point in route: # Check if there are any curve if point[2] == RoadOption.LEFT or point[2] == RoadOption.RIGHT: print("Rejected because of curve") return False yaw_difference = point_a.rotation.yaw - point_b.rotation.yaw print(" yaw difference is ", yaw_difference) if math.fabs(yaw_difference) > 10 and math.fabs(yaw_difference) < 340: print("Rejected because of curve") return False return True
def test_routes(args): """ Run all routes according to provided commandline args """ # Routes are always visible args.route_visible = True challenge = ChallengeEvaluator(args) # retrieve worlds annotations world_annotations = parser.parse_annotations_file(args.scenarios) # retrieve routes route_descriptions_list = parser.parse_routes_file(args.routes) # find and filter potential scenarios for each of the evaluated routes # For each of the routes and corresponding possible scenarios to be evaluated. route_description = route_descriptions_list[args.route_id] # setup world and client assuming that the CARLA server is up and running client = carla.Client(args.host, int(args.port)) client.set_timeout(challenge.client_timeout) # load the challenge.world variable to be used during the route challenge.load_world(client, route_description['town_name']) # Set the actor pool so the scenarios can prepare themselves when needed CarlaActorPool.set_client(client) CarlaActorPool.set_world(challenge.world) # Also se the Data provider pool. CarlaDataProvider.set_world(challenge.world) # tick world so we can start. challenge.world.tick() # prepare route's trajectory gps_route, route_description['trajectory'] = interpolate_trajectory( challenge.world, route_description['trajectory']) CarlaDataProvider.set_ego_vehicle_route( convert_transform_to_location(route_description['trajectory'])) potential_scenarios_definitions, existent_triggers = parser.scan_route_for_scenarios( route_description, world_annotations) potential_scenarios_definitions = challenge.filter_scenarios( potential_scenarios_definitions, args.removed_scenarios) print("WE HAVE This number of posibilities : ", len(potential_scenarios_definitions)) # Sample the scenarios to be used for this route instance. sampled_scenarios_definitions = challenge.scenario_sampling( potential_scenarios_definitions) # create agent challenge.agent_instance = getattr( challenge.module_agent, challenge.module_agent.__name__)(args.config) correct_sensors, error_message = challenge.valid_sensors_configuration( challenge.agent_instance, challenge.track) if not correct_sensors: # the sensor configuration is illegal challenge.report_fatal_error(args.filename, args.show_to_participant, error_message) return challenge.agent_instance.set_global_plan(gps_route, route_description['trajectory']) # prepare the ego car to run the route. # It starts on the first wp of the route elevate_transform = route_description['trajectory'][0][0] elevate_transform.location.z += 0.5 challenge.prepare_ego_car(elevate_transform) # build the master scenario based on the route and the target. challenge.master_scenario = challenge.build_master_scenario( route_description['trajectory'], route_description['town_name']) list_scenarios = [challenge.master_scenario] if args.background: background_scenario = challenge.build_background_scenario( route_description['town_name']) list_scenarios.append(background_scenario) # build the instance based on the parsed definitions. print("Definition of the scenarios present on the route ") pprint(sampled_scenarios_definitions) list_scenarios += challenge.build_scenario_instances( sampled_scenarios_definitions, route_description['town_name']) if args.debug > 0: for scenario in sampled_scenarios_definitions: loc = carla.Location( scenario['trigger_position']['x'], scenario['trigger_position']['y'], scenario['trigger_position']['z']) + carla.Location(z=2.0) challenge.world.debug.draw_point(loc, size=1.0, color=carla.Color(255, 0, 0), life_time=100000) challenge.world.debug.draw_string(loc, scenario['name'], draw_shadow=False, color=carla.Color(0, 0, 255), life_time=100000, persistent_lines=True) # Tick once to start the scenarios. print(" Running these scenarios --- ", list_scenarios) for scenario in list_scenarios: scenario.scenario.scenario_tree.tick_once() challenge.run_route(list_scenarios, route_description['trajectory']) # statistics recording challenge.record_route_statistics(route_description['id']) # clean up for scenario in list_scenarios: scenario.remove_all_actors() challenge.cleanup(ego=True) challenge.agent_instance.destroy()
def test_build_scenarios(self): args = Arguments() client = carla.Client(args.host, int(args.port)) client.set_timeout(25.0) challenge = ChallengeEvaluator(args) filename = os.path.join(self.root_route_file_position, 'all_towns_traffic_scenarios1_3_4.json') world_annotations = parser.parse_annotations_file(filename) # retrieve routes # Which type of file is expected ???? filename_train = os.path.join(self.root_route_file_position, 'routes_training.xml') filename_val = os.path.join(self.root_route_file_position, 'routes_devtest.xml') list_route_descriptions = parser.parse_routes_file( filename_train) + parser.parse_routes_file(filename_val) # For each of the routes to be evaluated. for route_description in list_route_descriptions: if route_description['town_name'] == 'Town03'\ or route_description['town_name'] == 'Town04': continue # or route_description['town_name'] == 'Town02': # continue print(" TOWN ", route_description['town_name']) challenge.world = client.load_world(route_description['town_name']) CarlaActorPool.set_client(client) # Set the actor pool so the scenarios can prepare themselves when needed CarlaActorPool.set_world(challenge.world) CarlaDataProvider.set_world(challenge.world) # find and filter potential scenarios # Returns the iterpolation in a different format challenge.world.wait_for_tick() gps_route, route_description[ 'trajectory'] = interpolate_trajectory( challenge.world, route_description['trajectory']) potential_scenarios_definitions, existent_triggers = parser.scan_route_for_scenarios( route_description, world_annotations) # Sample the scenarios sampled_scenarios = challenge.scenario_sampling( potential_scenarios_definitions) # prepare route's trajectory elevate_transform = route_description['trajectory'][0][0] elevate_transform.location.z += 0.5 challenge.prepare_ego_car(elevate_transform) # build the master scenario based on the route and the target. print("loading master") master_scenario = challenge.build_master_scenario( route_description['trajectory'], route_description['town_name']) list_scenarios = [master_scenario] if args.background: background_scenario = challenge.build_background_scenario( route_description['town_name']) list_scenarios.append(background_scenario) print(" Built the master scenario ") # build the instance based on the parsed definitions. print(sampled_scenarios) # remove scenario 8 and 9 scenario_removed = [] for possible_scenario in sampled_scenarios: if possible_scenario['name'] == 'Scenario8' or possible_scenario['name'] == 'Scenario7' or \ possible_scenario['name'] == 'Scenario9' or possible_scenario['name'] == 'Scenario5': continue else: scenario_removed.append(possible_scenario) list_scenarios += challenge.build_scenario_instances( scenario_removed, route_description['town_name']) print(" Scenarios present ", list_scenarios) challenge.cleanup(ego=True)
def __init__(self, client, vehicle_model, route, sensors, scenario_definitions, exp_params, agent_name): """ The experience is like a instance of the environment contains all the objects (vehicles, sensors) and scenarios of the the current experience :param vehicle_model: the model that is going to be used to spawn the ego CAR """ # The sensors dict is important to make videos after self._sensors_dict = sensors # We save the agent name for data savings self._agent_name = agent_name # save all the experiment parameters to be used later self._exp_params = exp_params # carla recorder mode save the full carla logs to do some replays if self._exp_params['carla_recording']: client.start_recorder('env_{}_number_{}_batch_{:0>4d}.log'.format(self._exp_params['env_name'], self._exp_params['env_number'], self._exp_params['exp_number'])) # this parameter sets all the sensor threads and the main thread into saving data self._save_data = exp_params['save_data'] # we can also toogle if we want to save sensors or not. self._save_sensors = exp_params['save_sensors'] # Start objects that are going to be created self.world = None # You try to reconnect a few times. self.MAX_CONNECTION_ATTEMPTS = 7 # Scenario definitions to perform the scenario building self.scenario_definitions = scenario_definitions self._ego_actor = None self._instanced_sensors = [] # set the client object connected to the self._client = client # We also set the town name to be used self._town_name = exp_params['town_name'] self._vehicle_model = vehicle_model # if data is being saved we create the writer object # if we are going to save, we keep track of a dictionary with all the data self._writer = Writer(exp_params['package_name'], exp_params['env_name'], exp_params['env_number'], exp_params['exp_number'], agent_name, other_vehicles=exp_params['save_opponents'], walkers=exp_params['save_walkers']) self._environment_data = {'exp_measurements': None, # The exp measurements are specific of the experience 'ego_controls': None, 'scenario_controls': None} # identify this exp self._exp_id = self._exp_params['exp_number'] # We try running all the necessary initalization, if we fail we clean the try: # Sensor interface, a buffer that contains all the read sensors self._sensor_interface = SensorInterface(number_threads_barrier=len(sensors)) # Load the world self._load_world() # Set the actor pool so the scenarios can prepare themselves when needed CarlaActorPool.set_client(client) CarlaActorPool.set_world(self.world) # Set the world for the global data provider CarlaDataProvider.set_world(self.world) # We get the lat lon ref that is important for the route self._lat_ref, self._lon_ref = _get_latlon_ref(self.world) # We instance the ego actor object _, self._route = interpolate_trajectory(self.world, route) # elevate the z transform to avoid spawning problems elevate_transform = self._route[0][0] elevate_transform.location.z += 0.5 self._spawn_ego_car(elevate_transform) # We setup all the instanced sensors self._setup_sensors(sensors, self._ego_actor) # We set all the traffic lights to green to avoid having this traffic scenario. self._reset_map() # Data for building the master scenario self._timeout = estimate_route_timeout(self._route) self._master_scenario = self.build_master_scenario(self._route, exp_params['town_name'], self._timeout, True) other_scenarios = self.build_scenario_instances(scenario_definitions, self._timeout) self._list_scenarios = [self._master_scenario] + other_scenarios # Route statistics, when the route is finished there will # be route statistics on this object. and nothing else self._route_statistics = None # We tick the world to have some starting points self.tick_world() except RuntimeError as r: # We clean the dataset if there is any exception on creation traceback.print_exc() if self._save_data: self._clean_bad_dataset() # Re raise the exception raise r
def scenario_route_generator(routes_file, scenarios_tag, scenario_name, number_of_routes): """ returns exp dictionaries containing a single scenario happening on that route. :param routes_file: :param scenarios_tag: :param scenario_number: The name of the scenario that is going to be used. :param number_of_routes: :return: """ # TODO I need to know the route points related to the scenario ... # retrieve worlds annotations world_annotations = parser.parse_annotations_file(scenarios_tag) # retrieve routes route_descriptions_list = parser.parse_routes_file(routes_file) # Connect to some carla here. env_vec = [] count = 0 client = carla.Client('localhost', 2000) client.set_timeout(32) for _route_description in route_descriptions_list: original_trajectory = _route_description['trajectory'] world = client.load_world(_route_description['town_name']) _, _route_description['trajectory'] = interpolate_trajectory( world, _route_description['trajectory']) potential_scenarios_definitions, _ = parser.scan_route_for_scenarios( _route_description, world_annotations) # remove the trigger positon clustering since we are only doing one scenario per time. # TODO add multi scenario route posibilities scenario_definitions = [] for trigger in potential_scenarios_definitions: scenario_definitions += potential_scenarios_definitions[trigger] # filter only the scenario that you want to generate the routes. # (Doing more than one would be quicker but i preffer this organization for now) filtered_scenario_definitions = [] for pot_scenario in scenario_definitions: if pot_scenario['name'] == scenario_name: filtered_scenario_definitions.append(pot_scenario) print(" The representations") print(original_trajectory, filtered_scenario_definitions) # Now we get the correspondent route points for the trigger for sce_def in filtered_scenario_definitions: route = find_closest_route_trip(sce_def['trigger_position'], original_trajectory) env_vec.append({ scenario_name + '_' + 'route_' + str(count): { 'route': route, 'scenarios': { scenario_name: sce_def['trigger_position'] }, 'town_name': _route_description['town_name'], 'vehicle_model': "vehicle.lincoln.mkz2017" } }) count += 1 return env_vec[0:number_of_routes]