Skip to content
GitLab
Commit 81dad22a authored by Edmond Irani Liu's avatar Edmond Irani Liu 🌊
Browse files

update description, add goal-less scenarios, add planner, change CF

parent d6f615c5
Hide whitespace changes
Inline Side-by-side
GSMP/motion_automata/automata/MotionPlanner_Astar.py
View file @ 81dad22a
......@@ -16,6 +16,7 @@ from typing import *
import warnings
import copy
# example A* planner
class MotionPlanner:
def __init__(self, scenario, planningProblem, automata):
......
GSMP/motion_automata/automata/MotionPlanner_gbfs.py
View file @ 81dad22a
......@@ -15,6 +15,7 @@ import heapq
import copy
from typing import *
# example Greedy Best First Search planner
class MotionPlanner:
def __init__(self, scenario, planningProblem, automata):
......
GSMP/motion_automata/automata/MotionPlanner_gbfs_only_time.py
View file @ 81dad22a
......@@ -15,6 +15,8 @@ import heapq
import copy
from typing import *
# example Greedy Best First Search planner with a heuristic function
# that only considers the time_step of the goal region
class MotionPlanner:
def __init__(self, scenario, planningProblem, automata):
......@@ -483,7 +485,7 @@ class MotionPlanner:
:param path: The path for which you want to calculate the cost
:param curPos: Last state of your current path. Every state has the following variables: position, velocity, orientation, time_step
"""
# a heuristic that considers time and goal lane (if any)
# a heuristic that only considers time of goal
if hasattr(self.planningProblem.goal.state_list[0], 'time_step'):
time_step_goal = (self.planningProblem.goal.state_list[0].time_step.start + \
self.planningProblem.goal.state_list[0].time_step.end) / 2
......
README.md
View file @ 81dad22a
......@@ -15,6 +15,7 @@ commonroad-search/
├batch_processing/
├motion_primitives_generator/
└tutorials/
├pdfs/
├scenarios/
├exercise/
└tutorial/
......@@ -36,7 +37,7 @@ docker run -it -p 9000:8888 --mount src="$(pwd)",target=/commonroad-search,type=
     and open the Jupyter Notebook by visiting `localhost:9000` in your web browser.
After you have set up your environment, please further proceed with `notebooks/tutorials/0_Guide_for_Exercise.pdf`.
After you have set up your environment, please further proceed with `pdfs/0_Guide_for_Exercise.pdf`.
## Installation guide
......@@ -81,12 +82,12 @@ As documented in CommonRoad-io [Documentation](https://commonroad.in.tum.de/stat
### 2. CommonRoad-Collision-Checker
Go to folder `GSMP/tools/commonroad-collision-checker/` and follow the instruction in README.rst. (You may navigate to it from this page by for a better rendering of the .rst file)
Go to folder `GSMP/tools/commonroad-collision-checker/` and follow the instruction in README.rst. (You may navigate to it from this page for a better rendering of the .rst file)
A tutorial of CommonRoad Collision Checker can be found [here](https://commonroad.in.tum.de/tutorials/).
### 3. CommonRoad-Road-Boundary
Go to folder `GSMP/tools/commonroad-road-boundary/` and follow the instruction in README.md. (You may navigate to it from this page by for a better rendering of the .md file) In case you face an error, refer to troubleshooting section.
Go to folder `GSMP/tools/commonroad-road-boundary/` and follow the instruction in README.md. (You may navigate to it from this page for a better rendering of the .md file) In case you face an error, refer to troubleshooting section.
## Tutorials
......@@ -95,7 +96,7 @@ Navigate your terminal to `commonroad-search/` folder, and start Jupyter Noteboo
$ jupyter notebook
```
In the prompt up page, navigate to `notebooks/tutorials/` and follow the tutorials `tutorial_commonroad-io.ipynb` and `tutorial_commonroad-search.ipynb`. Remember to refer to `tutorials/0_Guide_for_Exercise.pdf` for additional explanation. The executed Jupyter notebooks for tutorials can also be found [here](https://commonroad.in.tum.de/tutorials/).
In the prompt up page, navigate to `notebooks/tutorials/` and follow the tutorials `tutorial_commonroad-io.ipynb` and `tutorial_commonroad-search.ipynb`. Remember to refer to `pdfs/0_Guide_for_Exercise.pdf` for additional explanation. The executed Jupyter notebooks for tutorials can also be found [here](https://commonroad.in.tum.de/tutorials/).
## Implement your own search algorithm
......
notebooks/batch_processing/.ipynb_checkpoints/batch_processing-checkpoint.ipynb
View file @ 81dad22a
%% 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
### 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("=========================================================")
```
......
notebooks/batch_processing/batch_processing.ipynb
View file @ 81dad22a
%% 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
### 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("=========================================================")
```
......
notebooks/batch_processing/batch_processing_config.yaml
View file @ 81dad22a
......@@ -25,4 +25,4 @@ default:
# vehicle type, e.g, BMW 320i
vehicle_type: BMW_320i
# cost function
cost_function: JB1
cost_function: SM1
notebooks/batch_processing/batch_search.py
View file @ 81dad22a
......@@ -59,11 +59,14 @@ def execute_search_batch(scenario, planning_problem_set, veh_type_id=2,
motion_planner = MotionPlanner(scenario, planning_problem, automata)
print("Start search..")
time_start = time.process_time()
result = motion_planner.search_alg(initial_motion_primitive.Successors, max_tree_depth)
time_end = time.process_time()
print("Solving this scenario took {} seconds".format(round(time_end - time_start, 2)))
dict_result = {}
# result is in form of (final path, used_primitives)
if result is not None:
result_path = result[0]
list_state = list()
......
notebooks/tutorials/.ipynb_checkpoints/tutorial_commonroad-search-checkpoint.ipynb
View file @ 81dad22a
%% Cell type:markdown id: tags:
# Tutorial: CommonRoad Search
## Graph Search-Based Motion Planners with Motion Primitives
This tutorial shows how [CommonRoad Search](https://gitlab.lrz.de/tum-cps/commonroad-search), or **Graph Search-Based Motion Planner with Motion Primitives**, is used to search for trajectories that connect an **initial state** and a **goal region**.
%% Cell type:markdown id: tags:
## 0. Preparation
* Before you proceed, make sure you have read through the tutorial for [CommonRoad-io](https://commonroad.in.tum.de/static/docs/commonroad-io/index.html). Its tutorial can be found [here](https://commonroad.in.tum.de/tutorials/).
* Make sure you have installed all necessary modules for CommonRoad Search according to the installation manual.
* A documentation for the API of CommonRoad Search can be found [here](https://commonroad.in.tum.de/static/docs/commonroad-search/index.html).
* The API of CommonRoad-io can be found [here](https://commonroad.in.tum.de/static/docs/commonroad-io/api/index.html)
%% Cell type:markdown id: tags:
## 1. Load Python Environment
We first import necessary modules for motion planning, as well as some extensions for our convenience.
%% Cell type:code id: tags:
``` python
# enabling autoreload will reload modules automatically before entering the execution of code,
# so you could edit the code for your motion planner and execute it right away without reloading again
%load_ext autoreload
%autoreload 2
# always show execution time for each cell
%load_ext autotime
# use notebook to get interactive plots
%matplotlib notebook
```
%% Cell type:code id: tags:
``` python
# append main directory
import sys
sys.path.append("../../GSMP/motion_automata")
# load necessary modules and functions
from automata.HelperFunctions import *
```
%% Cell type:markdown id: tags:
Secondly, we specify the motion planner that we want to use:
%% Cell type:code id: tags:
``` python
# Uncomment the following to try out exemplary motion planners
# Greedy Best First Search
from automata.MotionPlanner_gbfs import MotionPlanner
# A* search
# from automata.MotionPlanner_Astar import MotionPlanner
# Uncomment the following to load your own motion planner
# from automata.MotionPlanner import MotionPlanner
```
%% Cell type:markdown id: tags:
## 2. Load Scenarios
We call automata.HelperFunctions.load_scenario() to load scenarios and planning problems. Scenarios beginning with letter C stand for cooeprative driving scenarios and possess multiple planning problems. In this case you should provide solutions to all planning problems.
%% Cell type:code id: tags:
``` python
scenario_path_prefix = '../../scenarios/exercise/'
scenario_id = 'USA_Lanker-1_2_T-1'
# construct file path
scenario_path = scenario_path_prefix + scenario_id + '.xml'
scenario, planning_problem_set = load_scenario(scenario_path)
# plot the scenario and planning problem set
plt.figure(figsize=(8, 8))
draw_object(scenario)
draw_object(planning_problem_set)
plt.gca().set_aspect('equal')
plt.margins(0,0)
plt.show()
```
%% Cell type:markdown id: tags:
## 3. Load Motion Primitives
We call automata.HelperFunctions.generate_automata() to generate a motion automata instance.
The variable veh_type_id specifies the id of the vehicle model that we want to use:
1: FORD_ESCORT
2: BMW320i
3: VW_VANAGON
%% Cell type:code id: tags:
``` python
veh_type_id = 2
if veh_type_id == 1: veh_type = VehicleType.FORD_ESCORT
elif veh_type_id == 2: veh_type = VehicleType.BMW_320i
elif veh_type_id == 3: veh_type = VehicleType.VW_VANAGON
automata = generate_automata(veh_type_id)
```
%% Cell type:markdown id: tags:
## 4. Initialize Motion Planner
In this step, we would like to configure the planning problem for the motion planner. Each **planning problem** has an individual **initial state** and a **goal region**.
%% Cell type:code id: tags:
``` python
# retrieve planning problem with given index (for cooperative scenario:0, 1, 2, ..., otherwise: 0)
planning_problem_idx = 0
planning_problem = list(planning_problem_set.planning_problem_dict.values())[planning_problem_idx]
# add initial state of planning problem to automata
automata, initial_motion_primitive = add_initial_state_to_automata(automata, planning_problem)
# construct motion planner.
motion_planner = MotionPlanner(scenario, planning_problem, automata)
```
%% Cell type:markdown id: tags:
## 5. Search for Solution
We call automata.HelperFunctions.start_search() to start searching for a feasible solution base on the given motion planner.
%% Cell type:code id: tags:
``` python
result_path, result_dict_status = start_search(scenario,
planning_problem,
automata,
motion_planner,
initial_motion_primitive)
```
%% Cell type:markdown id: tags:
## 6. Visualize Planned Trajectory
Given that we have found a feasible solution, we can also create an interactive visualizor for the solution.
We define two helper functions to retrieve state and its collision object at a given time step:
%% Cell type:code id: tags:
``` python
def get_state_at_time(t):
for state in result_path:
# return the first state that has time_step >= given t
if state.time_step >= t:
return state
# else return last state
return path[-1]
def draw_state(t):
print("current time step: ", t)
plt.clf()
draw_object(scenario, draw_params={'time_begin': t})
draw_object(planning_problem)
plt.gca().set_aspect('equal')
plt.gca().set_axis_off()
plt.margins(0,0)
plt.show()
if result_path is not None:
state = get_state_at_time(t)
trajectory = Trajectory(initial_time_step=int(state.time_step),state_list=[state])
prediction = TrajectoryPrediction(trajectory=trajectory, shape=automata.egoShape)
collision_object = create_collision_object(prediction)
draw_it(collision_object)
```
%% Cell type:code id: tags:
``` python
fig = plt.figure(figsize=(8,8))
# use the slider to have at look at every time step of the planned trajectory
widgets.interact(draw_state, t=widgets.IntSlider(min=0,max=result_path[-1].time_step,step=1,value=0))
```
%% Cell type:code id: tags:
``` python
# if you wish to visualize the final trajectory
if len(result_dict_status['path_current']) > 0:
plt.figure(figsize=(8, 8))
plt.clf()
draw_object(scenario)
draw_object(planning_problem)
plt.gca().set_aspect('equal')
plt.gca().set_axis_off()
plt.margins(0,0)
plt.show()
trajectory = Trajectory(0, result_dict_status['path_current'][0:])
prediction = TrajectoryPrediction(trajectory=trajectory, shape=automata.egoShape)
collision_object = create_collision_object(prediction)
draw_it(collision_object, draw_params={'collision': {'facecolor': 'green'}})
```
%% Cell type:code id: tags:
``` python
# if you wish to print out individual state
# for state in result_path:
# print(state)
```
%% Cell type:markdown id: tags:
## 7. Write Solution to CommonRoad XML File
The final step is writing your solution to the planning problem in the scenario into an XML file which is uploadable to our [Benchmark](https://commonroad.in.tum.de/submissions/create).
You may also refer to [Solution Writer](https://commonroad.in.tum.de/static/docs/commonroad-io/api/common.html#module-commonroad.common.solution_writer), [Cost Function](https://gitlab.lrz.de/tum-cps/commonroad-cost-functions/blob/master/costFunctions_commonRoad.pdf) and [Vehicle Model](https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/blob/master/vehicleModels_commonRoad.pdf) for additional information.
Note: In this tutorial we use Kinematic Single-Track Model.
%% Cell type:code id: tags:
``` python
list_state = list()
for state in result_path:
kwarg = {'position': state.position,
'velocity': state.velocity,
'steering_angle': state.steering_angle,
'orientation': state.orientation,
'time_step': state.time_step}
list_state.append(State(**kwarg))
trajectory = Trajectory(initial_time_step=list_state[0].time_step, state_list=list_state)
```
%% Cell type:code id: tags:
``` python
from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleModel, VehicleType, CostFunction
output_dir="../../solutions/"
# write solution to a xml file
csw = CommonRoadSolutionWriter(output_dir=output_dir, # output directory
scenario_id=scenario_id, # scenario id
step_size=0.1, # time step of scenario, typically 0.1
vehicle_type=veh_type, # vehicle type, change if needed
vehicle_model=VehicleModel.KS, # vehicle model, change if needed
cost_function=CostFunction.JB1) # cost funtion, change if needed
cost_function=CostFunction.SM1) # cost funtion, ** do not use JB1 **
# add trajectory solution
csw.add_solution_trajectory(trajectory=trajectory, planning_problem_id=planning_problem.planning_problem_id)
csw.write_to_file(overwrite=True)
```
%% Cell type:code id: tags:
``` python
```
......
notebooks/tutorials/tutorial_commonroad-search.ipynb
View file @ 81dad22a
%% Cell type:markdown id: tags:
# Tutorial: CommonRoad Search
## Graph Search-Based Motion Planners with Motion Primitives
This tutorial shows how [CommonRoad Search](https://gitlab.lrz.de/tum-cps/commonroad-search), or **Graph Search-Based Motion Planner with Motion Primitives**, is used to search for trajectories that connect an **initial state** and a **goal region**.
%% Cell type:markdown id: tags:
## 0. Preparation
* Before you proceed, make sure you have read through the tutorial for [CommonRoad-io](https://commonroad.in.tum.de/static/docs/commonroad-io/index.html). Its tutorial can be found [here](https://commonroad.in.tum.de/tutorials/).
* Make sure you have installed all necessary modules for CommonRoad Search according to the installation manual.
* A documentation for the API of CommonRoad Search can be found [here](https://commonroad.in.tum.de/static/docs/commonroad-search/index.html).
* The API of CommonRoad-io can be found [here](https://commonroad.in.tum.de/static/docs/commonroad-io/api/index.html)
%% Cell type:markdown id: tags:
## 1. Load Python Environment
We first import necessary modules for motion planning, as well as some extensions for our convenience.
%% Cell type:code id: tags:
``` python
# enabling autoreload will reload modules automatically before entering the execution of code,
# so you could edit the code for your motion planner and execute it right away without reloading again
%load_ext autoreload
%autoreload 2
# always show execution time for each cell
%load_ext autotime
# use notebook to get interactive plots
%matplotlib notebook
```
%% Cell type:code id: tags:
``` python
# append main directory
import sys
sys.path.append("../../GSMP/motion_automata")
# load necessary modules and functions
from automata.HelperFunctions import *
```
%% Cell type:markdown id: tags:
Secondly, we specify the motion planner that we want to use:
%% Cell type:code id: tags:
``` python
# Uncomment the following to try out exemplary motion planners
# Greedy Best First Search
from automata.MotionPlanner_gbfs import MotionPlanner
# A* search
# from automata.MotionPlanner_Astar import MotionPlanner
# Uncomment the following to load your own motion planner
# from automata.MotionPlanner import MotionPlanner
```
%% Cell type:markdown id: tags:
## 2. Load Scenarios
We call automata.HelperFunctions.load_scenario() to load scenarios and planning problems. Scenarios beginning with letter C stand for cooeprative driving scenarios and possess multiple planning problems. In this case you should provide solutions to all planning problems.
%% Cell type:code id: tags:
``` python
scenario_path_prefix = '../../scenarios/exercise/'
scenario_id = 'USA_Lanker-1_2_T-1'
# construct file path
scenario_path = scenario_path_prefix + scenario_id + '.xml'
scenario, planning_problem_set = load_scenario(scenario_path)
# plot the scenario and planning problem set
plt.figure(figsize=(8, 8))
draw_object(scenario)
draw_object(planning_problem_set)
plt.gca().set_aspect('equal')
plt.margins(0,0)
plt.show()
```
%% Cell type:markdown id: tags:
## 3. Load Motion Primitives
We call automata.HelperFunctions.generate_automata() to generate a motion automata instance.
The variable veh_type_id specifies the id of the vehicle model that we want to use:
1: FORD_ESCORT
2: BMW320i
3: VW_VANAGON
%% Cell type:code id: tags:
``` python
veh_type_id = 2
if veh_type_id == 1: veh_type = VehicleType.FORD_ESCORT
elif veh_type_id == 2: veh_type = VehicleType.BMW_320i
elif veh_type_id == 3: veh_type = VehicleType.VW_VANAGON
automata = generate_automata(veh_type_id)
```
%% Cell type:markdown id: tags:
## 4. Initialize Motion Planner
In this step, we would like to configure the planning problem for the motion planner. Each **planning problem** has an individual **initial state** and a **goal region**.
%% Cell type:code id: tags:
``` python
# retrieve planning problem with given index (for cooperative scenario:0, 1, 2, ..., otherwise: 0)
planning_problem_idx = 0
planning_problem = list(planning_problem_set.planning_problem_dict.values())[planning_problem_idx]
# add initial state of planning problem to automata
automata, initial_motion_primitive = add_initial_state_to_automata(automata, planning_problem)
# construct motion planner.
motion_planner = MotionPlanner(scenario, planning_problem, automata)
```
%% Cell type:markdown id: tags:
## 5. Search for Solution
We call automata.HelperFunctions.start_search() to start searching for a feasible solution base on the given motion planner.
%% Cell type:code id: tags:
``` python
result_path, result_dict_status = start_search(scenario,
planning_problem,
automata,
motion_planner,
initial_motion_primitive)
```
%% Cell type:markdown id: tags:
## 6. Visualize Planned Trajectory
Given that we have found a feasible solution, we can also create an interactive visualizor for the solution.
We define two helper functions to retrieve state and its collision object at a given time step:
%% Cell type:code id: tags:
``` python
def get_state_at_time(t):
for state in result_path:
# return the first state that has time_step >= given t
if state.time_step >= t:
return state
# else return last state
return path[-1]
def draw_state(t):
print("current time step: ", t)
plt.clf()
draw_object(scenario, draw_params={'time_begin': t})
draw_object(planning_problem)
plt.gca().set_aspect('equal')
plt.gca().set_axis_off()
plt.margins(0,0)
plt.show()
if result_path is not None:
state = get_state_at_time(t)
trajectory = Trajectory(initial_time_step=int(state.time_step),state_list=[state])
prediction = TrajectoryPrediction(trajectory=trajectory, shape=automata.egoShape)
collision_object = create_collision_object(prediction)
draw_it(collision_object)
```
%% Cell type:code id: tags:
``` python
fig = plt.figure(figsize=(8,8))
# use the slider to have at look at every time step of the planned trajectory
widgets.interact(draw_state, t=widgets.IntSlider(min=0,max=result_path[-1].time_step,step=1,value=0))
```
%% Cell type:code id: tags:
``` python
# if you wish to visualize the final trajectory
if len(result_dict_status['path_current']) > 0:
plt.figure(figsize=(8, 8))
plt.clf()
draw_object(scenario)
draw_object(planning_problem)
plt.gca().set_aspect('equal')
plt.gca().set_axis_off()
plt.margins(0,0)
plt.show()
trajectory = Trajectory(0, result_dict_status['path_current'][0:])
prediction = TrajectoryPrediction(trajectory=trajectory, shape=automata.egoShape)
collision_object = create_collision_object(prediction)
draw_it(collision_object, draw_params={'collision': {'facecolor': 'green'}})
```
%% Cell type:code id: tags:
``` python
# if you wish to print out individual state
# for state in result_path:
# print(state)
```
%% Cell type:markdown id: tags:
## 7. Write Solution to CommonRoad XML File
The final step is writing your solution to the planning problem in the scenario into an XML file which is uploadable to our [Benchmark](https://commonroad.in.tum.de/submissions/create).
You may also refer to [Solution Writer](https://commonroad.in.tum.de/static/docs/commonroad-io/api/common.html#module-commonroad.common.solution_writer), [Cost Function](https://gitlab.lrz.de/tum-cps/commonroad-cost-functions/blob/master/costFunctions_commonRoad.pdf) and [Vehicle Model](https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/blob/master/vehicleModels_commonRoad.pdf) for additional information.
Note: In this tutorial we use Kinematic Single-Track Model.
%% Cell type:code id: tags:
``` python
list_state = list()
for state in result_path:
kwarg = {'position': state.position,
'velocity': state.velocity,
'steering_angle': state.steering_angle,
'orientation': state.orientation,
'time_step': state.time_step}
list_state.append(State(**kwarg))
trajectory = Trajectory(initial_time_step=list_state[0].time_step, state_list=list_state)
```
%% Cell type:code id: tags:
``` python
from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleModel, VehicleType, CostFunction
output_dir="../../solutions/"
# write solution to a xml file
csw = CommonRoadSolutionWriter(output_dir=output_dir, # output directory
scenario_id=scenario_id, # scenario id
step_size=0.1, # time step of scenario, typically 0.1
vehicle_type=veh_type, # vehicle type, change if needed
vehicle_model=VehicleModel.KS, # vehicle model, change if needed
cost_function=CostFunction.JB1) # cost funtion, change if needed
cost_function=CostFunction.SM1) # cost funtion, ** do not use JB1 **
# add trajectory solution
csw.add_solution_trajectory(trajectory=trajectory, planning_problem_id=planning_problem.planning_problem_id)
csw.write_to_file(overwrite=True)
```
%% Cell type:code id: tags:
``` python
```
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment

LRZ Homepage | Datenschutz | Dokumentation und Betriebsbedingungen | Impressum