def Astar(start, goal, road, cost_map, vehicle, heuristic_map, cursor, static=True,weights=np.array([5., 10., 0.05, 0.2, 0.2, 0.2, 10., 0.5, 10., -2.])):
# pq - priority queue of states waiting to be extended, multiprocessing
# node_dict - {(i,j,k):state}
# edge_dict - store trajectory, {(state1, state2):trajectory}
# pq, node_dict, edge_dict are better defined outside, for multiprocessing
# count = 0
pq = PriorityQueue()
pq.put(start)
node_dict = {(start.r_i, start.r_j, int(round(start.v/2))):start, (goal.r_i, goal.r_j, int(round(goal.v/2))):goal}
edge_dict = {}
times = 0
while times<100 and not goal.reach and not pq.empty():
times += 1
current = pq.get()
successors = current.successors(state_dict=node_dict, road=road, goal=goal, vehicle=vehicle, heuristic_map=heuristic_map)
current.extend = True
for successor in successors:
# count += 1
traj = trajectory(current, successor, cursor)
if traj is not None:
if successor == goal:
cost = TG.eval_trajectory(traj, cost_map, vehicle=vehicle, road=road, truncate=False, static=static, weights=weights)
truncated = False
else:
cost, traj, truncated = TG.eval_trajectory(traj, cost_map, vehicle=vehicle, road=road, static=static, weights=weights)
if not np.isinf(cost) and traj is not None:
State.post_process(current, successor, goal, cost, traj, truncated, pq, node_dict, edge_dict, vehicle, road, cost_map, heuristic_map, cursor, static=static, weights=weights)
if goal.reach:
return True, node_dict, edge_dict
else:
return False, node_dict, edge_dict
# return count + len(node_dict) + len(edge_dict)
def post_process(current, successor, goal, cost, traj, truncated, pq, state_dict, traj_dict, vehicle, road, costmap, heuristic_map,cursor, static=True, weights=np.array([5., 10., 0.05, 0.2, 0.2, 0.2, 10., 0.5, 10., -2.])):
if not truncated: # successor.reach
if successor.update(current, cost, traj, traj_dict,heuristic_map,vehicle,static):
pq.put(successor)
if successor != goal:
i,j,k = successor.r_i, successor.r_j, int(round(successor.v/2))
if (i,j,k) not in state_dict:
state_dict[(i,j,k)] = successor
traj_dict[(current, successor)] = traj
else:
successor = State.update2(traj, road, heuristic_map, vehicle, static)
i, j, k = successor.r_i, successor.r_j, int(round(successor.v/2))
try:
state = state_dict[(i,j,k)]
if state.distance(successor) > 1.e-4:
traj = trajectory(current, state, cursor)
if traj is not None:
cost = TG.eval_trajectory(traj, costmap, vehicle=vehicle, road=road, truncate=False, weights=weights)
if not np.isinf(cost):
successor = state
else:
successor = None
else:
successor = None
else:
successor = state
except KeyError:
state_dict[(i,j,k)] = successor
finally:
if successor is not None:
if successor.update(current, cost, traj, traj_dict,heuristic_map,vehicle,static):
pq.put(successor)
traj_dict[(current, successor)] = traj
def trajectory_reverse(vi, vg, path, p, r, ai=0., cost_map=np.zeros((500,500)), \
truncate=False, p_lims=(0.2,0.15, 6.,-4., 2.1,-6.1,10.)):
if vi <= 0 and vg <= 0 and path is not None and p[4] > 0:
# u = TG.calc_velocity(vi, ai, vg, -p[4])
u = calc_velocity(vi, vg, -p[4])
if u[3] is not None and u[3]>0:
# traj = TG.calc_trajectory_reverse(u, p, r, s=p[4], path=path, ref_time=0., ref_length=0.)
traj = calc_trajectory_reverse(u, p, r, s=p[4], path=path, ref_time=0., ref_length=0.)
cost = TG.eval_trajectory(traj, costmap=cost_map, truncate=truncate, p_lims=p_lims)
if not np.isinf(cost):
return traj, u
return None, None
def extend_plot():
# database connection
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
# plot
fig = plt.figure()
ax1 = fig.add_subplot(111)
# road center line points
p = (0.,0.,0.,0.,90.) # (p0~p3, sg)
center_line = TG.spiral3_calc(p, q=(5.,50.,0.))
# print(center_line)
# road
road = Road(center_line)
for i in range(road.grid_num_lateral+1):
if (i % road.grid_num_per_lane) == 0:
ax1.plot(road.longitudinal_lines[:,2*i], road.longitudinal_lines[:,2*i+1], color='green', linewidth=1.5)
else:
ax1.plot(road.longitudinal_lines[:,2*i], road.longitudinal_lines[:,2*i+1], color='black', linewidth=0.3)
for i in range(road.grid_num_longitudinal+1):
ax1.plot(road.lateral_lines[:,2*i], road.lateral_lines[:,2*i+1],color='black', linewidth=0.3)
# vehicle
cfg0 = road.sl2xy(5.,0.)
veh = Vehicle(trajectory=np.array([[-1.,-1.,cfg0[0], cfg0[1], cfg0[2], cfg0[3], 0.,5.,0.]]))
# workspace
ws = Workspace(vehicle=veh, road=road)
road_lane_bitmap0 = ws.lane_grids[0]
road_lane_bitmap1 = ws.lane_grids[1]
road_lane_bitmap2 = ws.lane_grids[2]
# write the lane bitmaps into files
# np.savetxt('road_lane_bitmap0.txt', road_lane_bitmap0, fmt='%i',delimiter=' ')
# np.savetxt('road_lane_bitmap1.txt', road_lane_bitmap1, fmt='%i',delimiter=' ')
# np.savetxt('road_lane_bitmap2.txt', road_lane_bitmap2, fmt='%i',delimiter=' ')
# road bitmap
road_bitmap = road_lane_bitmap0 + road_lane_bitmap1 + road_lane_bitmap2
road_bitmap = np.where(road_bitmap>1.e-6, 1., 0.)
# np.savetxt('road_bitmap.txt', road_bitmap, fmt='%i', delimiter=' ')
# base bitmap
base = 1. - road_bitmap
# base = np.where(base>1.e-6, np.inf, 0)
# np.savetxt('base_bitmap.txt', base, fmt='%i', delimiter=' ')
# static obstacles
cfg1 = road.sl2xy(25., 0.)
cfg2 = road.sl2xy(25., -road.lane_width)
cfg3 = road.sl2xy(55.,0.)
cfg4 = road.sl2xy(55., road.lane_width)
obst1 = Vehicle(trajectory=np.array([[-1.,-1.,cfg1[0], cfg1[1], cfg1[2], cfg1[3], 0.,0.,0.]]))
obst2 = Vehicle(trajectory=np.array([[-1.,-1.,cfg2[0], cfg2[1], cfg2[2], cfg2[3], 0.,0.,0.]]))
obst3 = Vehicle(trajectory=np.array([[-1.,-1.,cfg3[0], cfg3[1], cfg3[2], cfg3[3], 0.,0.,0.]]))
obst4 = Vehicle(trajectory=np.array([[-1.,-1.,cfg4[0], cfg4[1], cfg4[2], cfg4[3], 0.,0.,0.]]))
base += ws.grids_occupied_by_polygon(obst1.vertex)
base += ws.grids_occupied_by_polygon(obst2.vertex)
base += ws.grids_occupied_by_polygon(obst3.vertex)
base += ws.grids_occupied_by_polygon(obst4.vertex)
base = np.where(base>1.e-6, 1.,0.)
# np.savetxt('scenario_1/static_bitmap.txt', base, fmt='%i', delimiter=' ')
# collision map
collision_map = cv2.filter2D(base, -1, ws.collision_filter)
collision_map = np.where(collision_map>1.e-6, 1., 0.)
# np.savetxt('scenario_1/collision_bitmap.txt', collision_map, fmt='%i', delimiter=' ')
# cost map
cost_map = cv2.filter2D(collision_map, -1, ws.cost_filter)
cost_map += collision_map
cost_map = np.where(cost_map>1., np.inf, cost_map)
cost_map = np.where(cost_map<1.e-16, 0., cost_map)
# np.savetxt('scenario_1/cost_grayscale_map.txt', cost_map, fmt='%1.6f', delimiter='\t')
# plot
# fig = plt.figure()
# ax1 = fig.add_subplot(111)
costmap_plot = np.where( cost_map >1., 1., cost_map)
ax1.imshow(costmap_plot, cmap=plt.cm.Reds, origin="lower",extent=(0.,ws.resolution*ws.row,0.,ws.resolution*ws.column))
ax1.plot(center_line[:,1], center_line[:,2], color='maroon', linestyle='--', linewidth=2.)
count = 0
start_state = State(road=road, r_s=5., r_l=0., v=5.)
ax1.plot(start_state.x, start_state.y, 'rs')
current = start_state
outs = current.out_set(road)
for (i,j,v,a) in outs:
# print(i,j,v,a)
next_state = State(road=road, r_i=i, r_j=j, v=v)
# print(current.q, next_state.q)
p, r = TG.calc_path(cursor, current.q, next_state.q)
if r is not None:
if p[4]>0:
u = TG.calc_velocity(current.v, a, v, p[4])
if u[3] is not None and u[3]>0:
path = TG.spiral3_calc(p,r,q=current.q,ref_delta_s=0.2)
traj = TG.calc_trajectory(u,p,r,s=p[4],path=path,q0=current.q, ref_time=current.time, ref_length=current.length)
# if next_state == goal_state:
# cost = TG.eval_trajectory(traj, cost_map, vehicle=veh, road=road, truncate=False)
# else:
cost, traj = TG.eval_trajectory(traj, cost_map, vehicle=veh, road=road)
if not np.isinf(cost) and traj is not None:
count += 1
next_state.update(cost, traj, current, road)
# plot
ax1.plot(traj[:,2], traj[:,3], linewidth=1.)
ax1.text(traj[-1,2], traj[-1,3],'{0:.2f}'.format(cost))
# close database connection
cursor.close()
conn.close()
#
# plt.legend()
plt.axis('equal')
# plt.savefig('scenario_1/planning_result.png', dpi=600)
plt.show()
