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]
