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)
def check_collision_free(self, path: List[State]) -> bool:
"""
Checks if path collides with an obstacle. Returns true for no collision and false otherwise.
:param path: The path you want to check
"""
trajectory = Trajectory(path[0].time_step, path)
# create a TrajectoryPrediction object consisting of the trajectory and the shape of the ego vehicle
traj_pred = TrajectoryPrediction(trajectory=trajectory,
shape=self.egoShape)
# create a collision object using the trajectory prediction of the ego vehicle
co = create_collision_object(traj_pred)
#check collision for motion primitive
if (self.collisionChecker.collide(co)):
return False
return True
def create_collision_object(self):
if self.trajectory is None:
raise VehicleModelException('Trajectory is not defined, could not create collision object!')
return create_collision_object(TrajectoryPrediction(trajectory=self.trajectory, shape=self.shape))
def __init__(self, scenario, planningProblem, automata):
# store input parameters
self.scenario = scenario
self.planningProblem = planningProblem
self.automata = automata
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
# remove unneeded attributes of initial state
if hasattr(self.initial_state, 'yaw_rate'):
del self.initial_state.yaw_rate
if hasattr(self.initial_state, 'slip_angle'):
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]
# 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 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
for scenario_p, solution_p in tqdm(zip(scenarios, solutions)):
scenario, planning_problem_set = CommonRoadFileReader(scenario_p).open()
solution = CommonRoadSolutionReader().open(solution_p)
plot_name = scenario_p[-10:-4]
ego_vehicle_trajectory = solution.planning_problem_solutions[0].trajectory
# ego_vehicle_trajectory_first = Trajectory(initial_time_step=0, state_list=ego_vehicle_trajectory.state_list[:2])
vehicle2 = parameters_vehicle2()
ego_vehicle_shape = Rectangle(length=vehicle2.l, width=vehicle2.w)
ego_vehicle_prediction = TrajectoryPrediction(
trajectory=ego_vehicle_trajectory, shape=ego_vehicle_shape)
cc = create_collision_checker(scenario)
co = create_collision_object(ego_vehicle_prediction)
if args.check_all and cc.collide(co):
print(plot_name)
collision_num += 1
else:
print('Does collision exist? ', cc.collide(co))
plt.figure()
draw_object(scenario.lanelet_network)
draw_object(cc, draw_params={'collision': {'facecolor': 'blue'}})
draw_object(co, draw_params={'collision': {'facecolor': 'green'}})
plt.autoscale()
plt.axis('equal')
plt.xlim(-10, 40)
plt.ylim(-15, 15)
