Commit 11ed2f46 authored by Sebastian Maierhofer's avatar Sebastian Maierhofer
Browse files

Add informed/uninformed search tutorials

parent 820b450b
Tutorials on Tree Search Algorithms
============
This is a short guide on how to use the search algorithms in tree_search.py for lectures and tutorials.
Set up
============
For using the code, you have to install the tools as described in the README in the root folder. For using the code in
Pycharm, create a new project in the folder `./GSMP/motion_automata`, to avoid path issues.
# Documentation
For running the search algorithms, you can currently specify three different configurations which mainly influence the
visualization of the search and are defined in `config.py`:
### Default:
* This configuration should only be used in the jupyter notebook tutorial (`cr_uninformed_search_tutorial` and
`cr_informed_search_tutorial`). It first runs the algorithm and stores the state of
each motion primitive, i.e., explored/frontier/currently exploring etc., for each step. Then it plots these steps using
ipywidgets, so the student can step through the search in the jupyter notebook.
* The color code for plotting is taken from the AIMA tutorial, but can be changed in `config.py`.
* By setting PLOT_LEGEND to True, a legend with all states of
motion primitives (see `MotionPrimitiveStatus` in `helper_tree_search.py`) is plotted.
* To reduce the number of steps, you can turn off that motion primitives with collisions are plotted one after the other by setting PLOT_COLLISION_STEPS
to False.
### StudentScript:
This configuration uses the same parameters as the Default configuration, but it can be run in Pycharm,
e.g. when executing `main.py`. It plots the different steps while executing the search algorithm. It should help
students to easily go through the code.
...@@ -7,9 +7,10 @@ sys.path.append(os.path.join(path_commonroad_search, "GSMP/tools/commonroad-coll ...@@ -7,9 +7,10 @@ sys.path.append(os.path.join(path_commonroad_search, "GSMP/tools/commonroad-coll
sys.path.append(os.path.join(path_commonroad_search, "GSMP/tools/commonroad-road-boundary")) 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")) sys.path.append(os.path.join(path_commonroad_search, "GSMP/motion_automata/vehicle_model"))
import time import time
from multiprocessing import Manager, Process from multiprocessing import Manager, Process
import enum
import ipywidgets as widgets
import numpy as np import numpy as np
from math import atan2 from math import atan2
...@@ -17,12 +18,16 @@ from math import atan2 ...@@ -17,12 +18,16 @@ from math import atan2
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from IPython import display from IPython import display
from ipywidgets import widgets from ipywidgets import widgets
from IPython.display import display
# import CommonRoad-io modules # import CommonRoad-io modules
from commonroad.visualization.draw_dispatch_cr import draw_object from commonroad.visualization.draw_dispatch_cr import draw_object
from commonroad.common.file_reader import CommonRoadFileReader from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.scenario.trajectory import Trajectory, State from commonroad.scenario.trajectory import Trajectory, State
from commonroad.scenario.scenario import Scenario
from commonroad.geometry.shape import Rectangle from commonroad.geometry.shape import Rectangle
from commonroad.planning.planning_problem import PlanningProblem
from commonroad.scenario.obstacle import StaticObstacle, ObstacleRole, ObstacleType, DynamicObstacle
from commonroad.prediction.prediction import TrajectoryPrediction from commonroad.prediction.prediction import TrajectoryPrediction
from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_object from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_object
from commonroad_cc.visualization.draw_dispatch import draw_object as draw_it from commonroad_cc.visualization.draw_dispatch import draw_object as draw_it
...@@ -32,6 +37,8 @@ from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleT ...@@ -32,6 +37,8 @@ from commonroad.common.solution_writer import CommonRoadSolutionWriter, VehicleT
from automata.MotionAutomata import MotionAutomata from automata.MotionAutomata import MotionAutomata
from automata.MotionPrimitive import MotionPrimitive from automata.MotionPrimitive import MotionPrimitive
from automata.States import FinalState from automata.States import FinalState
from typing import List, Dict, Tuple
# ================================== scenario ================================== # # ================================== scenario ================================== #
def load_scenario(scenario_path): def load_scenario(scenario_path):
...@@ -42,8 +49,8 @@ def load_scenario(scenario_path): ...@@ -42,8 +49,8 @@ def load_scenario(scenario_path):
# ================================== automata ================================== # # ================================== automata ================================== #
def generate_automata(veh_type_id: int): def generate_automata(veh_type_id, mp_file=None, search_tutorial=False):
""" """
1. We first create a Motion Automata object, then load pre-computed motion primitives into it. 1. We first create a Motion Automata object, then load pre-computed motion primitives into it.
Note that every motion primitive is a short trajectory with varied initial states that lead to different final states. Note that every motion primitive is a short trajectory with varied initial states that lead to different final states.
...@@ -51,10 +58,15 @@ def generate_automata(veh_type_id: int): ...@@ -51,10 +58,15 @@ def generate_automata(veh_type_id: int):
which contains all connectable primitives from the focused primitive. which contains all connectable primitives from the focused primitive.
If the velocity and steering angel of the start state of primitive B match If the velocity and steering angel of the start state of primitive B match
those of the final state of primitive A, we say B is connectable from A. those of the final state of primitive A, we say B is connectable from A.
:param veh_type_id: id for the vehicle model in the motion primitives
:param mp_file: optional parameter for specifying the path to a motion primitives xml file
:param search_tutorial: boolean value to specify if the search is used for a tutorial ->
motion primitives are sorted by their y-coordinate in the final state
""" """
# the primitives vary for different vehicle models 1, 2 and 3. # the primitives vary for different vehicle models 1, 2 and 3.
assert veh_type_id in (1,2,3), "Input vehicle type id is not valid! Must be either 1, 2 or 3." assert veh_type_id in (1, 2, 3), "Input vehicle type id is not valid! Must be either 1, 2 or 3."
# step 1 # step 1
automata = MotionAutomata(veh_type_id) automata = MotionAutomata(veh_type_id)
...@@ -62,22 +74,38 @@ def generate_automata(veh_type_id: int): ...@@ -62,22 +74,38 @@ def generate_automata(veh_type_id: int):
print("Reading motion primitives...") print("Reading motion primitives...")
try: try:
prefix = '../../GSMP/motion_automata/motion_primitives/' prefix = '../../GSMP/motion_automata/motion_primitives/'
if mp_file is not None:
automata.readFromXML(prefix + mp_file)
else:
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')
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: except Exception:
prefix = '../../../GSMP/motion_automata/motion_primitives/' prefix = '../../../GSMP/motion_automata/motion_primitives/'
if veh_type_id == 1: if mp_file is not None:
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') automata.readFromXML(prefix + mp_file)
elif veh_type_id == 2: else:
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') if veh_type_id == 1:
elif veh_type_id == 3: automata.readFromXML(
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') 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')
# additional step for search examples -> sorting motion primitives by max. y-coordinate
if search_tutorial:
automata.sort_primitives()
# step 2 # step 2
automata.createConnectivityLists() automata.createConnectivityLists()
...@@ -90,36 +118,34 @@ def generate_automata(veh_type_id: int): ...@@ -90,36 +118,34 @@ def generate_automata(veh_type_id: int):
return automata return automata
def add_initial_state_to_automata(automata, planning_problem, flag_print_states = True):
def add_initial_state_to_automata(automata, planning_problem):
# set initial steering angle to zero # set initial steering angle to zero
planning_problem.initial_state.steering_angle = 0.0 planning_problem.initial_state.steering_angle = 0.0
# the initial velocity of the planning problem may be any value, we need to obtain the closest velocity to it # the initial velocity of the planning problem may be any value, we need to obtain the closest velocity to it
# from StartStates of the primitives in order to get the feasible successors of the planning problem # from StartStates of the primitives in order to get the feasible successors of the planning problem
velocity_closest = automata.getClosestStartVelocity(planning_problem.initial_state.velocity) velocity_closest = automata.getClosestStartVelocity(planning_problem.initial_state.velocity)
planning_problem.initial_state.velocity = velocity_closest planning_problem.initial_state.velocity = velocity_closest
initial_state = planning_problem.initial_state initial_state = planning_problem.initial_state
goal_region = planning_problem.goal.state_list[0]
# if flag_print_states: # turn initial state into a motion primitive to check for connectivity to subsequent motion primitives
# print("Initial State:", initial_state) final_state_primitive = FinalState(x=initial_state.position[0],
# print("Goal Region:", goal_region) y=initial_state.position[1],
# turn intial state into a motion primitive to check for connectivity to subsequent motion primitives
final_state_primitive = FinalState(x=initial_state.position[0],
y=initial_state.position[1],
steering_angle=initial_state.steering_angle, steering_angle=initial_state.steering_angle,
velocity=initial_state.velocity, velocity=initial_state.velocity,
orientation=initial_state.orientation, orientation=initial_state.orientation,
time_step=initial_state.time_step) time_step=initial_state.time_step)
initial_motion_primitive = MotionPrimitive(startState=None, finalState=final_state_primitive, timeStepSize=0, trajectory=None) initial_motion_primitive = MotionPrimitive(startState=None, finalState=final_state_primitive, timeStepSize=0,
trajectory=None)
# create connectivity list for this imaginary motion primitive
automata.createConnectivityListPrimitive(initial_motion_primitive)
# create connectivity list for this imaginary motion primitive return automata, initial_motion_primitive
automata.createConnectivityListPrimitive(initial_motion_primitive)
return automata, initial_motion_primitive
# ================================== search for solution ================================== # # ================================== search for solution ================================== #
""" """
...@@ -133,6 +159,7 @@ More information on python Prcess could be found at [1](https://docs.python.org/ ...@@ -133,6 +159,7 @@ More information on python Prcess could be found at [1](https://docs.python.org/
3. To execute the search, we need to define an individual wrapper function and set it as the target of the new Process. 3. To execute the search, we need to define an individual wrapper function and set it as the target of the new Process.
""" """
def execute_search(motion_planner, initial_motion_primitive, path_shared, dict_status_shared, max_tree_depth): def execute_search(motion_planner, initial_motion_primitive, path_shared, dict_status_shared, max_tree_depth):
""" """
definition of the wrapper function for search definition of the wrapper function for search
...@@ -148,49 +175,51 @@ def execute_search(motion_planner, initial_motion_primitive, path_shared, dict_s ...@@ -148,49 +175,51 @@ def execute_search(motion_planner, initial_motion_primitive, path_shared, dict_s
else: else:
display.clear_output(wait=True) display.clear_output(wait=True)
path_shared.value = result[0] path_shared.value = result[0]
if len(result[1]) > 0: if len(result[1]) > 0:
print("Found primitives") print("Found primitives")
# for primitive in result[1]: # for primitive in result[1]:
# print('\t', primitive) # print('\t', primitive)
def start_search(scenario, planning_problem, automata, motion_planner, initial_motion_primitive, flag_plot_intermediate_results=True, flag_plot_planning_problem=True):
def start_search(scenario, planning_problem, automata, motion_planner, initial_motion_primitive,
flag_plot_intermediate_results=True, flag_plot_planning_problem=True):
# create Manager object to manage shared variables between different Processes # create Manager object to manage shared variables between different Processes
process_manager = Manager() process_manager = Manager()
# create shared variables between Processes # create shared variables between Processes
path_shared = process_manager.Value('path_shared', None) path_shared = process_manager.Value('path_shared', None)
dict_status_shared = process_manager.Value('dict_status_shared', None) dict_status_shared = process_manager.Value('dict_status_shared', None)
# initialize status with a dictionary. # initialize status with a dictionary.
# IMPORTANT NOTE: if the shared variable contains complex object types (list, dict, ...), to change its value, # IMPORTANT NOTE: if the shared variable contains complex object types (list, dict, ...), to change its value,
# we need to do it via another object (here dict_status) and pass it to the shared variable at the end # we need to do it via another object (here dict_status) and pass it to the shared variable at the end
dict_status = {'cost_current': 0, 'path_current':[]} dict_status = {'cost_current': 0, 'path_current': []}
dict_status_shared.value = dict_status dict_status_shared.value = dict_status
# maximum number of concatenated primitives # maximum number of concatenated primitives
max_tree_depth = 100 max_tree_depth = 100
# create and start the Process for search # create and start the Process for search
process_search = Process(target=execute_search, process_search = Process(target=execute_search,
args=(motion_planner, args=(motion_planner,
initial_motion_primitive, initial_motion_primitive,
path_shared, path_shared,
dict_status_shared, dict_status_shared,
max_tree_depth)) max_tree_depth))
process_search.start() process_search.start()
if flag_plot_intermediate_results: if flag_plot_intermediate_results:
# create a figure, plot the scenario and planning problem # create a figure, plot the scenario and planning problem
fig = plt.figure(figsize=(9, 9)) fig = plt.figure(figsize=(9, 9))
plt.clf() plt.clf()
draw_object(scenario) draw_object(scenario)
if flag_plot_planning_problem: if flag_plot_planning_problem:
draw_object(planning_problem) draw_object(planning_problem)
plt.gca().set_aspect('equal') plt.gca().set_aspect('equal')
plt.gca().set_axis_off() plt.gca().set_axis_off()
plt.margins(0,0) plt.margins(0, 0)
plt.show() plt.show()
refresh_rate_plot = 5.0 refresh_rate_plot = 5.0
...@@ -200,14 +229,14 @@ def start_search(scenario, planning_problem, automata, motion_planner, initial_m ...@@ -200,14 +229,14 @@ def start_search(scenario, planning_problem, automata, motion_planner, initial_m
# break the loop if process_search is not alive anymore # break the loop if process_search is not alive anymore
if not process_search.is_alive(): if not process_search.is_alive():
break break
# print current status # print current status
string_status = "Cost so far:", round(dict_status_shared.value['cost_current'], 2) string_status = "Cost so far:", round(dict_status_shared.value['cost_current'], 2)
string_status += "Time step:", round(dict_status_shared.value['path_current'][-1].time_step, 2) string_status += "Time step:", round(dict_status_shared.value['path_current'][-1].time_step, 2)
string_status += "Orientation:",round(dict_status_shared.value['path_current'][-1].orientation, 2) string_status += "Orientation:", round(dict_status_shared.value['path_current'][-1].orientation, 2)
string_status += "Velocity:", round(dict_status_shared.value['path_current'][-1].velocity, 2) string_status += "Velocity:", round(dict_status_shared.value['path_current'][-1].velocity, 2)
print (string_status, end='\r') print(string_status, end='\r')
# if there is a path to be visualized # if there is a path to be visualized
if len(dict_status_shared.value['path_current']) > 0: if len(dict_status_shared.value['path_current']) > 0:
if flag_plot_intermediate_results: if flag_plot_intermediate_results:
...@@ -217,28 +246,28 @@ def start_search(scenario, planning_problem, automata, motion_planner, initial_m ...@@ -217,28 +246,28 @@ def start_search(scenario, planning_problem, automata, motion_planner, initial_m
draw_object(planning_problem) draw_object(planning_problem)
plt.gca().set_aspect('equal') plt.gca().set_aspect('equal')
plt.gca().set_axis_off() plt.gca().set_axis_off()
plt.margins(0,0) plt.margins(0, 0)
plt.show() plt.show()
# create a Trajectory from the current states of the path # create a Trajectory from the current states of the path
# Trajectory is essentially a list of states # Trajectory is essentially a list of states
trajectory = Trajectory(initial_time_step=0, state_list=dict_status_shared.value['path_current']) trajectory = Trajectory(initial_time_step=0, state_list=dict_status_shared.value['path_current'])
# create an occupancy Prediction via the generated trajectory and shape of ego vehicle # create an occupancy Prediction via the generated trajectory and shape of ego vehicle
# the Prediction includes a Trajectory and Shape of the object # the Prediction includes a Trajectory and Shape of the object
prediction = TrajectoryPrediction(trajectory=trajectory, shape=automata.egoShape) prediction = TrajectoryPrediction(trajectory=trajectory, shape=automata.egoShape)
# create a colission object from prediction # create a collision object from prediction
# here it is used for visualization purpose # here it is used for visualization purpose
collision_object = create_collision_object(prediction) collision_object = create_collision_object(prediction)
# draw this collision object # draw this collision object
draw_it(collision_object, draw_params={'collision': {'facecolor': 'magenta'}}) draw_it(collision_object, draw_params={'collision': {'facecolor': 'magenta'}})
# visualize current trajectory # visualize current trajectory
fig.canvas.draw() fig.canvas.draw()
# wait till process_search terminates # wait till process_search terminates
process_search.join() process_search.join()
time.sleep(0.5) time.sleep(0.5)
print("Search finished.") print("Search finished.")
...@@ -250,22 +279,5 @@ def start_search(scenario, planning_problem, automata, motion_planner, initial_m ...@@ -250,22 +279,5 @@ def start_search(scenario, planning_problem, automata, motion_planner, initial_m
return path_shared.value, dict_status_shared.value return path_shared.value, dict_status_shared.value
# ================================== visualization ================================== #
def get_state_at_time(t):
for state in path_shared.value:
# 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)
draw_figure()
if path_shared 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)
\ No newline at end of file
...@@ -5,6 +5,7 @@ from automata.MotionPrimitiveParser import MotionPrimitiveParser ...@@ -5,6 +5,7 @@ from automata.MotionPrimitiveParser import MotionPrimitiveParser
from automata.MotionPrimitive import MotionPrimitive from automata.MotionPrimitive import MotionPrimitive
from commonroad.geometry.shape import Rectangle from commonroad.geometry.shape import Rectangle
class MotionAutomata: class MotionAutomata:
""" """
Class to handle motion primitives for motion planning Class to handle motion primitives for motion planning
...@@ -39,6 +40,9 @@ class MotionAutomata: ...@@ -39,6 +40,9 @@ class MotionAutomata:
self.setVehicleTypeIdPrimitives() self.setVehicleTypeIdPrimitives()
return return
def sort_primitives(self):
self.Primitives.sort(key=lambda x: x.finalState.y, reverse=False)
def createConnectivityListPrimitive(self, primitive: MotionPrimitive) -> None: def createConnectivityListPrimitive(self, primitive: MotionPrimitive) -> None:
""" """
Creates the successor list for a single primitive and stores them in a successor list of the given primitive. Creates the successor list for a single primitive and stores them in a successor list of the given primitive.
......
...@@ -24,7 +24,7 @@ class MotionPrimitiveParser: ...@@ -24,7 +24,7 @@ class MotionPrimitiveParser:
startSteeringAngle = float(startNode.find('steering_angle').text) startSteeringAngle = float(startNode.find('steering_angle').text)
startVelocity = float(startNode.find('velocity').text) startVelocity = float(startNode.find('velocity').text)
startOrientation = float(startNode.find('orientation').text) startOrientation = float(startNode.find('orientation').text)
startTimeStep = float(startNode.find('time_step').text) startTimeStep = int(startNode.find('time_step').text)
startState = StartState(startX, startY, startSteeringAngle, startVelocity, startOrientation, startTimeStep) startState = StartState(startX, startY, startSteeringAngle, startVelocity, startOrientation, startTimeStep)
...@@ -35,13 +35,14 @@ class MotionPrimitiveParser: ...@@ -35,13 +35,14 @@ class MotionPrimitiveParser:
finalSteeringAngle = float(finalNode.find('steering_angle').text) finalSteeringAngle = float(finalNode.find('steering_angle').text)
finalVelocity = float(finalNode.find('velocity').text) finalVelocity = float(finalNode.find('velocity').text)
finalOrientation = float(finalNode.find('orientation').text) finalOrientation = float(finalNode.find('orientation').text)
finalTimeStep = float(finalNode.find('time_step').text) finalTimeStep = int(finalNode.find('time_step').text)
finalState = FinalState(finalX, finalY, finalSteeringAngle, finalVelocity, finalOrientation, finalTimeStep) finalState = FinalState(finalX, finalY, finalSteeringAngle, finalVelocity, finalOrientation, finalTimeStep)
# create trajectory from path node and start/final states # create trajectory from path node and start/final states
pathNode = xmlNode.find('Path') pathNode = xmlNode.find('Path')
timeStepSize = 1.0 / (len(pathNode) + 1) duration = xmlNode.find('Duration')
timeStepSize = float(duration.text) / (len(pathNode) + 1)
trajectory = MotionPrimitiveParser.createTrajectoryFromPathStates(pathNode, startState, finalState, timeStepSize) trajectory = MotionPrimitiveParser.createTrajectoryFromPathStates(pathNode, startState, finalState, timeStepSize)
......
__author__ = "Anna-Katharina Rettinger"
__copyright__ = "TUM Cyber-Physical Systems Group"
__credits__ = ["CoPlan"]
__version__ = "0.1"
__maintainer__ = "Anna-Katharina Rettinger"
__email__ = "commonroad-i06@in.tum.de"
__status__ = "Beta"
import enum
import ipywidgets as widgets
import matplotlib.pyplot as plt
from matplotlib.lines import Line2D
from IPython import display
from ipywidgets import widgets
from IPython.display import display
# import CommonRoad-io modules
from commonroad.visualization.draw_dispatch_cr import draw_object
from commonroad.scenario.trajectory import State
from commonroad.scenario.scenario import Scenario
from commonroad.geometry.shape import Rectangle
from commonroad.planning.planning_problem import PlanningProblem
from commonroad.scenario.obstacle import ObstacleType, DynamicObstacle
# import Motion Automaton modules
from automata.MotionPrimitive import MotionPrimitive
from typing import List, Dict, Tuple
@enum.unique
class MotionPrimitiveStatus(enum.Enum):
IN_FRONTIER = 0
INVALID = 1
CURRENTLY_EXPLORED = 2
EXPLORED = 3
SOLUTION = 4
def plot_legend(plotting_config):
if hasattr(plotting_config, 'LABELS'):
node_status, labels = plotting_config.LABELS
else:
node_status = [status.value for status in MotionPrimitiveStatus]
labels = ['Frontier', 'Collision', 'Currently Exploring', 'Explored', 'Final Solution']
custom_lines = []
for value in node_status:
custom_lines.append(Line2D([0], [0], color=plotting_config.PLOTTING_PARAMS[value][0],
linestyle=plotting_config.PLOTTING_PARAMS[value][1],
linewidth=plotting_config.PLOTTING_PARAMS[value][2]))
legend = plt.legend(handles=custom_lines, labels=labels, loc='lower left',
bbox_to_anchor=(0.02, 0.51), prop={'size': 18})
legend.set_zorder(30)
plt.rcParams["legend.framealpha"] = 1.0
plt.rcParams["legend.shadow"] = True
def plot_search_scenario(scenario, initial_state, ego_shape, planning_problem, config):
plt.figure(figsize=(22.5, 4.5))
plt.axis('equal')
plt.xlim(55, 97)
plt.ylim(-2.5, 5.5)
draw_params = {'scenario': {'lanelet': {'facecolor': '#F8F8F8'}}}
draw_object(scenario, draw_params=draw_params)