def main() -> None:
scenario, planning_problem = load_scenario(
'../scenarios/DEU_B471-1_1_T-1_mod.xml')
measurements: Dict[int, float] = {}
max_steps: int = 21
plt.figure(figsize=(19.20, 10.80), dpi=100)
plt.suptitle("Duration for generating states for DEU_B471-1_1_T-1_mod")
plt.gca().yaxis.set_major_formatter(
FuncFormatter(lambda x, pos: str(timedelta(seconds=x))))
plt.xlim([0, max_steps])
plt.xticks(arange(0, max_steps, 1.0))
for num_steps in range(max_steps):
start_time: datetime = datetime.now()
GenerationHelp.generate_states(scenario,
MyState(planning_problem.initial_state),
num_steps)
duration: timedelta = datetime.now() - start_time
print("Generated " + str(num_steps) + " in " + str(duration) + ".")
measurements[num_steps] = duration.total_seconds()
plt.ylim([0, max(60.0, duration.total_seconds())])
plt.plot(measurements.keys(), measurements.values(), 'x',
measurements.keys(), measurements.values(), '-')
plt.pause(0.05)
plt.show()
def main() -> None:
scenario, planning_problem = load_scenario(
'scenarios/DEU_B471-1_1_T-1_mod.xml')
start_time: datetime = datetime.now()
ego_vehicle: MyState = MyState(copy(planning_problem.initial_state))
vehicles: List[VehicleInfo] = [VehicleInfo(ego_vehicle, -1)]
optimized_scenario(vehicles, 3000, 10, 10,
array([10, 9, 8, 7, 6, 5, 4, 3, 2, 1]), scenario,
planning_problem)
print("Optimization in " + str(datetime.now() - start_time))
print("Optimized velocity: " +
str(planning_problem.initial_state.velocity))
def main() -> None:
scenario, planning_problem = load_scenario('scenarios/DEU_B471-1_1_T-1_mod.xml')
fig = plt.figure(figsize=(25, 10))
DrawHelp.draw(DrawHelp.convert_to_drawable(scenario))
DrawHelp.draw(DrawHelp.convert_to_drawable(planning_problem.initial_state))
# DrawHelp.draw(DrawHelp.convert_to_drawable(planning_problem.goal))
MyState.set_variable_to(planning_problem.initial_state, 0, 15)
start_time: datetime = datetime.now()
valid_converted, num_states_processed \
= GenerationHelp.generate_states(scenario, MyState(planning_problem.initial_state), num_steps)
print("Processed " + str(num_states_processed) + " states in " + str(datetime.now() - start_time))
all_states: List[VehicleInfo] = flatten_dict_values(valid_converted)
frames: list = []
for i in range(1, num_steps + 1):
frame = list(map(lambda v: DrawHelp.draw(v.drawable), filter(lambda v: v.state.state.time_step <= i, all_states)))
union: MultiPolygon = DrawHelp.union_to_polygon(frame)
artist: Optional[Artist] = DrawHelp.draw(union)
if artist is not None:
if isinstance(artist, list):
frame.extend(artist)
else:
frame.append(artist)
frames.append(frame)
print("Drivable area: " + str(union.area))
plt.gca().set_aspect('equal')
plt.ylim([35, 60])
plt.xlim([92, 150])
ani = ArtistAnimation(fig, frames, interval=500, blit=True, repeat_delay=1000)
# ani.save("2019-02-16_SimpleDrivableArea_10steps_35ms.mp4", writer="ffmpeg") # mp4 broken
plt.show()
def main() -> None:
scenario, planning_problem = load_scenario('scenarios/DEU_B471-1_1_T-1_mod.xml')
plt.figure(figsize=(25, 10))
DrawHelp.draw(DrawHelp.convert_to_drawable(scenario))
DrawHelp.draw(DrawHelp.convert_to_drawable(planning_problem.initial_state))
start_time: datetime = datetime.now()
generation_result: Tuple[Dict[int, List[VehicleInfo]], int] \
= GenerationHelp.generate_states(scenario, MyState(planning_problem.initial_state), 15)
# NOTE The value 50 is taken from the commonroad file
num_states_processed: int = generation_result[1]
valid_converted: Dict[int, List[VehicleInfo]] = generation_result[0]
valid_states: List[State] = list(map(lambda v: v.state.state, flatten_dict_values(valid_converted)))
print("Processed " + str(num_states_processed) + " states in " + str(datetime.now() - start_time))
# FIXME Properly define goal states
goal_states: List[State] = []
for goal in planning_problem.goal.state_list:
if isinstance(goal.position, Rectangle):
goal.position = goal.position.center
goal.orientation = (goal.orientation.end + goal.orientation.start) / 2
goal.time_step = 16
goal_states.append(goal)
search_result: Optional[Tuple[List[State], float]] = dijkstra_search(
planning_problem.initial_state, goal_states, valid_states)
if search_result:
for state in search_result[0]:
DrawHelp.draw(DrawHelp.convert_to_drawable(state))
else:
print("Could not find path to goal")
plt.gca().set_aspect('equal')
plt.show()
def main() -> None:
scenario, planning_problem = load_scenario(
'scenarios/DEU_B471-1_1_T-1_mod_2.xml')
num_time_steps: int = 15
fixed_drawables: List[drawable_types] = []
for obj in [scenario.lanelet_network, planning_problem.initial_state]:
fixed_drawables.append(DrawHelp.convert_to_drawable(obj))
for time_step in range(1, num_time_steps + 1):
for obs in scenario.static_obstacles:
fixed_drawables.append(
DrawHelp.convert_to_drawable(
[obs.occupancy_at_time(time_step)]))
for obs in scenario.dynamic_obstacles:
fixed_drawables.append(
DrawHelp.convert_to_drawable(
[obs.occupancy_at_time(time_step)]))
min_velocity: int = 40
max_velocity: int = 59
velocity_step_size: float = 0.1
drivable_areas: Dict[int, float] = {}
config_file: TextIO = open("config.conf", "w")
config_file.write("Velocities from " + str(min_velocity) + " to " +
str(max_velocity))
config_file.write("num_time_steps: " + str(num_time_steps))
config_file.write("time_step_size: " + str(scenario.dt))
config_file.write("GenerationConfig.max_yaw: " +
str(GenerationConfig.max_yaw))
config_file.write("GenerationConfig.yaw_steps: " +
str(GenerationConfig.yaw_steps))
config_file.write("GenerationConfig.num_threads: " +
str(GenerationConfig.num_threads))
config_file.write("GenerationConfig.position_threshold: " +
str(GenerationConfig.position_threshold))
config_file.write("GenerationConfig.angle_threshold: " +
str(GenerationConfig.angle_threshold))
config_file.write("DrawConfig.car_length: " + str(DrawConfig.car_length))
config_file.write("DrawConfig.car_width: " + str(DrawConfig.car_width))
config_file.close()
overall_time: datetime = datetime.now()
for velocity in arange(min_velocity, max_velocity + velocity_step_size,
velocity_step_size):
print("Velocity: " + str(velocity))
# Prepare figure and subplots
main_fig, (scenario_fig, area_fig) = plt.subplots(nrows=1,
ncols=2,
figsize=(19.20,
10.80),
dpi=100)
main_fig.suptitle("Influence of velocity to drivable area",
fontsize=32)
plt.sca(scenario_fig)
# Draw artists to be shown always
for fixed in fixed_drawables:
DrawHelp.draw(copy(fixed))
# Generate and draw states resulting from the current velocity of the ego vehicle
planning_problem.initial_state.velocity = velocity
start_time: datetime = datetime.now()
valid_converted, num_states_processed \
= GenerationHelp.generate_states(scenario, MyState(planning_problem.initial_state), num_time_steps)
print("Processed " + str(num_states_processed) + " states in " +
str(datetime.now() - start_time))
all_states: List[VehicleInfo] = flatten_dict_values(valid_converted)
for vehicle in all_states:
DrawHelp.draw(vehicle.drawable)
union: MultiPolygon = DrawHelp.union_to_polygon(
list(map(lambda v: v.drawable, all_states)))
DrawHelp.draw(union)
drivable_areas[velocity] = union.area
scenario_fig.set_aspect('equal')
scenario_fig.set_xlim([100, 200])
scenario_fig.set_ylim([30, 100])
scenario_fig.set_xlabel("position [m]")
scenario_fig.set_ylabel("position [m]")
# Draw changes of drivable area
area_fig.set_xlim([min_velocity, max_velocity])
area_fig.set_ylim([0, max(270.0, max(drivable_areas.values()))])
area_fig.plot(drivable_areas.keys(), drivable_areas.values(), 'x',
drivable_areas.keys(), drivable_areas.values(), '-')
area_fig.set_xlabel("velocity [m/s]")
area_fig.set_ylabel("area [m²]")
# Save the figure
plt.savefig("out_velocity_" + str(velocity) + ".png")
# plt.close() # FIXME Causes EOFErrors
print("Overall generation took: " + str(datetime.now() - overall_time))