Commit 73ab8e0a authored by Edmond Irani Liu's avatar Edmond Irani Liu 🏂
Browse files

version for programming exercise

parent 4a237c6c
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
import time
from multiprocessing import Manager, Process
import numpy as np
from math import atan2
import matplotlib.pyplot as plt
from IPython import display
from ipywidgets import widgets
# import CommonRoad-io modules
from commonroad.visualization.draw_dispatch_cr import draw_object
from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.scenario.trajectory import Trajectory, State
from commonroad.geometry.shape import Rectangle
from commonroad.prediction.prediction import TrajectoryPrediction
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.common.solution_writer import CommonRoadSolutionWriter, VehicleType, CostFunction
# import Motion Automata modules
from automata.MotionAutomata import MotionAutomata
from automata.MotionPrimitive import MotionPrimitive
from automata.States import FinalState
# ================================== scenario ================================== #
def load_scenario(scenario_path):
# open and read in scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(scenario_path).open()
return scenario, planning_problem_set
# ================================== automata ================================== #
def generate_automata(veh_type_id: int):
"""
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.
2. We create a connectivity list for every pritimive in the Motion Automata,
which contains all connectable primitives from the focused primitive.
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.
"""
# 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."
# step 1
automata = MotionAutomata(veh_type_id)
prefix = '../../GSMP/motion_automata/motion_primitives/'
print("Reading 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')
# step 2
automata.createConnectivityLists()
# assign vehicle type id to all primitives
automata.setVehicleTypeIdPrimitives()
print("Automata created.")
print('Number of loaded primitives: ' + str(len(automata.Primitives)))
return automata
def add_initial_state_to_automata(automata, planning_problem, flag_print_states = True):
# set initial steering angle to zero
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
# 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)
planning_problem.initial_state.velocity = velocity_closest
initial_state = planning_problem.initial_state
goal_region = planning_problem.goal.state_list[0]
# if flag_print_states:
# print("Initial State:", initial_state)
# print("Goal Region:", goal_region)
# 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,
velocity=initial_state.velocity,
orientation=initial_state.orientation,
time_step=initial_state.time_step)
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)
return automata, initial_motion_primitive
# ================================== search for solution ================================== #
"""
1. Searching for a possible solution could be time consuming, and also we would like to visualize the intermediate states during the search.
Thus, we create a separate Process to handle the search, and visualize the intermediate states via Main Process.
More information on python Prcess could be found at [1](https://docs.python.org/3/library/multiprocessing.html) and
[2](http://www.blog.pythonlibrary.org/2016/08/02/python-201-a-multiprocessing-tutorial/).
2. We need multiprocessing.Manager.Value() to help us share variables between different Processes.
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):
"""
definition of the wrapper function for search
calls search algorithm and update the status dictionary for visualization in search algorithm
note that dict_status_shared is being shared between difference Processes
"""
result = motion_planner.search_alg(initial_motion_primitive.Successors, max_tree_depth, dict_status_shared)
# result is in form of (final path, used_primitives)
if result is None:
# no path found
return
else:
display.clear_output(wait=True)
path_shared.value = result[0]
if len(result[1]) > 0:
print("Found primitives")
# for primitive in result[1]:
# 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):
# create Manager object to manage shared variables between different Processes
process_manager = Manager()
# create shared variables between Processes
path_shared = process_manager.Value('path_shared', None)
dict_status_shared = process_manager.Value('dict_status_shared', None)
# initialize status with a dictionary.
# 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
dict_status = {'cost_current': 0, 'path_current':[]}
dict_status_shared.value = dict_status
# maximum number of concatenated primitives
max_tree_depth = 100
# create and start the Process for search
process_search = Process(target=execute_search,
args=(motion_planner,
initial_motion_primitive,
path_shared,
dict_status_shared,
max_tree_depth))
process_search.start()
if flag_plot_intermediate_results:
# create a figure, plot the scenario and planning problem
fig = plt.figure(figsize=(9, 9))
plt.clf()
draw_object(scenario)
if flag_plot_planning_problem:
draw_object(planning_problem)
plt.gca().set_aspect('equal')
plt.gca().set_axis_off()
plt.margins(0,0)
plt.show()
refresh_rate_plot = 5.0
while True:
time.sleep(1 / refresh_rate_plot)
# break the loop if process_search is not alive anymore
if not process_search.is_alive():
break
# print current status
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 += "Orientation:",round(dict_status_shared.value['path_current'][-1].orientation, 2)
string_status += "Velocity:", round(dict_status_shared.value['path_current'][-1].velocity, 2)
print (string_status, end='\r')
# if there is a path to be visualized
if len(dict_status_shared.value['path_current']) > 0:
if flag_plot_intermediate_results:
plt.clf()
draw_object(scenario)
if flag_plot_planning_problem:
draw_object(planning_problem)
plt.gca().set_aspect('equal')
plt.gca().set_axis_off()
plt.margins(0,0)
plt.show()
# create a Trajectory from the current states of the path
# Trajectory is essentially a list of states
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
# the Prediction includes a Trajectory and Shape of the object
prediction = TrajectoryPrediction(trajectory=trajectory, shape=automata.egoShape)
# create a colission object from prediction
# here it is used for visualization purpose
collision_object = create_collision_object(prediction)
# draw this collision object
draw_it(collision_object, draw_params={'collision': {'facecolor': 'magenta'}})
# visualize current trajectory
fig.canvas.draw()
# wait till process_search terminates
process_search.join()
time.sleep(0.5)
print("Search finished.")
if path_shared.value is not None and len(path_shared.value) > 1:
print("Solution successfully found :)")
else:
print("Finding solution failed :(")
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
import copy
import numpy as np
import xml.etree.ElementTree as et
from sphinx.util import xmlname_checker
from Automata.MotionPrimitiveParser import MotionPrimitiveParser
from Automata.MotionPrimitive import MotionPrimitive
from automata.MotionPrimitiveParser import MotionPrimitiveParser
from automata.MotionPrimitive import MotionPrimitive
from commonroad.geometry.shape import Rectangle
class MotionAutomata:
def __init__(self, state_tuple, dt):
self.state_tuple = state_tuple
"""
Class to handle motion primitives for motion planning
"""
def __init__(self, veh_type_id):
self.numPrimitives = 0
self.Primitives = []#np.empty(0, dtype=MotionPrimitive)
#self.dt = dt
return
self.Primitives = []
self.veh_type_id = veh_type_id
self.egoShape = None
if veh_type_id == 1:
self.egoShape = Rectangle(length=4.298, width=1.674)
elif veh_type_id == 2:
self.egoShape = Rectangle(length=4.508, width=1.610)
elif veh_type_id == 3:
self.egoShape = Rectangle(length=4.569, width=1.844)
def readFromXML(self, filename: str) -> None:
"""
......@@ -19,45 +28,73 @@ class MotionAutomata:
:param filename: the name of the file to be read from
"""
# parse XML file
xmlTree = et.parse(filename).getroot()
# add trajectories
self.numPrimitives = self.numPrimitives + len(xmlTree.findall('Trajectory'))
#self.Primitives = np.empty(self.numPrimitives, dtype=MotionPrimitive)
#i = 0
for t in xmlTree.findall('Trajectory'):
self.Primitives.append(MotionPrimitiveParser.createFromNode(t)) # self.dt
#self.Primitives[i].id = i
#i = i + 1
self.Primitives.append(MotionPrimitiveParser.createFromNode(t))
self.setVehicleTypeIdPrimitives()
return
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.
:param primitive: the primitive to be connected
"""
for i in range(self.numPrimitives):
if primitive.checkConnectivityToNext(self.Primitives[i]):
primitive.Successors.append(self.Primitives[i])
for self_primitive in self.Primitives:
if primitive.checkConnectivityToNext(self_primitive):
primitive.Successors.append(self_primitive)
def createConnectivityLists(self)->None:
def createConnectivityLists(self) -> None:
"""
Creates a connectivity list for every primitive (let every primitive has its corresponding successor list).
"""
for t in self.Primitives:
self.createConnectivityListPrimitive(t)
for self_primitive in self.Primitives:
self.createConnectivityListPrimitive(self_primitive)
return
def createMirroredPrimitives(self)->None:
def createMirroredPrimitives(self) -> None:
"""
Creates the mirrored motion primitives since the file to load primitives by default only contains left curves. This function computes the right curves.
"""
import copy
oldPrimitives = self.Primitives
self.numPrimitives = 2 * self.numPrimitives
self.Primitives = np.empty(self.numPrimitives, dtype=MotionPrimitive)
for i in range(len(oldPrimitives)):
self.Primitives[i] = oldPrimitives[i]
newPrim = copy.deepcopy(self.Primitives[i])
newPrim.mirror()
self.Primitives[i + len(oldPrimitives)] = newPrim
# create mirrored primitives for the old primitives
newPrimitive = copy.deepcopy(self.Primitives[i])
newPrimitive.mirror()
# add the new mirrored primitive to self primitive list
self.Primitives[i + len(oldPrimitives)] = newPrimitive
return
def getClosestStartVelocity(self, initial_velocity):
"""
get the velocity among start states that is closest to the given initial velocity
"""
min_error = float('inf')
min_idx = 0
for i in range(len(self.Primitives)):
primitive = self.Primitives[i]
error_velocity = abs(initial_velocity - primitive.startState.velocity)
if error_velocity < min_error:
min_error = error_velocity
min_idx = i
return self.Primitives[min_idx].startState.velocity
def setVehicleTypeIdPrimitives(self):
"""
assign vehicle type id to all primitives
"""
for primitive in self.Primitives:
primitive.veh_type_id = self.veh_type_id
from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_checker, create_collision_object
from commonroad.planning.planning_problem import PlanningProblemSet, PlanningProblem
from commonroad.scenario.obstacle import StaticObstacle, ObstacleType, Obstacle
from commonroad.scenario.trajectory import State as StateTupleFactory
from commonroad.scenario.trajectory import State
from commonroad.prediction.prediction import TrajectoryPrediction
from commonroad.visualization.draw_dispatch_cr import draw_object
from commonroad.geometry.shape import Polygon, ShapeGroup, Circle
from commonroad.common.util import Interval, AngleInterval
from commonroad.scenario.lanelet import LaneletNetwork, Lanelet
from commonroad.common.util import Interval
from commonroad.scenario.lanelet import Lanelet
from commonroad.scenario.trajectory import Trajectory
from commonroad.planning.goal import GoalRegion
from commonroad.geometry.shape import Rectangle
from Automata.States import StartState, FinalState
from Automata.MotionAutomata import MotionAutomata, MotionPrimitive
import time
from automata.MotionAutomata import MotionPrimitive
import numpy as np
import math
import construction
import heapq
from typing import *
class MotionPlanner:
def __init__(self, scenario, planningProblem, automata, state_tuple, shapeEgoVehicle):
def __init__(self, scenario, planningProblem, automata):
# store input parameters
self.scenario = scenario
self.planningProblem = planningProblem
self.automata = automata
self.state_tuple = state_tuple
self.frontier = PriorityQueue()
self.egoShape = automata.egoShape
# create necessary attributes
self.lanelet_network = self.scenario.lanelet_network
self.priority_queue = PriorityQueue()
self.obstacles = self.scenario.obstacles
self.initial_state = self.planningProblem.initial_state
self.egoShape = shapeEgoVehicle
# remove unneeded attributes of initial state
if hasattr(self.initial_state, 'yaw_rate'):
del(self.initial_state.yaw_rate)
del self.initial_state.yaw_rate
if hasattr(self.initial_state, 'slip_angle'):
del(self.initial_state.slip_angle)
self.startLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position([self.planningProblem.initial_state.position])[0]
if hasattr(self.planningProblem.goal.state_list[0].position, 'center'):
self.goalLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position([self.planningProblem.goal.state_list[0].position.center])[0]
elif hasattr(planningProblem.goal.state_list[0].position, 'shapes'):
self.goalLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position([self.planningProblem.goal.state_list[0].position.shapes[0].center])[0]
self.planningProblem.goal.state_list[0].position.center = self.planningProblem.goal.state_list[0].position.shapes[0].center
self.initial_distance = distance(planningProblem.initial_state.position, planningProblem.goal.state_list[0].position.center)
self.lanelet_network = self.scenario.lanelet_network
self.lanelet_cost = {}
for lanelet_id in self.lanelet_network._lanelets.keys():
self.lanelet_cost[lanelet_id] = -1
for goal_lanelet in self.goalLanelet_ids:
self.lanelet_cost[goal_lanelet] = 0
for goal_lanelet in self.goalLanelet_ids:
visited_lanelets = []
self.calc_lanelet_cost(self.scenario.lanelet_network.find_lanelet_by_id(goal_lanelet), 1, visited_lanelets)
del self.initial_state.slip_angle
# get lanelet id of the starting lanelet (of initial state)
self.startLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position(
[self.planningProblem.initial_state.position])[0]
build = ['section_triangles', 'triangulation']
boundary = construction.construct(self.scenario, build, [], [])
road_boundary_shape_list = list()
for r in boundary['triangulation'].unpack():
initial_state = StateTupleFactory(position=np.array([0, 0]), orientation=0.0, time_step=0)
p = Polygon(np.array(r.vertices()))
road_boundary_shape_list.append(p)
road_bound = StaticObstacle(obstacle_id=scenario.generate_object_id(), obstacle_type=ObstacleType.ROAD_BOUNDARY, obstacle_shape=ShapeGroup(road_boundary_shape_list), initial_state=initial_state)
self.collisionChecker = create_collision_checker(self.scenario)
self.collisionChecker.add_collision_object(create_collision_object(road_bound))
# get lanelet id of the ending lanelet (of goal state),this depends on type of goal state
if hasattr(self.planningProblem.goal.state_list[0].position, 'center'):
self.goalLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position(
[self.planningProblem.goal.state_list[0].position.center])[0]
elif hasattr(planningProblem.goal.state_list[0].position, 'shapes'):
self.goalLanelet_ids = self.scenario.lanelet_network.find_lanelet_by_position(
[self.planningProblem.goal.state_list[0].position.shapes[0].center])[0]
self.planningProblem.goal.state_list[0].position.center = \
self.planningProblem.goal.state_list[0].position.shapes[0].center
# set specifications from given goal state
if hasattr(self.planningProblem.goal.state_list[0], 'time_step'):
self.desired_time = self.planningProblem.goal.state_list[0].time_step
else:
self.desired_time = Interval(0, np.inf)
if hasattr(self.planningProblem.goal.state_list[0], 'velocity'):
self.desired_velocity = self.planningProblem.goal.state_list[0].velocity
else:
self.desired_velocity = Interval(0, np.inf)
if hasattr(self.planningProblem.goal.state_list[0], 'orientation'):
self.desired_orientation = self.planningProblem.goal.state_list[0].orientation
else:
self.desired_orientation = Interval(-math.pi, math.pi)
# create necessary attributes
self.initial_distance = distance(planningProblem.initial_state.position,
planningProblem.goal.state_list[0].position.center)
# set lanelet costs to -1, except goal lanelet
self.lanelet_cost = {}
for lanelet in scenario.lanelet_network.lanelets:
self.lanelet_cost[lanelet.lanelet_id] = -1
for goal_lanelet_id in self.goalLanelet_ids:
self.lanelet_cost[goal_lanelet_id] = 0
# calculate costs for lanelets, this is a recursive method
for goal_lanelet_id in self.goalLanelet_ids:
visited_lanelets = []
self.calc_lanelet_cost(self.scenario.lanelet_network.find_lanelet_by_id(goal_lanelet_id), 1, visited_lanelets)
# construct commonroad boundaries and collision checker object
build = ['section_triangles', 'triangulation']
boundary = construction.construct(self.scenario, build, [], [])
road_boundary_shape_list = list()
initial_state = None
for r in boundary['triangulation'].unpack():
initial_state = StateTupleFactory(position=np.array([0, 0]), orientation=0.0, time_step=0)
p = Polygon(np.array(r.vertices()))
road_boundary_shape_list.append(p)
road_bound = StaticObstacle(obstacle_id=scenario.generate_object_id(), obstacle_type=ObstacleType.ROAD_BOUNDARY,
obstacle_shape=ShapeGroup(road_boundary_shape_list), initial_state=initial_state)
self.collisionChecker = create_collision_checker(self.scenario)
self.collisionChecker.add_collision_object(create_collision_object(road_bound))
def map_obstacles_to_lanelets(self, time_step : int) -> dict:
def map_obstacles_to_lanelets(self, time_step: int):
"""
Finds all obstacles that are located in every lanelet at time step t and returns a dictionary where obstacles are stored according to their lanelet ids.
Find all obstacles that are located in every lanelet at time step t and returns a dictionary where obstacles are stored according to lanelet id.
:param time_step: maps obstacles in this time step to the lanelet
:param time_step: The time step in which the obstacle is in the current lanelet network.
:Return type: dict[lanelet_id]
"""
mapping = {}
for l in self.lanelet_network._lanelets.values():
for lanelet in self.lanelet_network.lanelets:
# map obstacles to current lanelet
mapped_objs = self.get_obstacles(l, time_step)
mapped_objs = self.get_obstacles(lanelet, time_step)
# check if mapping is not empty
if len(mapped_objs) > 0:
mapping[l.lanelet_id] = mapped_objs
mapping[lanelet.lanelet_id] = mapped_objs
return mapping
def get_obstacles(self, laneletObj: Lanelet, time_step : int) -> List[Obstacle]:
def get_obstacles(self, laneletObj: Lanelet, time_step: int) -> List[Obstacle]:
"""
Returns the subset of obstacles, which are located in the given lanelet.
......@@ -121,7 +142,7 @@ class MotionPlanner:
else:
vertices.append(sh.vertices)
else:
# distinguish between type of shape (circle has no vertices)
# distinguish between type of shape (circle has no vertices)
if isinstance(o_shape, Circle):
vertices = o_shape.center
else:
......@@ -132,37 +153,40 @@ class MotionPlanner:
res.append(o)
return res
def calc_lanelet_cost(self, curLanelet: Lanelet, dist: int, visited_lanelets: List[int]):
"""
Calculates distances of all lanelets which can be reached through recursive adjacency/predecessor relationship by the current lanelet.
This is a recursive implementation.
:param curLanelet: the current lanelet object (often set to the goal lanelet)
:param dist: the initial distance between 2 adjacent lanelets (often set to 1). This value will increase recursively during the execution of this function.
:param visited_lanelets: list of visited lanelet ids. In the iterations, visited lanelets will not be considered. This value changes during the recursive implementation.
:param curLanelet: the current lanelet object (Often set to the goal lanelet).
:param dist: the initial distance between 2 adjacent lanelets (Often set to 1). This value will increase recursively during the execution of this function.
:param visited_lanelets: list of visited lanelet id. In the iterations, visited lanelets will not be considered. This value changes during the recursive implementation.
The calculated costs will be stored in dictionary self.lanelet_cost[Lanelet].
"""
if curLanelet.lanelet_id in visited_lanelets:
return
visited_lanelets.append(curLanelet.lanelet_id)
if curLanelet._predecessor is not None: