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))
import matplotlib.pyplot as plt
def eval_restrictor(scenario,
construct_build=[
'section_triangles', ('triangulation', {
'hull': False
})
],
construct_kwargs={
'draw_order': [],
'plot_order': []
},
count=10_000,
repeat=10):
t = construct(scenario, construct_build, **construct_kwargs)
boundary_rep = t.get('triangulation')
corners = t.get('corners')
lanelet_network = scenario.lanelet_network
lanelets = lanelet_network.lanelets
def inside_road(vehicle, rep, fill_holes):
candidates = [
polygon for polygon, collision_mesh in rep
if collision_mesh.collide(vehicle)
]
if not candidates:
return False
def tri_test():
build = ['section_triangles', 'triangulation']
draw = ['triangulation']
scenario = open_measurable('GER_Ffb_2')
construct(scenario, build, draw)
def shell_test():
build = ['simple_triangles', 'section_triangles', 'shell']
draw = ['shell']
scenario = open_measurable('GER_Ffb_2')
construct(scenario, build, draw)
def quad_test():
"""Builds a quadtree for an example scenario"""
build = ['simple_triangles', 'quads']
draw = ['quads']
scenario = open_measurable('GER_Ffb_2')
construct(scenario, build, draw)
def construct_from_file(scenario_file, *vargs, **kwargs):
"""Opens the scenario_file and then calls construct"""
scenario = open_scenario(scenario_file)
return construct(scenario, *vargs, **kwargs)