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 calculate_B(initial_vehicles: List[VehicleInfo],
current_vehicles: List[VehicleInfo], scenario: Scenario,
total_steps: int) -> Dict[Tuple[int, int], float]:
p: int = len(initial_vehicles)
initial_area_profiles: Dict[int, ndarray] = {}
for j in range(len(current_vehicles)):
initial_area_profiles[j], _ \
= GenerationHelp.generate_states(scenario, initial_vehicles[j].state, total_steps)
B: Dict[Tuple[int, int], float] = {}
for j in range(p):
new_states, _ = GenerationHelp.generate_states(
scenario, current_vehicles[j].state, total_steps)
new_area_profile: ndarray = calculate_area_profile(
flatten_dict_values(new_states))
state_j: MyState = current_vehicles[j].state
initial_state_j: MyState = initial_vehicles[j].state
n_j: int = len(state_j.variables)
for i in range(n_j):
variation_ij = state_j.variable(i) - initial_state_j.variable(i)
if variation_ij == 0:
B[(i, j)] = -1
else:
B[(i, j)] = norm(
(new_area_profile - initial_area_profiles[j]) /
variation_ij)
return B
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))
def optimized_scenario(initial_vehicles: List[VehicleInfo],
epsilon: float,
it_max: int,
my: int,
a_ref: ndarray,
scenario: Scenario,
planning_problem: PlanningProblem,
W: np.matrix = None):
# Require
# x_0 initial state vector
# epsilon threshold
# it_max iteration limit
# my binary search iteration limit
# W weighted matrix
# a_ref reference area profile
# Ensure
# critical scenario S
# kappa_new <- 0
# kappa_old <- -inf
# it <- 0
# x_{0,curr} <- x_o
# while abs(kappa_new - kappa_old) >= epsilon and it < it_max do
# success <- true
# while abs(kappa_new - kappa_old) >= epsilon and success = true do
# kappa_old <- kappa_new
# x_{0,old} <- x_{0,curr}
# x_{0,curr}, S, success <- quadProg(solve(quadratic optimization problem))
# kappa_new <- kappa(S, a_ref, W)
# end while
# x_{0,s)^v <- binarySearch(x_{0,old}, x_{0,curr}, my)
# S <- updateScenario(S, x_{0,s}^v)
# kappa_new <- kappa(S, a_ref, W)
# it <- it + 1
# end while
p: int = len(initial_vehicles)
r: int = len(MyState.variables)
total_steps: int = len(a_ref)
if not W:
W = eye(p)
kappa_new: float = 0
kappa_old: float = -np.inf
it: int = 0
current_initial_vehicles = initial_vehicles
while abs(kappa_new - kappa_old) >= epsilon and it < it_max:
success: bool = True
while abs(kappa_new - kappa_old) >= epsilon and success:
kappa_old = kappa_new
old_initial_vehicles: List[VehicleInfo] = current_initial_vehicles
current_generated_vehicles, _ = GenerationHelp.generate_states(
scenario, current_initial_vehicles[0].state,
total_steps) # FIXME Only ego vehicle considered
# x_{0,curr}, S, success <- quadProg(solve(quadratic optimization problem))
B: matrix = calculate_B_as_matrix(old_initial_vehicles,
current_initial_vehicles,
scenario, total_steps)
W_tilde: matrix = np.asmatrix(B.transpose() * W * B)
delta_a0: matrix = np.asmatrix(
calculate_area_profile(
flatten_dict_values(current_generated_vehicles)) - a_ref)
c: ndarray = delta_a0.transpose() * (W * B + W.transpose() * B)
delta_x: Variable = Variable((1, r))
objective: Minimize = Minimize(
cvxpy.norm(quad_form(delta_x, W_tilde) + c * delta_x))
# Instead of the "real" goal region constraint use a minimum velocity needed for getting to the goal region
# within the given amount of steps
# FIXME Only ego vehicle considered
current_initial_position: Point = Point(
current_initial_vehicles[0].state.state.position)
# FIXME Only first goal region entry recognized
goal_region: Polygon = planning_problem.goal.state_list[
0].position.shapely_object
distance_to_goal_region: float = current_initial_position.distance(
goal_region)
min_velocity: float = distance_to_goal_region / (scenario.dt *
total_steps)
print("min_velocity: " + str(min_velocity))
# FIXME Really use delta_a0?
# FIXME If min_velocity is greater than zero the optimization fails
constraints = [
delta_x >= 0, delta_a0 + B * delta_x >= 0,
delta_x[0] >= min_velocity
]
problem = Problem(objective, constraints)
assert is_dccp(problem)
print("Problem solve result: " +
str(problem.solve(method='dccp', solver='ECOS')))
print("Current optimization result: " + str(delta_x.value))
# FIXME Change current initial vehicles like this?
for i in range(len(current_initial_vehicles)):
for j in range(len(MyState.variables)):
# FIXME Only ego vehicle considered
current_initial_vehicles[i].state.set_variable(
j, delta_x.value[i][j])
kappa_new = kappa(delta_a0, a_ref, W) # FIXME Really use delta_a0?
print("Difference between new and old costs: " +
str(abs(kappa_new - kappa_old)))
binary_search(my, old_initial_vehicles, current_initial_vehicles,
scenario,
total_steps) # FIXME What to do with this value?
# initial_vehicles = ? # FIXME What to do here?
update_scenario_vehicles(scenario, planning_problem, initial_vehicles)
kappa_new = kappa(calculate_area_profile(initial_vehicles), a_ref, W)
it += 1
def binary_search(my: int, initial_vehicles_before: List[VehicleInfo],
initial_vehicles_after: List[VehicleInfo],
scenario: Scenario, total_steps: int) -> float:
# Require
# x_{0,before,i}^j initial state before last quadratic update
# x_{0,after,i}^j initial state after last quadratic update
# my iteration limit
# Ensure
# x_{0,i}^j so that solution space is not empty
# b_max <- 0
# for j = 1...p
# for i = 1...n(j)
# b_{abs,i}^j <- abs((the new area profile - initial profile) / variation of i-th component of initial state of vehicle V_j)
# end for
# end for
# b_sorted = sort(b_abs)
# while not empty(b_sorted) do
# vI, sI <- pop(b_sorted)
# for theta = 1...my do
# x_{0,curr,sI}^vI <- 0.5 * (x_{0,before,sI}^vI + x_{0,after,sI}^vI)
# a_curr <- S(<sI,vI>, delta x_{0,sI}^vI)
# if for all l: a_{curr,l} > 0 then
# return x_{0,curr,sI}^vI
# else
# x_{0,after,sI} <- x_{0,curr,sI}^vI
# end if
# end for
# end while
# return x_{0,before,sI}^vI
b_max = 0 # FIXME What is this variable for?
# Absolute values of sensitivities
b_abs: Dict[Tuple[int, int], float] \
= calculate_B(initial_vehicles_before, initial_vehicles_after, scenario, total_steps)
for key in b_abs.keys():
b_abs[key] = abs(b_abs[key])
b_sorted: Queue[Tuple[int, int]] = Queue()
for bij in sorted(b_abs, key=lambda key: b_abs[key],
reverse=True): # Sort based on sensitivity descending
b_sorted.put(bij)
current_vehicles: List[
VehicleInfo] = initial_vehicles_before # States to be modified step by step
while not b_sorted.empty():
v_i, s_i = b_sorted.get()
for theta in range(my):
state_sum: float = initial_vehicles_before[s_i].state.variable(v_i) \
+ initial_vehicles_after[s_i].state.variable(v_i)
current_vehicles[s_i].state.set_variable(v_i, 0.5 * state_sum)
current_area: ndarray = calculate_area_profile(
flatten_dict_values(
GenerationHelp.generate_states(scenario,
current_vehicles[s_i].state,
total_steps)[0]))
if all(current_area > 0):
return current_vehicles[s_i].state.variable(v_i)
else:
initial_vehicles_after[s_i].state.set_variable(
v_i, current_vehicles[s_i].state.variable(v_i))
return initial_vehicles_before[s_i].state.variable(v_i)