update batch processing to handle errors

import sys

sys.path.append("../../GSMP/tools/")

sys.path.append("../../GSMP/tools/commonroad-collision-checker")

sys.path.append("../../GSMP/tools/commonroad-road-boundary")

sys.path.append("../../GSMP/motion_automata/vehicle_model")

import os

path_commonroad_search = "../../"

sys.path.append(os.path.join(path_commonroad_search, "GSMP/tools/"))

sys.path.append(os.path.join(path_commonroad_search, "GSMP/tools/commonroad-collision-checker"))

sys.path.append(os.path.join(path_commonroad_search, "GSMP/tools/commonroad-road-boundary"))

sys.path.append(os.path.join(path_commonroad_search, "GSMP/motion_automata/vehicle_model"))

import time

from multiprocessing import Manager, Process

    # step 1

    automata = MotionAutomata(veh_type_id)

    prefix = '../../GSMP/motion_automata/motion_primitives/'

    print("Reading motion primitives...")

    try:

        prefix = '../../GSMP/motion_automata/motion_primitives/'

        if veh_type_id == 1:

            automata.readFromXML(prefix + 'V_0.0_20.0_Vstep_1.0_SA_-0.91_0.91_SAstep_0.23_T_0.5_Model_FORD_ESCORT.xml')

        elif veh_type_id == 2:

            automata.readFromXML(prefix + 'V_0.0_20.0_Vstep_1.0_SA_-1.066_1.066_SAstep_0.27_T_0.5_Model_BMW320i.xml')

        elif veh_type_id == 3:

            automata.readFromXML(prefix + 'V_0.0_20.0_Vstep_1.0_SA_-1.023_1.023_SAstep_0.26_T_0.5_Model_VW_VANAGON.xml')

    except Exception:

        prefix = '../../../GSMP/motion_automata/motion_primitives/'

        if veh_type_id == 1:

            automata.readFromXML(prefix + 'V_0.0_20.0_Vstep_1.0_SA_-0.91_0.91_SAstep_0.23_T_0.5_Model_FORD_ESCORT.xml')

%% Cell type:markdown id: tags:

# Batch Processing

%% Cell type:markdown id: tags:

This is a script that helps you to batch process your code to solve for solution to different scenarios. It is composed of a configuration file (.yaml) and the codes below. Before you run this code, you should make sure the configuration file is set correctly. Here are the explanation for each of the parameters in the configuration file:

* input_path: the input directory of CommonRoad scenarios that you indend to solve.

* output_path: the output directory of the solution files.

* overwrite: the falg to determine whether to overwrite existing solution files.

* timeout: timeout time for your motion planner, unit in seconds

* trajectory_planner_path: input directory where the module containing the function to execute your motion planner is located

* trajectory_planner_module_name: name of the module taht contains the function to execute your motion planner

* trajectory_planner_function_name: name of the function that executes your motion planner

* default: the parameters specified under this will be applied to all scenarios. if you wish to specify a different paramter for specific scenarios, simply copy the section and replace 'default' with the id of your scenario.

* vehicle_model: model of the vehicle, its value could be PM, KS, ST and MB.

* vehicle_type type of the vehicle, its value could be FORD_ESCORT, BMW_320i and VW_VANAGON.

* cost_function: identifier of cost function. Please refer to [Cost Functions](https://gitlab.lrz.de/tum-cps/commonroad-cost-functions/blob/master/costFunctions_commonRoad.pdf) for more information.

%% Cell type:markdown id: tags:

### Helper functions

%% Cell type:code id: tags:

``` python

import os

import pathlib

import multiprocessing

import yaml

import sys

import warnings

from commonroad.common.file_reader import CommonRoadFileReader

from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleModel, VehicleType, CostFunction

def parse_vehicle_model(model):

    if model == 'PM':

        cr_model = VehicleModel.PM

    elif model == 'ST':

        cr_model = VehicleModel.ST

    elif model == 'KS':

        cr_model = VehicleModel.KS

    elif model == 'MB':

        cr_model = VehicleModel.MB

    else:

        raise ValueError('Selected vehicle model is not valid: {}.'.format(model))

    return cr_model

def parse_vehicle_type(type):

    if type == 'FORD_ESCORT':

        cr_type = VehicleType.FORD_ESCORT

        cr_type_id = 1

    elif type == 'BMW_320i':

        cr_type = VehicleType.BMW_320i

        cr_type_id = 2

    elif type == 'VW_VANAGON':

        cr_type = VehicleType.VW_VANAGON

        cr_type_id = 3

    else:

        raise ValueError('Selected vehicle type is not valid: {}.'.format(type))

    return cr_type, cr_type_id

def parse_cost_function(cost):

    if cost == 'JB1':

        cr_cost = CostFunction.JB1

    elif cost == 'SA1':

        cr_cost = CostFunction.SA1

    elif cost == 'WX1':

        cr_cost = CostFunction.WX1

    elif cost == 'SM1':

        cr_cost = CostFunction.SM1

    elif cost == 'SM2':

        cr_cost = CostFunction.SM2

    elif cost == 'SM3':

        cr_cost = CostFunction.SM3

    else:

        raise ValueError('Selected cost function is not valid: {}.'.format(cost))

    return cr_cost

def call_trajectory_planner(queue, function, scenario, planning_problem_set, vehicle_type_id):

    queue.put(function(scenario, planning_problem_set, vehicle_type_id))

```

%% Cell type:markdown id: tags:

### Read configuration

%% Cell type:code id: tags:

``` python

# open config file

with open('batch_processing_config.yaml', 'r') as stream:

    try:

        settings = yaml.load(stream)

    except yaml.YAMLError as exc:

        print(exc)

# get planning wrapper function

sys.path.append(os.getcwd() + os.path.dirname(settings['trajectory_planner_path']))

module = __import__(settings['trajectory_planner_module_name'])

function = getattr(module, settings['trajectory_planner_function_name'])

time_timeout = settings['timeout']

```

%% Cell type:markdown id: tags:

### Start Processing

%% Cell type:code id: tags:

``` python

# iterate through scenarios

num_files = len(os.listdir(settings['input_path']))

print("Total number of files to be processed: {}".format(num_files))

print("Timeout setting: {} seconds\n".format(time_timeout))

count_processed = 0

for filename in os.listdir(settings['input_path']):

    count_processed += 1

    print("File No. {} / {}".format(count_processed, num_files))

    if not filename.endswith('.xml'):

        continue

    fullname = os.path.join(settings['input_path'], filename)

    print("Started processing scenario {}".format(filename))

    scenario, planning_problem_set = CommonRoadFileReader(fullname).open()

    # get settings for each scenario

    if scenario.benchmark_id in settings.keys():

        # specific

        vehicle_model = parse_vehicle_model(settings[scenario.benchmark_id]['vehicle_model'])

        vehicle_type,vehicle_type_id = parse_vehicle_type(settings[scenario.benchmark_id]['vehicle_type'])

        cost_function = parse_cost_function(settings[scenario.benchmark_id]['cost_function'])

    else:

        # default

        vehicle_model = parse_vehicle_model(settings['default']['vehicle_model'])

        vehicle_type, vehicle_type_id = parse_vehicle_type(settings['default']['vehicle_type'])

        cost_function = parse_cost_function(settings['default']['cost_function'])

    queue = multiprocessing.Queue()

    # create process, pass in required arguements

    p = multiprocessing.Process(target=call_trajectory_planner, name="trajectory_planner",

                                args=(queue, function, scenario, planning_problem_set, vehicle_type_id))

    # start planning

    p.start()

    # wait till process ends or skip if timed out

    p.join(timeout=time_timeout)

    if p.is_alive():

        print("===> Trajectory planner timeout.")

        p.terminate()

        p.join()

        solution_trajectories = {}

    else:

        print("Planning finished.")

        solution_trajectories = queue.get()

    # create path for solutions

    pathlib.Path(settings['output_path']).mkdir(parents=True, exist_ok=True)

    error = False

    cr_solution_writer = CommonRoadSolutionWriter(settings['output_path'],

                                                  scenario.benchmark_id,

                                                  scenario.dt,

                                                  vehicle_type,

                                                  vehicle_model,

                                                  cost_function)

    # inspect whether all planning problems are solved

    for planning_problem_id, planning_problem in planning_problem_set.planning_problem_dict.items():

        if planning_problem_id not in solution_trajectories.keys():

            print('Solution for planning problem with ID={} is not provided for scenario {}. Solution skipped.'.format(

                planning_problem_id, filename))

            error = True

            break

        else:

            cr_solution_writer.add_solution_trajectory(

                solution_trajectories[planning_problem_id], planning_problem_id)

    if not error:

        cr_solution_writer.write_to_file(overwrite=settings['overwrite'])

    print("=========================================================")

```

import os

import pathlib

import multiprocessing

import yaml

import sys

import warnings

from commonroad.common.file_reader import CommonRoadFileReader

from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleModel, VehicleType, CostFunction

def parse_vehicle_model(model):

    if model == 'PM':

        cr_model = VehicleModel.PM

    elif model == 'ST':

        cr_model = VehicleModel.ST

    elif model == 'KS':

        cr_model = VehicleModel.KS

    elif model == 'MB':

        cr_model = VehicleModel.MB

    else:

        raise ValueError('Selected vehicle model is not valid: {}.'.format(model))

    return cr_model

def parse_vehicle_type(type):

    if type == 'FORD_ESCORT':

        cr_type = VehicleType.FORD_ESCORT

        cr_type_id = 1

    elif type == 'BMW_320i':

        cr_type = VehicleType.BMW_320i

        cr_type_id = 2

    elif type == 'VW_VANAGON':

        cr_type = VehicleType.VW_VANAGON

        cr_type_id = 3

    else:

        raise ValueError('Selected vehicle type is not valid: {}.'.format(type))

    return cr_type, cr_type_id

def parse_cost_function(cost):

    if cost == 'JB1':

        cr_cost = CostFunction.JB1

    elif cost == 'SA1':

        cr_cost = CostFunction.SA1

    elif cost == 'WX1':

        cr_cost = CostFunction.WX1

    elif cost == 'SM1':

        cr_cost = CostFunction.SM1

    elif cost == 'SM2':

        cr_cost = CostFunction.SM2

    elif cost == 'SM3':

        cr_cost = CostFunction.SM3

    else:

        raise ValueError('Selected cost function is not valid: {}.'.format(cost))

    return cr_cost

def call_trajectory_planner(queue, function, scenario, planning_problem_set, vehicle_type_id):

    queue.put(function(scenario, planning_problem_set, vehicle_type_id))

# open config file

with open('batch_processing_config.yaml', 'r') as stream:

    try:

        settings = yaml.load(stream)

    except yaml.YAMLError as exc:

        print(exc)

# get planning wrapper function

sys.path.append(os.getcwd() + os.path.dirname(settings['trajectory_planner_path']))

module = __import__(settings['trajectory_planner_module_name'])

function = getattr(module, settings['trajectory_planner_function_name'])

if __name__ == '__main__':

    # iterate through scenarios

    for filename in os.listdir(settings['input_path']):

        if not filename.endswith('.xml'):

            continue

        fullname = os.path.join(settings['input_path'], filename)

        print("Started processing scenario {}".format(filename))

        scenario, planning_problem_set = CommonRoadFileReader(fullname).open()

        # get settings for each scenario

        if scenario.benchmark_id in settings.keys():

            # specific

            vehicle_model = parse_vehicle_model(settings[scenario.benchmark_id]['vehicle_model'])

            vehicle_type,vehicle_type_id = parse_vehicle_type(settings[scenario.benchmark_id]['vehicle_type'])

            cost_function = parse_cost_function(settings[scenario.benchmark_id]['cost_function'])

        else:

            # default

            vehicle_model = parse_vehicle_model(settings['default']['vehicle_model'])

            vehicle_type, vehicle_type_id = parse_vehicle_type(settings['default']['vehicle_type'])

            cost_function = parse_cost_function(settings['default']['cost_function'])

        queue = multiprocessing.Queue()

        # create process, pass in required arguements

        p = multiprocessing.Process(target=call_trajectory_planner, name="trajectory_planner",

                                    args=(queue, function, scenario, planning_problem_set, vehicle_type_id))

        # start planning

        p.start()

        # wait till process ends or skip if timed out

        p.join(timeout=settings['timeout'])

        if p.is_alive():

            print("Trajectory planner timeout.")

            p.terminate()

            p.join()

            solution_trajectories = {}

        else:

            print("Planning finished.")

            solution_trajectories = queue.get()

        # create path for solutions

        pathlib.Path(settings['output_path']).mkdir(parents=True, exist_ok=True)

        error = False

        cr_solution_writer = CommonRoadSolutionWriter(settings['output_path'], 

                                                      scenario.benchmark_id, 

                                                      scenario.dt,

                                                      vehicle_type, 

                                                      vehicle_model, 

                                                      cost_function)

        # inspect whether all planning problems are solved

        for planning_problem_id, planning_problem in planning_problem_set.planning_problem_dict.items():

            if planning_problem_id not in solution_trajectories.keys():

                warnings.warn('Solution for planning problem with ID={} is not provided for scenario {}. Solution skipped.'.format(

                    planning_problem_id, filename))

                error = True

                break

            else:

                cr_solution_writer.add_solution_trajectory(

                    solution_trajectories[planning_problem_id], planning_problem_id)

        if not error:

            cr_solution_writer.write_to_file(overwrite=settings['overwrite'])

            print("Solution written.")

        print("=========================================================")

 No newline at end of file

# all paths need to be specified relative to file batch_processing.py

# input directory of your intended CommonRoad scenarios

input_path: ../../scenarios/exercise/

# output directory for CommonRoad solution files

output_path: ../../solutions/

# overwrite solution file if it already exists

overwrite: True

# timeout time for trajectory planner [s]

timeout: 120

# name of function which calls the trajectory planner

# Inputs: scenario and planning problem set

# Output: dictionary, key: planning problem ID, value: commonroad trajectory

trajectory_planner_path: ./

trajectory_planner_module_name: batch_search

trajectory_planner_function_name: execute_search_batch

# benchmark evaluation default parameters

# change 'defult' to scenario id to specify exclusive paramters for that scenario

# e.g. change 'default' to 'USA_US101-6_2_T-1'

default:

  # vehicle model, e.g., kinematic single-track model

  vehicle_model: KS

    # vehicle type, e.g, BMW 320i

  vehicle_type: BMW_320i

    # cost function

  cost_function: SM1

# paths can be either relative or absolute

# input directory of your intended CommonRoad scenarios

input_path: ../../../scenarios/exercise

# output directory of CommonRoad solution files

output_path: ../../../solutions/

# overwrite solution file if it already exists

overwrite: True

# timeout time for motion planner [s]

timeout: 60

# name of the function which calls the motion planner

motion_planner_path: ../target/

motion_planner_module_name: batch_search

motion_planner_function_name: execute_search_batch

# benchmark evaluation parameters

# change 'defult' to scenario id to specify exclusive parameters for that scenario

# e.g. change 'default' to 'USA_US101-6_2_T-1'

default:

  # vehicle model, e.g., kinematic single-track model

  vehicle_model: KS

  # vehicle type, e.g, BMW 320i

  vehicle_type: BMW_320i

  # cost function

  cost_function: SM1

  # used to specify id of planner. in this example, we have

  # 1 = Greedy Best First Search, 2 = A*, else = Your own planner

  planner_id: 1

  # planner problem index. for cooperative scenarios: 0, 1, 2, ..., otherwise: 0

  planning_problem_idx: 0

  # maximum permissible depth of the search tree

  max_tree_depth: 100