utils for generating additional routes and scenarios

tools/generate_intersection_routes.py

+import glob
+import os
+import sys
+import lxml.etree as ET
+import argparse
+import random
+import time
+
+import carla
+
+SAMPLING_DISTANCE = [100]
+
+def add_intersection(transform, root, route_id):
+    '''
+    Sample (start wp, end wp) pair along the canonical axes in a 100x100 grid
+    Args:
+        transform: carla transform of the grid center (position of traffic light)
+        root: root of the xml tree structure
+        route_id: route counter
+    '''
+    x, y, yaw = transform.location.x, transform.location.y, transform.rotation.yaw
+    req_yaw = yaw + 180.0 # the vehicle should be opposite the traffic light
+    for dist in SAMPLING_DISTANCE:
+        for mul in [-1, 1]:
+            route = ET.SubElement(root, 'route', id='%d'%route_id, town=args.town)
+            ET.SubElement(route, 'waypoint', x='%f'%(x+mul*dist), y='%f'%(y), z='0.0', 
+                                             pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+            ET.SubElement(route, 'waypoint', x='%f'%(x), y='%f'%(y+mul*dist), z='0.0', 
+                                             pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+            route_id += 1
+
+            route = ET.SubElement(root, 'route', id='%d'%route_id, town=args.town)
+            ET.SubElement(route, 'waypoint', x='%f'%(x+mul*dist), y='%f'%y, z='0.0', 
+                                             pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+            ET.SubElement(route, 'waypoint', x='%f'%(x), y='%f'%(y-mul*dist), z='0.0', 
+                                             pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+            route_id += 1
+
+            route = ET.SubElement(root, 'route', id='%d'%route_id, town=args.town)
+            ET.SubElement(route, 'waypoint', x='%f'%(x-mul*dist), y='%f'%(y), z='0.0', 
+                                             pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+            ET.SubElement(route, 'waypoint', x='%f'%(x), y='%f'%(y+mul*dist), z='0.0', 
+                                             pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+            route_id += 1
+
+            route = ET.SubElement(root, 'route', id='%d'%route_id, town=args.town)
+            ET.SubElement(route, 'waypoint', x='%f'%(x+mul*dist), y='%f'%y, z='0.0', 
+                                             pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+            ET.SubElement(route, 'waypoint', x='%f'%(x), y='%f'%(y-mul*dist), z='0.0', 
+                                             pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+            route_id += 1
+
+    return root, route_id
+
+def add_intersection_subsample(transform, root, route_id):
+    '''
+    Same function as above but samples 75% fewer routes
+    Args:
+        transform: carla transform of the grid center (position of traffic light)
+        root: root of the xml tree structure
+        route_id: route counter
+    '''
+    x, y, yaw = transform.location.x, transform.location.y, transform.rotation.yaw
+    req_yaw = yaw + 180.0 # the vehicle should be opposite the traffic light
+    for dist in SAMPLING_DISTANCE:
+        for mul in [-1, 1]:
+            if random.randint(0,7) == 0:
+                route = ET.SubElement(root, 'route', id='%d'%route_id, town=args.town)
+                ET.SubElement(route, 'waypoint', x='%f'%(x+mul*dist), y='%f'%(y), z='0.0', 
+                                                 pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+                ET.SubElement(route, 'waypoint', x='%f'%(x), y='%f'%(y+mul*dist), z='0.0', 
+                                                 pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+                route_id += 1
+
+            if random.randint(0,7) == 0:
+                route = ET.SubElement(root, 'route', id='%d'%route_id, town=args.town)
+                ET.SubElement(route, 'waypoint', x='%f'%(x+mul*dist), y='%f'%y, z='0.0', 
+                                                 pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+                ET.SubElement(route, 'waypoint', x='%f'%(x), y='%f'%(y-mul*dist), z='0.0', 
+                                                 pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+                route_id += 1
+
+            if random.randint(0,7) == 0:
+                route = ET.SubElement(root, 'route', id='%d'%route_id, town=args.town)
+                ET.SubElement(route, 'waypoint', x='%f'%(x-mul*dist), y='%f'%(y), z='0.0', 
+                                                 pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+                ET.SubElement(route, 'waypoint', x='%f'%(x), y='%f'%(y+mul*dist), z='0.0', 
+                                                 pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+                route_id += 1
+
+            if random.randint(0,7) == 0:
+                route = ET.SubElement(root, 'route', id='%d'%route_id, town=args.town)
+                ET.SubElement(route, 'waypoint', x='%f'%(x+mul*dist), y='%f'%y, z='0.0', 
+                                                 pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+                ET.SubElement(route, 'waypoint', x='%f'%(x), y='%f'%(y-mul*dist), z='0.0', 
+                                                 pitch='0.0', roll='0.0', yaw='%f'%req_yaw)
+                route_id += 1
+
+    return root, route_id
+
+def main():
+    client = carla.Client('localhost', 2100)
+    client.set_timeout(200.0)
+    world = client.load_world(args.town)
+    print ('loaded world')
+
+    actors = world.get_actors()
+    traffic_lights_list = actors.filter('*traffic_light')
+    print ('got %d traffic lights'%len(traffic_lights_list))
+
+    # each traffic light group at an intersection counted once
+    count = 0
+    route_id = 0
+    root = ET.Element('routes')
+    traffic_light_visited = []
+    for traffic_light in traffic_lights_list:
+        if traffic_light.id not in traffic_light_visited:
+            traffic_light_visited.append(traffic_light.id)
+            count += 1
+            if not args.subsample:
+                root, route_id = add_intersection(traffic_light.get_transform(), root, route_id)
+            else:
+                root, route_id = add_intersection_subsample(traffic_light.get_transform(), root, route_id)
+            for adjacent_traffic_light in traffic_light.get_group_traffic_lights():
+                traffic_light_visited.append(adjacent_traffic_light.id)
+    print ('unique intersections: ', count)
+
+    tree = ET.ElementTree(root)
+
+    if args.save_file is not None:
+        tree.write(args.save_file, xml_declaration=True, encoding='utf-8', pretty_print=True)
+    
+    len_tree = 0
+    for _ in tree.iter('route'):
+        len_tree += 1
+    print ('total routes: ', len_tree)
+
+
+if __name__ == '__main__':
+    global args
+
+    parser = argparse.ArgumentParser()
+
+    parser.add_argument('--save_file', type=str, required=False, default=None, help='xml file path to save the route waypoints')
+    parser.add_argument('--town', type=str, default='Town05', help='town for generating routes')
+    parser.add_argument('--subsample', action='store_true', default=False, help='sample 75% fewer routes')
+    
+    args = parser.parse_args()
+
+    main()

tools/generate_scenarios.py

+import os
+import sys
+import numpy as np
+import json
+import argparse
+import xml.etree.ElementTree as ET
+import cv2
+import copy
+import glob
+import ast
+from  pathlib import Path
+
+
+def main(args):
+	if args.towns == 'all':
+		towns = ['Town01', 'Town02', 'Town03', 'Town04', 'Town05', 'Town06']
+	else:
+		towns = [args.towns]
+
+	# aggregate transforms of all the scenarios present in the provided json and save them individually
+	scenarios = open(args.scenarios_file, 'r')
+	scenarios_data = json.load(scenarios)
+	scenarios.close()
+
+	available_event_configurations = []
+	scenario_types = []
+	available_scenarios = scenarios_data['available_scenarios'][0]
+	for town in available_scenarios.keys():
+	    if town not in towns:
+	        continue
+	    scenarios_town = available_scenarios[town]
+	    for scenario in scenarios_town:
+	        transforms_list = scenario['available_event_configurations']
+	        scenario_type = scenario['scenario_type']
+	        for transform in transforms_list:
+	            curr_transform = transform['transform']
+	            for attrib in curr_transform.keys():
+	                curr_transform[attrib] = float(curr_transform[attrib])
+	            curr_transform['z'] = 0.0
+	            if curr_transform not in available_event_configurations:
+	                available_event_configurations.append(curr_transform)
+	        scenario_types.append(scenario_type)
+
+	available_event_configurations_dict_list = []
+	for sample in available_event_configurations:
+	    available_event_configurations_dict_list.append({'transform': sample})
+	print (len(available_event_configurations_dict_list))
+
+	# sample dense points along each trigger transform provided
+	aggr_available_event_config = []
+	for curr_transform in available_event_configurations_dict_list:
+	    augmented_transforms = []
+	    def add_yaw_augmentation(transform): # trigger points can have any orientation
+	        aggr_transform = []
+	        for mul in range(4):
+	            aug_transform = copy.deepcopy(transform)
+	            aug_transform['transform']['yaw'] += mul*90.0
+	            aggr_transform.append(aug_transform)
+	        return aggr_transform
+	    
+	    augmented_transforms += add_yaw_augmentation(curr_transform) # add base transform
+	    for dist in range(-10,11,5): # sample extra points along each axes
+	        if dist == 0: # base transform already added
+	            continue
+	        new_transform_x = copy.deepcopy(curr_transform)
+	        new_transform_x['transform']['x'] += dist
+	        augmented_transforms += add_yaw_augmentation(new_transform_x)
+	        new_transform_y = copy.deepcopy(curr_transform)
+	        new_transform_y['transform']['y'] += dist
+	        augmented_transforms += add_yaw_augmentation(new_transform_y)
+	    
+	    aggr_available_event_config += augmented_transforms
+	print (len(aggr_available_event_config))
+
+	json_list_data = {'available_scenarios': []}
+	for town in towns:
+	    all_scenarios = []
+	    for index in range(1,11):
+	        if index == 2 or index == 5 or index == 6: # discard lane changing scenarios since expert is not configured for it
+	            continue
+	        all_scenarios.append({'available_event_configurations': aggr_available_event_config, 'scenario_type': 'Scenario%d'%index})
+	    json_list_data['available_scenarios'].append({town: all_scenarios})
+
+	with open(args.save_file, 'w') as f:
+	    json.dump(json_list_data, f, indent=4)
+	f.close()
+
+if __name__ == '__main__':
+
+	parser = argparse.ArgumentParser()
+
+	parser.add_argument('--scenarios_file', type=str, required=True, help='original file reference scenarios')
+	parser.add_argument('--save_file', type=str, required=False, help='output file with dense scenarios')
+	parser.add_argument('--towns', type=str, required=True, help='Town01/Town02/Town03/Town04/Town05/Town06/all')
+	
+	args = parser.parse_args()
+
+	main(args)
\ No newline at end of file

tools/interpolate_route.py

+import os
+import sys
+import json
+import argparse
+import xml.etree.ElementTree as ET
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+import carla
+from agents.navigation.global_route_planner import GlobalRoutePlanner
+from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
+
+# navigational commands: RoadOption:LEFT and so on
+NV_DICT = {1: 'LEFT', 2: 'RIGHT', 3: 'STRAIGHT', 4: 'LANEFOLLOW', 5: 'CHANGELANELEFT', 6: 'CHANGELANERIGHT'}
+
+def interpolate_trajectory(world_map, waypoints_trajectory, hop_resolution=1.0):
+    """
+    Given some raw keypoints interpolate a full dense trajectory to be used by the user.
+    Args:
+        world: an reference to the CARLA world so we can use the planner
+        waypoints_trajectory: the current coarse trajectory
+        hop_resolution: is the resolution, how dense is the provided trajectory going to be made
+    Return: 
+        route: full interpolated route both in GPS coordinates and also in its original form.
+    """
+
+    dao = GlobalRoutePlannerDAO(world_map, hop_resolution)
+    grp = GlobalRoutePlanner(dao)
+    grp.setup()
+    # Obtain route plan
+    route = []
+    for i in range(len(waypoints_trajectory) - 1):   # Goes until the one before the last.
+
+        waypoint = waypoints_trajectory[i]
+        waypoint_next = waypoints_trajectory[i + 1]
+        interpolated_trace = grp.trace_route(waypoint, waypoint_next)
+        for wp_tuple in interpolated_trace:
+            route.append((wp_tuple[0].transform, wp_tuple[1]))
+            # print (wp_tuple[0].transform.location, wp_tuple[1])
+
+    return route
+
+
+def parse_routes_file(route_filename, single_route=None):
+    """
+    Returns a list of route elements that is where the challenge is going to happen.
+    Args:
+        route_filename: the path to a set of routes.
+        single_route: If set, only this route shall be returned
+    Return:
+        list_route_descriptions: List of dicts containing the waypoints, id and town of the routes
+    """
+
+    list_route_descriptions = []
+    tree = ET.parse(route_filename)
+    for route in tree.iter("route"):
+        route_town = route.attrib['town']
+        route_id = route.attrib['id']
+        if single_route and route_id != single_route:
+            continue
+
+        waypoint_list = []  # the list of waypoints that can be found on this route
+        for waypoint in route.iter('waypoint'):
+            waypoint_list.append(carla.Location(x=float(waypoint.attrib['x']),
+                                                y=float(waypoint.attrib['y']),
+                                                z=float(waypoint.attrib['z'])))
+
+        list_route_descriptions.append({
+            'id': route_id,
+            'town_name': route_town,
+            'trajectory': waypoint_list
+        })
+
+    return list_route_descriptions
+
+
+def main(args):
+
+	client = carla.Client('localhost', 2100)
+	client.set_timeout(200.0)
+
+	routes_list = parse_routes_file(args.routes_file)
+
+	anomaly = []
+	total_waypoints = 0
+	waypoint_threshold = args.wp_threshold
+	nv_cnt = [0]*7 # total 6 navigational commands [1,6]
+
+	for index, route in enumerate(routes_list):
+		if index == 0 or routes_list[index]['town_name'] != routes_list[index-1]['town_name']:
+			world = client.load_world(route['town_name'])
+			world_map = world.get_map()
+		interpolated_route = interpolate_trajectory(world_map, route['trajectory'])
+		# print ('interpolated route {} from {} waypoints to {} waypoints'.format(route['id'], 
+		# 									len(route['trajectory']), len(interpolated_route)))
+		total_waypoints += len(interpolated_route)
+		if len(interpolated_route) >= waypoint_threshold:
+			anomaly.append(route['id'])
+			continue
+		for waypoint in interpolated_route:
+			nv_cnt[waypoint[1].value] += 1
+
+	print ('found anomalies in routes ids: ', anomaly)
+
+	print ('retained {:.3f} % data after discarding routes with more than {} waypoints'.format(100*np.sum(nv_cnt)/total_waypoints, waypoint_threshold))
+	for index in range(1,7):
+		print (NV_DICT[index], nv_cnt[index], nv_cnt[index]/np.sum(nv_cnt))
+
+
+if __name__ == '__main__':
+
+	parser = argparse.ArgumentParser()
+
+	parser.add_argument('--routes_file', type=str, required=True, help='file containing the route waypoints')
+	parser.add_argument('--wp_threshold', type=int, required=False, default=1e10)
+
+	args = parser.parse_args()
+
+	main(args)
\ No newline at end of file

tools/sample_junctions.py

+import os
+import sys
+import json
+import time
+import argparse
+import multiprocessing
+import lxml.etree as ET
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+import carla
+from agents.navigation.global_route_planner import GlobalRoutePlanner
+from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
+from agents.navigation.local_planner import RoadOption
+
+def interpolate_trajectory(world_map, waypoints_trajectory, hop_resolution=1.0):
+    """
+    Given some raw keypoints interpolate a full dense trajectory to be used by the user.
+    Args:
+        world: an reference to the CARLA world so we can use the planner
+        waypoints_trajectory: the current coarse trajectory
+        hop_resolution: is the resolution, how dense is the provided trajectory going to be made
+    Return: 
+        route: full interpolated route both in GPS coordinates and also in its original form.
+    """
+
+    dao = GlobalRoutePlannerDAO(world_map, hop_resolution)
+    grp = GlobalRoutePlanner(dao)
+    grp.setup()
+    # Obtain route plan
+    route = []
+    for i in range(len(waypoints_trajectory) - 1):   # Goes until the one before the last.
+
+        waypoint = waypoints_trajectory[i]
+        waypoint_next = waypoints_trajectory[i + 1]
+        interpolated_trace = grp.trace_route(waypoint, waypoint_next)
+        for wp_tuple in interpolated_trace:
+            route.append((wp_tuple[0].transform, wp_tuple[1]))
+
+    return route
+
+
+def parse_routes_file(route_filename, single_route=None):
+    """
+    Returns a list of route elements that is where the challenge is going to happen.
+    Args:
+        route_filename: the path to a set of routes.
+        single_route: If set, only this route shall be returned
+    Return:
+        list_route_descriptions: List of dicts containing the waypoints, id and town of the routes
+    """
+
+    list_route_descriptions = []
+    tree = ET.parse(route_filename)
+    for route in tree.iter("route"):
+        route_town = route.attrib['town']
+        route_id = route.attrib['id']
+        if single_route and route_id != single_route:
+            continue
+
+        waypoint_list = []  # the list of waypoints that can be found on this route
+        for waypoint in route.iter('waypoint'):
+            waypoint_list.append(carla.Location(x=float(waypoint.attrib['x']),
+                                                y=float(waypoint.attrib['y']),
+                                                z=float(waypoint.attrib['z'])))
+
+            # Waypoints is basically a list of XML nodes
+
+        list_route_descriptions.append({
+            'id': route_id,
+            'town_name': route_town,
+            'trajectory': waypoint_list
+        })
+
+    return list_route_descriptions
+
+
+def sample_junctions(world_map, route):
+    """
+    Sample individual junctions from the interpolated routes
+    Args:
+        world_map: town map
+        route: interpolated route
+    Return:
+        custom_routes: list of (start wp, end wp) each representing an individual junction
+    """
+    custom_routes = []
+    start_id = -1
+    end_id = -1
+    for index in range(18, len(route)):
+        if route[index-1][1] == RoadOption.LANEFOLLOW and route[index][1] != RoadOption.LANEFOLLOW:
+            start_id = index-18 # start from 18m before rather than exactly at the intersection otherwise traffic lights don't get detected properly
+        elif start_id != -1 and route[index][1] == RoadOption.LANEFOLLOW:
+            end_id = index-1
+            if end_id > start_id + 28: # at least 28m distance
+                
+                # extra check to make sure that long/invalid routes are discarded
+                start_wp = carla.Location(x=route[start_id][0].location.x, 
+                            y=route[start_id][0].location.y, z=route[start_id][0].location.z)
+                end_wp = carla.Location(x=route[end_id][0].location.x, 
+                            y=route[end_id][0].location.y, z=route[end_id][0].location.z)
+                waypoint_list = [start_wp, end_wp]
+                extended_route = interpolate_trajectory(world_map, waypoint_list)
+                if len(extended_route) >= 100 or len(extended_route) == 1:
+                    start_id = -1
+                    end_id = -1
+                    continue
+                
+                start_transform = (route[start_id][0].location.x, route[start_id][0].location.y, 
+                                route[start_id][0].location.z, route[start_id][0].rotation.yaw)
+                end_transform = (route[end_id][0].location.x, route[end_id][0].location.y, 
+                                route[end_id][0].location.z, route[end_id][0].rotation.yaw)
+                custom_routes.append([start_transform, end_transform])
+                
+            start_id = -1
+            end_id = -1
+
+    return custom_routes
+
+def process_route(world_map, route, return_dict):
+    interpolated_route = interpolate_trajectory(world_map, route['trajectory'])
+    wp_list = sample_junctions(world_map, interpolated_route)
+    print ('got {} junctions in interpolated route {} from {} waypoints to {} waypoints'.format(
+                    len(wp_list), route['id'], len(route['trajectory']), len(interpolated_route)))
+
+    return_dict[route['id']] = {'wp_list': wp_list, 'town_name': route['town_name'], 'length': len(interpolated_route)}
+
+
+def main(args):
+
+    client = carla.Client('localhost', 2100)
+    client.set_timeout(200.0)
+
+    routes_list = parse_routes_file(args.routes_file)
+
+    manager = multiprocessing.Manager()
+    return_dict = manager.dict()
+    jobs = []
+
+    st = time.time()
+    for index, route in enumerate(routes_list):
+        if index == 0 or routes_list[index]['town_name'] != routes_list[index-1]['town_name']:
+            world = client.load_world(route['town_name'])
+            world_map = world.get_map()
+        p = multiprocessing.Process(target=process_route, args=(world_map, route, return_dict))
+        jobs.append(p)
+        p.start()
+
+    for process in jobs:
+        process.join()
+    print ('{} routes processed in {} seconds'.format(len(return_dict), time.time()-st))
+
+    route_id = 0
+    total_junctions = 0
+    route_lengths = []
+    root = ET.Element('routes')
+    for curr_route in return_dict.keys():
+        wp_list = return_dict[curr_route]['wp_list']
+        town_name = return_dict[curr_route]['town_name']
+        total_junctions += len(wp_list)
+        route_lengths.append(return_dict[curr_route]['length'])
+        for wps in wp_list:
+            add_route = ET.SubElement(root, 'route', id='%d'%route_id, town=town_name) # 'town' in carla 0.9.10, 'map' in carla 0.9.9
+            ET.SubElement(add_route, 'waypoint', x='%f'%wps[0][0], y='%f'%wps[0][1],  z='%f'%wps[0][2], 
+                                                            pitch='0.0', roll='0.0', yaw='%f'%wps[0][3])
+            ET.SubElement(add_route, 'waypoint', x='%f'%wps[1][0], y='%f'%wps[1][1], z='%f'%wps[1][2], 
+                                                            pitch='0.0', roll='0.0', yaw='%f'%wps[1][3])
+            route_id += 1
+
+    print ('total_junctions: ', total_junctions)
+    print ('mean distance: ', np.array(route_lengths).mean())
+    print ('median distance: ', np.median(np.array(route_lengths)))
+        
+    if args.save_file is not None:
+        tree = ET.ElementTree(root)
+        tree.write(args.save_file, xml_declaration=True, encoding='utf-8', pretty_print=True)
+
+if __name__ == '__main__':
+
+    parser = argparse.ArgumentParser()
+
+    parser.add_argument('--routes_file', type=str, required=True, help='file containing the route waypoints')
+    parser.add_argument('--save_file', type=str, required=False, default=None, help='xml file path to save the route waypoints')
+
+    args = parser.parse_args()
+
+    main(args)
\ No newline at end of file