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 test_diff_init_val():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
q0 = (20., 20., -np.pi/6., -0.01)
q1 = (70., 30., np.pi/6., -0.01)
p1, r1 = TG.calc_path(cursor, q0, q1)
p2, r2 = TG.calc_path_no_init_val(q0,q1)
print('p={0},r={1}'.format(p1,r1))
print('p={0},r={1}'.format(p2,r2))
line1 = TG.spiral3_calc(p1, r=r1, q=q0)
line2 = TG.spiral3_calc(p2, r=r2, q=q0)
# plt.figure(figsize=(80,60))
plt.plot(line1[:,1], line1[:,2], color='black', linewidth=4)
plt.plot(line2[:,1], line2[:,2], color='black', linewidth=4)
plt.xlabel('$x (m)$', fontsize=40)
plt.ylabel('$y (m)$', fontsize=40)
# plt.text(P0[0]-3, P0[1]+3,'$p_0=k(0)$', fontsize=40)
# plt.text(P1[0]-3, P1[1]-3,'$p_1=k(\\frac{s_g}{3})$', fontsize=40)
# plt.text(P2[0]-3, P2[1]-3,'$p_2=k(\\frac{2s_g}{3})$', fontsize=40)
# plt.text(P3[0]-3, P3[1]-3,'$p_3=k(s_g)$', fontsize=40)
plt.axis('equal')
figManager = plt.get_current_fig_manager()
figManager.window.showMaximized()
# plt.savefig('img/coordinate_transform.png',dpi=600)
plt.show()
cursor.close()
conn.close()
def test_traj_sampling():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
q0 = (10., 50., 0., 0.)
q1 = (40., 60., 0., 0)
p, r = TG.calc_path(cursor, q0, q1)
# print('p={0},r={1}'.format(p,r))
line = TG.spiral3_calc(p, r=r, q=q0)
# print('Goal Configuration: {0}'.format(line[-1,:]))
# print(line.shape)
fig = plt.figure()
ax1 = fig.add_subplot(111)
# ax2 = fig.add_subplot(122)
ax1.plot(line[:,1], line[:,2], color='navy', linewidth=2.)
for i in range(65):
# ax1.plot(line[i*5, 1], line[i*5, 2], 'ro')
k = floor(324/64**2*i**2)
ax1.plot(line[k, 1], line[k, 2], 'ro')
ax1.plot(line[-1, 1], line[-1, 2], 'ro')
plt.axis('equal')
# plt.axis('off')
plt.show()
cursor.close()
conn.close()
def path_reverse(q0, q1, cursor):
p, r = TG.calc_path(cursor, q1, q0)
if r is not None and p[4] > 0.:
path = TG.spiral3_calc(p, r=r, q=q1)
path[:,1:] = path[::-1,1:]
return path, p, r # big error
return None, None, None
def test_long_path():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
q0 = (10., 50., 0., 0.)
q1 = (800., 10., 0., 0)
p, r = TG.calc_path(cursor, q0, q1)
print('p={0},r={1}'.format(p,r))
line = TG.spiral3_calc(p, r=r, q=q0)
print('Goal Configuration: {0}'.format(line[-1,:]))
fig = plt.figure()
ax1 = fig.add_subplot(111)
# ax2 = fig.add_subplot(122)
ax1.plot(line[:,1], line[:,2], color='black', linewidth=4)
plt.axis('equal')
# plt.axis('off')
plt.show()
cursor.close()
conn.close()
def trajectory(start, goal, cursor):
p, r = TG.calc_path(cursor, start.q, goal.q)
if r is not None and p[4]>0:
u = TG.calc_velocity(start.v, start.a, goal.v, p[4])
if u[3] is not None and u[3]>0:
path = TG.spiral3_calc(p,r,q=start.q,ref_delta_s=0.2)
traj = TG.calc_trajectory(u,p,r,s=p[4],path=path,q0=start.q, ref_time=start.time, ref_length=start.length)
return traj
return None
def test_select():
bd_con = (3.9/40, 7.1*3.0625+1, 2.1*6.25, 3.9*3.141592654/16, 4.1/40)
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
initial_guess = TG.select_init_val(cursor, bd_con)
print(initial_guess)
initial_guess2 = (0.0145, 0.0037, 23.5967)
pp = TG.optimize(bd_con, initial_guess2)
print(pp)
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 Build_InitialGuessTable():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
key_queue = [(0,i,0,0,0) for i in range(17)] # 数据库中当前已有数据，可以从数据库中读取已有数据
# print(key_queue)
items = 17
while len(key_queue)>0:
key = key_queue.pop(0)
# print(key)
init_val = db_query(cursor, key)
# print(init_val)
nears = neighbors(cursor, key) # 与key的曼哈顿距离为1的点
# print(nears)
for (i,j,k,l,m) in nears:
bd_con = (i/40, 1+3.0625*j, 6.25*k, l*np.pi/16, m/40)
val = TG.optimize(bd_con, init_val)
key_val = (i,j,k,l,m, val[0], val[1], val[2])
items += 1
print('Progress: {0:%}, Content: {1}'.format(items/1419857, key_val))
cursor.execute('insert or ignore into InitialGuessTable values (?,?,?,?,?,?,?,?)', key_val)
key_queue.append((i,j,k,l,m))
# 数据库操作要注意，插入数据之后要提交才能生效，游标处理要注意
conn.commit()
cursor.close()
conn.close()
def Build_InitialGuessTable():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
key_queue = [(0,i,0,0,0) for i in range(17)] # current daten in database. can also direct select from database
# print(key_queue)
items = 17
while len(key_queue)>0:
key = key_queue.pop(0)
# print(key)
init_val = db_query(cursor, key)
# print(init_val)
nears = neighbors(cursor, key) # select keys whose Manhattan Distance to key equals to 1.
# print(nears)
for (i,j,k,l,m) in nears:
bd_con = (i/40, 1+3.0625*j, 6.25*k, l*np.pi/16, m/40)
val = TG.optimize(bd_con, init_val)
key_val = (i,j,k,l,m, val[0], val[1], val[2])
items += 1
print('Progress: {0:%}, Content: {1}'.format(items/1419857, key_val))
cursor.execute('insert or ignore into InitialGuessTable values (?,?,?,?,?,?,?,?)', key_val)
key_queue.append((i,j,k,l,m))
#
conn.commit()
cursor.close()
conn.close()
def trajectory_reverse(start, goal, cursor):
if start.v<=0. and goal.v<=0:
q1 = (start.x, start.y, start.theta, start.k)
q0 = (goal.x, goal.y, goal.theta, goal.k)
p, r = TG.calc_path(cursor, q0, q1)
# print(p,r)
if r is not None and p[4]>0:
u = TG.calc_velocity(start.v, start.a, goal.v, -p[4])
# print(u)
if u[3] is not None and u[3]>0:
path = TG.spiral3_calc(p,r,q=q0,ref_delta_s=0.2)
# print(path[:,0])
path = path[::-1, :] #reverse
# print(path[:,0])
traj = TG.calc_trajectory_reverse(u, p, r, s=p[4], path=path, ref_time=start.time, ref_length=start.length)
return traj
return None
def test_coordinate_trasform():
from math import pi
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
q0 = (5., 0., pi/30, -0.01)
q1 = (55., 20., pi/30, 0.01)
p, r = TG.calc_path(cursor, q0, q1)
print('p={0},r={1}'.format(p,r))
sg = p[4]
# print(sg)
line = TG.spiral3_calc(p, r=r, q=q0)
print(line[-1,:])
P0 = q0[0:2]
P1 = TG.spiral3_calc(p, r=r, s=sg/3, q=q0)[-1, 1:3]
P2 = TG.spiral3_calc(p, r=r, s=2*sg/3, q=q0)[-1, 1:3]
P3 = TG.spiral3_calc(p, r=r, s=sg, q=q0)[-1, 1:3]
# plt.figure(figsize=(80,60))
plt.plot(line[:,1], line[:,2], color='black', linewidth=4)
# plt.plot(P0[0], P0[1], 'go', linewidth=4)
# plt.plot(P1[0], P1[1], 'go', linewidth=4)
# plt.plot(P2[0], P2[1], 'go', linewidth=4)
# plt.plot(P3[0], P3[1], 'go', linewidth=4)
plt.scatter(P0[0], P0[1], s=200)
plt.scatter(P1[0], P1[1], s=200)
plt.scatter(P2[0], P2[1], s=200)
plt.scatter(P3[0], P3[1], s=200)
plt.xlabel('$x (m)$', fontsize=40)
plt.ylabel('$y (m)$', fontsize=40)
plt.text(P0[0]-3, P0[1]+3,'$p_0=k(0)$', fontsize=40)
plt.text(P1[0]-3, P1[1]-3,'$p_1=k(\\frac{s_g}{3})$', fontsize=40)
plt.text(P2[0]-3, P2[1]-3,'$p_2=k(\\frac{2s_g}{3})$', fontsize=40)
plt.text(P3[0]-3, P3[1]-3,'$p_3=k(s_g)$', fontsize=40)
plt.axis('equal')
figManager = plt.get_current_fig_manager()
figManager.window.showMaximized()
# plt.savefig('img/coordinate_transform.png',dpi=600)
plt.show()
cursor.close()
conn.close()
def test_workspace():
# veh = Vehicle(trajectory=np.array([[-1.,-1.,2.,30.,0.,0.,0.,0.,0.,0.]]))
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=100., q=(2.,15.,0))
road = Road(center_line=center_line)
#
cfg = road.ij2xy(5,-4)
traj = np.array([[-1,-1,cfg[0],cfg[1],cfg[2],cfg[3],0.,0.,0.,0.]])
veh = Vehicle(trajectory=traj)
#
lane_costs = [0.3,0.6,0.9]
#static_obsts
cs1 = road.ij2xy(20,8)
cs2 = road.ij2xy(50,-6)
traj1 = np.array([[-1,-1,cs1[0],cs1[1],cs1[2],cs1[3],0.,0.,0.,0.]])
traj2 = np.array([[-1,-1,cs2[0],cs2[1],cs2[2],cs2[3],0.,0.,0.,0.]])
ob1 = Vehicle(trajectory=traj1)
ob2 = Vehicle(trajectory=traj2)
obsts = [ob1,ob2]
#
ws = Workspace(vehicle=veh, road=road, lane_costs=lane_costs, static_obsts=obsts)
# test_disk = ws.disk_filter(2)
# test_veh = ws.vehicle_filter(theta = np.pi/4)
#
fig = plt.figure()
ax1 = fig.add_subplot(111)
lane_map = 0.2*ws.lane_grids[0] + 0.4*ws.lane_grids[1]
lane_map = np.where(lane_map > 0.4, 0.4, lane_map)
lane_map += 0.6*ws.lane_grids[2]
lane_map = np.where(lane_map > 0.6, 0.6, lane_map)
ax1.imshow(lane_map, cmap=plt.cm.Greens, origin="lower",extent=(0.,100.,0.,100.))
# ax1.colorbar()
import pylab as pl
mycmap = plt.cm.Greens
mynames=['low cost dense','intermediate cost dense','high cost dense']
for entry in [0.2,0.4,0.6]:
mycolor = mycmap(255)
pl.plot(0, 0, "-", c=mycolor, alpha=entry, label=mynames[int(entry*5-1)], linewidth=8)
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='black', linewidth=2.)
# else:
# ax1.plot(road.longitudinal_lines[:,2*i], road.longitudinal_lines[:,2*i+1], color='black', linewidth=1.)
# 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)
#ax1.imshow(ws.cost_filter, cmap=plt.cm.Greens, origin="lower",extent=(0.,ws.resolution*ws.cost_filter.shape[1],0.,ws.resolution*ws.cost_filter.shape[0]))
# np.savetxt('cost_filter.txt',ws.cost_filter,fmt='%i',delimiter=' ')
# plt.savefig('img/road_cost.png', dpi=600)
# plt.axis('equal')
plt.legend()
plt.xlabel('$x (m)$', fontsize=20)
plt.ylabel('$y (m)$', fontsize=20)
# plt.savefig('img/road_cost.png', dpi=600)
plt.show()
def test_traj():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
q0 = (100.,100.,0., 0.02)
q1 = (125.,130.,np.pi/6,-0.02)
p,r = TG.calc_path(cursor,q0,q1)
print('p={0},r={1}'.format(p,r))
cursor.close()
conn.close()
def test_cost_maps5():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=100.,q=(3.,25.,0.))
road = Road(center_line, ref_grid_width=0.5, ref_grid_length=1.)
cost_maps = np.zeros((150,500,500))
with open('scenario_5/cost_maps.pickle', 'rb') as f1:
cost_maps = pickle.load(f1)
# heuristic_maps = np.zeros((150,500,500))
# with open('scenario_5/heuristic_maps.pickle', 'rb') as f2:
# heuristic_maps = pickle.load(f2)
heuristic_maps = np.zeros((150,500,500))
goal = tuple(road.sl2xy(90.,0.))
for i in range(150):
hm = heuristic_map_constructor(goal, cost_maps[i,:,:])
heuristic_maps[i,:,:] = hm
with open('scenario_5/heuristic_maps.pickle','wb') as f2:
pickle.dump(heuristic_maps, f2)
fig = plt.figure()
ax1 = fig.add_subplot(331)
ax2 = fig.add_subplot(332)
ax3 = fig.add_subplot(333)
ax4 = fig.add_subplot(334)
ax5 = fig.add_subplot(335)
ax6 = fig.add_subplot(336)
ax7 = fig.add_subplot(337)
ax8 = fig.add_subplot(338)
ax9 = fig.add_subplot(339)
ax = [ax1,ax2,ax3,ax4,ax5,ax6,ax7,ax8,ax9]
for i in range(9):
# cost_map = cost_maps[int(i*10),:,:]
# cost_map = np.where(cost_map>np.finfo('d').max, 1., cost_map)
# ax[i].imshow(cost_map, cmap=plt.cm.Blues, origin='lower', extent=(0,100,0,100))
heuristic_map = heuristic_maps[int(i*10),:,:]
ax[i].imshow(heuristic_map, cmap=plt.cm.Blues, origin='lower', extent=(0,100,0,100))
plt.show()
# print(heuristic_maps[0,:,:])
# hm = heuristic_map_constructor(goal, cost_maps[0,:,:])
# for i in range(500):
# for j in range(500):
# if not np.isinf(hm[i,j]):
# print(hm[i,j])
# plt.imshow(hm, cmap=plt.cm.Blues, origin='lower', extent=(0,100,0,100))
# plt.show()
cursor.close()
conn.close()
def test_different_initval():
# conn = sqlite3.connect('InitialGuessTable.db')
# cursor = conn.cursor()
q0 = (0.,0.,0.,0.07)
q1 = (43.21, 10.53, 0.47, -0.03)
# init_val = (-0.011680200827827, 0.027105283154636, 45.369339753738281)
init_val = (-0.010248903904925, 0.000396612698255, 45.158514552345096)
bd_con = (0.07, 43.21, 10.53, 0.47, -0.03)
pp = TG.optimize(bd_con, init_val=init_val)
print(pp)
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 test_database():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
k0,k1=0.,0.
cursor.execute('select p1,p2,sg from InitialGuessTable where k0=? and k1=? and y1>=0 and theta1>=0',(int(k0*40),int(k1*40)))
pps = cursor.fetchall()
t=0
for pp in pps:
if pp[2]>0:
t+=1
path = TG.spiral3_calc(p=(k0,pp[0],pp[1],k1,pp[2]))
plt.plot(path[:,1],path[:,2])
print(t)
plt.title('k0 = {0}, k1 = {1}'.format(k0,k1))
plt.axis('equal')
plt.savefig('img/initialguesstable(k0={0}k1={1}).png'.format(k0,k1),dpi=600)
plt.show()
cursor.close()
conn.close()
def eval_trajectory(traj, costmap, vehicle=Env.Vehicle(), resolution=0.2, time_resolution=0.1, static=True, weights=np.array([1.5, 2., 2., 3.]), p_lims=(0.2,0.15,20.01,-0.01,2.1,-6.1,9.81), truncate=False):
"""
evaluate the cost of given trajctory
trajectory - [(t, s, x, y, theta, k, dk, v, a)]
weights - trajectory length, trajectory time, end velocity, environment
p_lims - { k_m, dk_m, v_max, v_min, a_max, a_min, ac_m }
"""
cost_matrix = np.zeros((traj.shape[0], 6)) # k, dk, v, a, a_c, env
cost_matrix[:,0] = np.where(np.abs(traj[:,5])>p_lims[0], np.inf, 0.) # k
cost_matrix[:,1] = np.where(np.abs(traj[:,6])>p_lims[1], np.inf, 0.) # dk
cost_matrix[:,2] = np.where(traj[:,7]>p_lims[2], np.inf, 0.) # v
cost_matrix[:,2] = np.where(traj[:,7]<p_lims[3], np.inf, 0.) # v
cost_matrix[:,3] = np.where(traj[:,8]>p_lims[4], np.inf, 0.) # a
cost_matrix[:,3] = np.where(traj[:,8]<p_lims[5], np.inf, 0.) # a
cost_matrix[:,4] = np.where(np.abs(traj[:,7]**2 * traj[:,5])>p_lims[6], np.inf, 0.) # a_c
cost_matrix[:,5] = weights[3] * TG.query_cost(traj,costmap,static=static,vehicle=vehicle,resolution=resolution, time_resolution=time_resolution)[:,0]
cost_sum = cost_matrix.sum()
if np.isinf(cost_sum):
return np.inf
return cost_sum*(traj[1,1]-traj[0,1]) + weights[0]*(traj[-1,1]-traj[0,1]) - 0.6*np.abs(traj[-1,7]) #+ weights[1]*(traj[-1,0] - traj[0,0]) #+ weights[2]*np.abs(traj[-1,7])
def test_optimize():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
bd_con = (-0.01, 70., 30., np.pi/6, 0.01)
init_val = (0., 0., 100)
# init_val = (-0.0033333333333333335, 0.0033333333333333335, 78.775724936630581)
pp1 = TG.optimize(bd_con, init_val=init_val)
p1 = (bd_con[0], pp1[0], pp1[1], bd_con[4], pp1[2])
r1 = (TG.__a(p1), TG.__b(p1), TG.__c(p1), TG.__d(p1))
print('p1 = {0}'.format(p1))
# q0 = (0., 0., 0., -0.01)
# q1 = (70., 30., np.pi/6., 0.01)
# p2, r2 = TG.calc_path(cursor, q0, q1)
# print('p2 = {0}'.format(p2))
line1 = TG.spiral3_calc(p1, r=r1, q=(0.,0.,0.))
# line2 = TG.spiral3_calc(p2, r=r2, q=q0)
plt.plot(line1[:,1], line1[:,2], color='black', linewidth=4)
# plt.plot(line2[:,1], line2[:,2], color='green', linewidth=4)
plt.axis('equal')
# figManager = plt.get_current_fig_manager()
# figManager.window.showMaximized()
# plt.savefig('img/coordinate_transform.png',dpi=600)
plt.show()
cursor.close()
conn.close()
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()
def costmap_plot():
# 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.)
plt.axis('equal')
plt.show()
def test():
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, q=(0.,0.,0.))
print(center_line[-1,:])
def senarios_4():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
fig = plt.figure()
ax = fig.add_subplot(111)
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=100.,q=(3.,25.,0.))
road = Road(center_line, ref_grid_width=0.5, ref_grid_length=1.)
# ax.plot(center_line[:,1], center_line[:,2], color='red', linewidth=1.)
ax.plot(road.lateral_lines[:,0], road.lateral_lines[:,1],color='green', linewidth=1.)
ax.plot(road.lateral_lines[:,-2], road.lateral_lines[:,-1],color='green', linewidth=1.)
for i in range(road.grid_num_lateral+1):
if (i % road.grid_num_per_lane) == 0:
ax.plot(road.longitudinal_lines[:,2*i], road.longitudinal_lines[:,2*i+1], color='green', linewidth=1.)
#
# ws = Workspace(road=road, lane_costs=[0.4,0.1,0.2])
# cost_map_base = ws.lane_map
# ax.imshow(ws.lane_map, cmap=plt.cm.Blues, origin='lower', extent=(0,100,0,100))
s1 = State(time=0.,length=0.,road=road,r_s=25.,r_l=road.lane_width, v=15.)
g1 = State(road=road,r_s=95.,r_l=road.lane_width,v=15.)
traj1 = trajectory_forward(s1,g1,cursor)
# print(traj1[-1,:])
# ax.plot(traj1[:,2], traj1[:,3], color='navy', linewidth=2.)
s2 = State(time=0.,length=0.,road=road,r_s=20.,r_l=0.,v=6.)
g2 = State(road=road,r_s=95.,r_l=0.,v=6.)
traj2 = trajectory_forward(s2,g2,cursor)
# print(traj2[-1,:])
# ax.plot(traj2[:,2], traj2[:,3], color='navy', linewidth=2.)
cfg3 = road.sl2xy(30.,-road.lane_width)
obst_s = Vehicle(trajectory=np.array([[-1.,-1.,cfg3[0], cfg3[1], cfg3[2], cfg3[3], 0.,0.,0.]]))
cfg0 = road.sl2xy(5.,0.)
veh0 = Vehicle(trajectory=np.array([[-1.,-1.,cfg0[0], cfg0[1], cfg0[2], cfg0[3], 0.,0.,0.]]))
cfg1 = road.sl2xy(90.,0.)
veh1 = Vehicle(trajectory=np.array([[-1.,-1.,cfg1[0], cfg1[1], cfg1[2], cfg1[3], 0.,0.,0.]]))
ax.plot(cfg0[0], cfg0[1], 'ko')
ax.text(cfg0[0], cfg0[1]+0.4, 'Start')
ax.plot(cfg1[0], cfg1[1], 'ko')
ax.text(cfg1[0], cfg1[1]+0.4, 'Goal')
#
codes6 = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
verts_s = [tuple(obst_s.vertex[i]) for i in range(6)]
verts_s.append(verts_s[0])
ax.add_patch(patches.PathPatch(Path(verts_s, codes6), facecolor='cyan'))
verts0 = [tuple(veh0.vertex[i]) for i in range(6)]
verts0.append(verts0[0])
ax.add_patch(patches.PathPatch(Path(verts0, codes6), facecolor='green', alpha=0.5))
verts1 = [tuple(veh1.vertex[i]) for i in range(6)]
verts1.append(verts1[0])
ax.add_patch(patches.PathPatch(Path(verts1, codes6), facecolor='red', alpha=0.5))
for i in range(20):
state1 = trajectory_interp(traj1, i*0.2)
state2 = trajectory_interp(traj2, i*0.2)
if state1 is not None:
obst_d1 = Vehicle(trajectory=np.array([[-1.,-1.,state1[2], state1[3], state1[4], 0., 0.,0.,0.]]))
verts_d1 = [tuple(obst_d1.vertex[i]) for i in range(6)]
verts_d1.append(verts_d1[0])
ax.add_patch(patches.PathPatch(Path(verts_d1, codes6), facecolor='cyan', alpha=(i+1)/20.))
if state2 is not None:
obst_d2 = Vehicle(trajectory=np.array([[-1.,-1.,state2[2], state2[3], state2[4], 0., 0.,0.,0.]]))
verts_d2 = [tuple(obst_d2.vertex[i]) for i in range(6)]
verts_d2.append(verts_d2[0])
ax.add_patch(patches.PathPatch(Path(verts_d2, codes6), facecolor='cyan', alpha=(i+1)/20.))
#
# cost_maps = np.zeros((150,500,500))
# grids_s = ws.grids_occupied_by_polygon(obst_s.vertex)
# lane_grids = sum(ws.lane_grids)
# lane_grids = np.where(lane_grids>1.,1., lane_grids)
# off_road_map = 1. - lane_grids
# grids_s += off_road_map
# for i in range(150):
# obst_map = np.zeros((500,500))
# obst_map += grids_s
# state1 = trajectory_interp(traj1, i/10.)
# state2 = trajectory_interp(traj2, i/10.)
# if state1 is not None:
# obst_d1 = Vehicle(trajectory=np.array([[-1.,-1.,state1[2], state1[3], state1[4], 0., 0.,0.,0.]]))
# grids_d1 = ws.grids_occupied_by_polygon(obst_d1.vertex)
# obst_map += grids_d1
# if state2 is not None:
# obst_d2 = Vehicle(trajectory=np.array([[-1.,-1.,state2[2], state2[3], state2[4], 0., 0.,0.,0.]]))
# grids_d2 = ws.grids_occupied_by_polygon(obst_d2.vertex)
# obst_map += grids_d2
# collision_map = cv2.filter2D(obst_map, -1, ws.collision_filter)
# collision_map = np.where(collision_map>1.e-6, 1., 0.)
# 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-8, 0., cost_map)
# cost_map += cost_map_base
# cost_maps[i,:,:] = cost_map
# with open('scenario_4/cost_maps.pickle','wb') as f1:
# pickle.dump(cost_maps, f1)
# plt.xlabel('$x (m)$', fontsize=20)
# plt.ylabel('$y (m)$', fontsize=20)
plt.axis('equal')
# plt.axis('off')
# plt.savefig('scenario_4/obstacles2.png', dpi=600)
plt.show()
cursor.close()
conn.close()
def env_plot():
# 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)
# 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.]]))
verts1 = [tuple(obst1.vertex[i]) for i in range(6)]
verts1.append(verts1[0])
verts2 = [tuple(obst2.vertex[i]) for i in range(6)]
verts2.append(verts2[0])
verts3 = [tuple(obst3.vertex[i]) for i in range(6)]
verts3.append(verts3[0])
verts4 = [tuple(obst4.vertex[i]) for i in range(6)]
verts4.append(verts4[0])
codes = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
path1 = Path(verts1, codes)
patch1 = patches.PathPatch(path1,facecolor='green')
ax1.add_patch(patch1)
path2 = Path(verts2, codes)
patch2 = patches.PathPatch(path2,facecolor='green')
ax1.add_patch(patch2)
path3 = Path(verts3, codes)
patch3 = patches.PathPatch(path3,facecolor='green')
ax1.add_patch(patch3)
path4 = Path(verts4, codes)
patch4 = patches.PathPatch(path4,facecolor='green')
ax1.add_patch(patch4)
plt.axis('equal')
plt.show()
def test_road():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=70., q=(5.,30.,np.pi/9))
road = Road(center_line, ref_grid_width=1, ref_grid_length=2.)
fig = plt.figure()
ax1 = fig.add_subplot(111)
# ax1.plot(center_line[:,1], center_line[:,2], color='red', linestyle='--', linewidth=2.)
ax1.plot(center_line[:,1], center_line[:,2], color='red', linewidth=1.)
ax1.plot(road.lateral_lines[:,0], road.lateral_lines[:,1],color='red', linewidth=1.)
ax1.plot(road.lateral_lines[:,-2], road.lateral_lines[:,-1],color='green', linewidth=1.)
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.)
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)
#
cfg0 = road.ij2xy(2,-4)
veh = Vehicle(trajectory=np.array([[-1.,-1.,cfg0[0], cfg0[1], cfg0[2], cfg0[3], 0.,5.,0.]]))
verts0 = [tuple(veh.vertex[i]) for i in range(6)]
verts0.append(verts0[0])
cfg1 = road.ij2xy(15, -4)
cfg2 = road.ij2xy(15, 0)
cfg3 = road.ij2xy(30, 0)
cfg4 = road.ij2xy(30., 4)
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.]]))
verts1 = [tuple(obst1.vertex[i]) for i in range(6)]
verts1.append(verts1[0])
verts2 = [tuple(obst2.vertex[i]) for i in range(6)]
verts2.append(verts2[0])
verts3 = [tuple(obst3.vertex[i]) for i in range(6)]
verts3.append(verts3[0])
verts4 = [tuple(obst4.vertex[i]) for i in range(6)]
verts4.append(verts4[0])
codes = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
path0 = Path(verts0, codes)
patch0 = patches.PathPatch(path0,facecolor='cyan')
ax1.add_patch(patch0)
path1 = Path(verts1, codes)
patch1 = patches.PathPatch(path1,facecolor='cyan')
ax1.add_patch(patch1)
path2 = Path(verts2, codes)
patch2 = patches.PathPatch(path2,facecolor='cyan')
ax1.add_patch(patch2)
path3 = Path(verts3, codes)
patch3 = patches.PathPatch(path3,facecolor='cyan')
ax1.add_patch(patch3)
path4 = Path(verts4, codes)
patch4 = patches.PathPatch(path4,facecolor='cyan')
ax1.add_patch(patch4)
#
ax1.plot([cfg0[0], cfg1[0], cfg2[0], cfg3[0], cfg4[0]], [cfg0[1], cfg1[1], cfg2[1], cfg3[1], cfg4[1]], 'ro')
for q1 in [cfg1, cfg2]:
p, r = TG.calc_path(cursor, cfg0, q1)
line = TG.spiral3_calc(p, r=r, q=cfg0)
ax1.plot(line[:,1], line[:,2], color='magenta', linewidth=2)
for q0 in [cfg1, cfg2]:
for q1 in [cfg3, cfg4]:
p, r = TG.calc_path(cursor, q0, q1)
line = TG.spiral3_calc(p, r=r, q=q0)
ax1.plot(line[:,1], line[:,2], color='magenta', linewidth=2)
#
plt.xlabel('$x (m)$', fontsize=20)
plt.ylabel('$y (m)$', fontsize=20)
plt.axis('equal')
# plt.axis('off')
# plt.savefig('img/road_grid.png', dpi=600)
# plt.savefig('img/road_shape.png', dpi=600)
# plt.savefig('img/road_spiral3.png', dpi=600)
plt.show()
cursor.close()
conn.close()
from Simulation_env import Simulation_env
import TrajectoryGeneration
import numpy as np
""" generate and run trajectories"""
# initialize test environment
nbins = 10
num_actions = 4
action_lims = np.array([[-1,1],[-1,1],[-1,1],[-1,1]])
env = Simulation_env('bixler3')
trajectories = TrajectoryGeneration.generate_trajectories(num_actions,nbins,action_lims,env)
def test_cost_fun_discrete():
from math import pi
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
q0 = (5., 0., 0., 0.01)
q1 = (55., 20., pi/6, -0.01)
p, r = TG.calc_path(cursor, q0, q1)
print('p={0},r={1}'.format(p,r))
sg = p[4]
line = TG.spiral3_calc(p, r=r, q=q0)
P0 = q0[0:3]
P1 = TG.spiral3_calc(p, r=r, s=sg/3, q=q0)[-1, 1:4]
P2 = TG.spiral3_calc(p, r=r, s=2*sg/3, q=q0)[-1, 1:4]
P3 = TG.spiral3_calc(p, r=r, s=sg, q=q0)[-1, 1:4]
####################
veh0 = Vehicle(trajectory=np.array([[-1.,-1.,P0[0],P0[1],P0[2],0.,0.,0.,0.]]))
veh1 = Vehicle(trajectory=np.array([[-1.,-1.,P1[0],P1[1],P1[2],0.,0.,0.,0.]]))
veh2 = Vehicle(trajectory=np.array([[-1.,-1.,P2[0],P2[1],P2[2],0.,0.,0.,0.]]))
veh3 = Vehicle(trajectory=np.array([[-1.,-1.,P3[0],P3[1],P3[2],0.,0.,0.,0.]]))
fig = plt.figure()
ax1 = fig.add_subplot(111)
# ax2 = fig.add_subplot(122)
ax1.plot(line[:,1], line[:,2], color='black', linewidth=4)
# ax2.plot(line[:,1], line[:,2], color='black', linewidth=4)
verts0 = [tuple(veh0.vertex[i]) for i in range(6)]
verts0.append(verts0[0])
verts1 = [tuple(veh1.vertex[i]) for i in range(6)]
verts1.append(verts1[0])
verts2 = [tuple(veh2.vertex[i]) for i in range(6)]
verts2.append(verts2[0])
verts3 = [tuple(veh3.vertex[i]) for i in range(6)]
verts3.append(verts3[0])
codes = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
path0 = Path(verts0, codes)
path1 = Path(verts1, codes)
path2 = Path(verts2, codes)
path3 = Path(verts3, codes)
patch0 = patches.PathPatch(path0,facecolor='green')
patch1 = patches.PathPatch(path1,facecolor='green')
patch2 = patches.PathPatch(path2,facecolor='green')
patch3 = patches.PathPatch(path3,facecolor='green')
ax1.add_patch(patch0)
ax1.add_patch(patch1)
ax1.add_patch(patch2)
ax1.add_patch(patch3)
plt.axis('equal')
# plt.axis('off')
plt.show()
cursor.close()
conn.close()
def test_transition():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=70., q=(5.,30.,np.pi/9))
road = Road(center_line, ref_grid_width=0.5, ref_grid_length=1.)
fig = plt.figure()
ax1 = fig.add_subplot(111)
# ax1.plot(center_line[:,1], center_line[:,2], color='red', linestyle='--', linewidth=2.)
ax1.plot(center_line[:,1], center_line[:,2], color='red', linewidth=1.)
ax1.plot(road.lateral_lines[:,0], road.lateral_lines[:,1],color='red', linewidth=1.)
ax1.plot(road.lateral_lines[:,-2], road.lateral_lines[:,-1],color='green', linewidth=1.)
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.)
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)
#
cfg0 = road.ij2xy(4, 0)
veh = Vehicle(trajectory=np.array([[-1.,-1.,cfg0[0], cfg0[1], cfg0[2], cfg0[3], 0.,5.,0.]]))
# verts0 = [tuple(veh.vertex[i]) for i in range(6)]
# verts0.append(verts0[0])
# codes = [Path.MOVETO,
# Path.LINETO,
# Path.LINETO,
# Path.LINETO,
# Path.LINETO,
# Path.LINETO,
# Path.CLOSEPOLY,
# ]
# path0 = Path(verts0, codes)
# patch0 = patches.PathPatch(path0,facecolor='cyan')
# ax1.add_patch(patch0)
#
cm = np.zeros((500,500))
hm = np.zeros((500,500))
goal = State(road=road,r_i=15,r_j=-2,v=6.)
goal2 = State(road=road,r_s=goal.r_s+0.3,r_l=goal.r_l+0.1,v=6.)
goal1 = State(road=road,r_s=goal.r_s+0.7,r_l=goal.r_l+0.3,v=6.)
ax1.text(goal1.x, goal1.y+0.4, 'g1', fontsize=15)
ax1.text(goal2.x, goal2.y-0.4, 'g2', fontsize=15)
ax1.plot(goal1.x, goal1.y, 'mo')
ax1.plot(goal2.x, goal2.y, 'mo')
state1 = State(time=0.,length=0.,road=road,r_i=4,r_j=0,v=5.,cost=0., heuristic_map=hm)
state2 = State(time=0.,length=0.,road=road,r_i=5,r_j=-4,v=5.,cost=0., heuristic_map=hm)
ax1.text(state1.x, state1.y+0.4, 's1', fontsize=15)
ax1.text(state2.x, state2.y-0.4, 's2', fontsize=15)
ax1.plot(state1.x, state1.y, 'mo')
ax1.plot(state2.x, state2.y, 'mo')
# ax1.plot(state.x, state.y, 'rs')
# for s1 in [state1, state2]:
# for s2 in [goal1,goal2]:
# traj = trajectory(s1,s2,cursor)
# if traj is not None:
# ax1.plot(traj[:,2], traj[:,3], linewidth=2)
# ax1.plot(traj[0,2], traj[0,3], 'mo')
# ax1.plot(traj[-1,2], traj[-1,3], 'mo')
traj1 = trajectory(state1,goal1,cursor)
traj2 = trajectory(state2,goal2,cursor)
traj3 = trajectory(state2,goal1,cursor)
ax1.plot(traj1[:,2], traj1[:,3], linewidth=2., color='red')
ax1.plot(traj2[:,2], traj2[:,3], linewidth=2., color='black')
ax1.plot(traj3[:,2], traj3[:,3], 'b--', linewidth=2.)
# node_dict = {}
# # successors = state.successors(state_dict=node_dict, road=road, goal=goal, vehicle=veh, heuristic_map=hm, accs=[-3., -2., -1., 0., 1.], v_offset=[-1.,-0.5, 0., 0.5, 1.],times=[3.])
# successors = state.successors(state_dict=node_dict, road=road, goal=goal, vehicle=veh, heuristic_map=hm, times = [2.5]) # accs=[ 0.], v_offset=[-1.,-0.5, 0., 0.5, 1.],times=[1.,2.,4.]
# print("number of successors: {0}".format(len(successors)))
# for successor in successors:
# # p, r = TG.calc_path(cursor, state.q, successor.q)
# # if p is not None and p[4]>0:
# # line = TG.spiral3_calc(p, r=r, q=state.q)
# # ax1.plot(successor.x, successor.y, 'mo')
# # ax1.plot(line[:,1], line[:,2], linewidth=2)
# traj1 = trajectory(state,successor,cursor)
# if traj1 is not None:
# _, traj, _ = TG.eval_trajectory(traj1,costmap=cm,road=road)
# if traj is not None:
# ax1.plot(traj[:,2], traj[:,3], linewidth=2)
# ax1.plot(traj[-1,2], traj[-1,3], 'mo')
#
# ax1.plot([cfg0[0], cfg1[0], cfg2[0], cfg3[0], cfg4[0]], [cfg0[1], cfg1[1], cfg2[1], cfg3[1], cfg4[1]], 'ro')
# for q1 in [cfg1, cfg2]:
# p, r = TG.calc_path(cursor, cfg0, q1)
# line = TG.spiral3_calc(p, r=r, q=cfg0)
# ax1.plot(line[:,1], line[:,2], color='magenta', linewidth=2)
# for q0 in [cfg1, cfg2]:
# for q1 in [cfg3, cfg4]:
# p, r = TG.calc_path(cursor, q0, q1)
# line = TG.spiral3_calc(p, r=r, q=q0)
# ax1.plot(line[:,1], line[:,2], color='magenta', linewidth=2)
#
plt.xlabel('$x (m)$', fontsize=20)
plt.ylabel('$y (m)$', fontsize=20)
plt.axis('equal')
# plt.axis('off')
# plt.savefig('img/road_grid.png', dpi=600)
# plt.savefig('img/road_shape.png', dpi=600)
# plt.savefig('img/road_spiral3.png', dpi=600)
plt.show()
cursor.close()
conn.close()
def scenario_4_sim():
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
cost_maps = np.zeros((150,500,500))
with open('scenario_4/cost_maps.pickle', 'rb') as f1:
cost_maps = pickle.load(f1)
heuristic_maps = np.zeros((150,500,500))
with open('scenario_4/heuristic_maps.pickle', 'rb') as f2:
heuristic_maps = pickle.load(f2)
fig = plt.figure()
ax = fig.add_subplot(111)
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=100.,q=(3.,25.,0.))
road = Road(center_line, ref_grid_width=0.5, ref_grid_length=1.)
# ax.plot(center_line[:,1], center_line[:,2], color='red', linewidth=1.)
ax.plot(road.lateral_lines[:,0], road.lateral_lines[:,1],color='green', linewidth=1.)
ax.plot(road.lateral_lines[:,-2], road.lateral_lines[:,-1],color='green', linewidth=1.)
for i in range(road.grid_num_lateral+1):
if (i % road.grid_num_per_lane) == 0:
ax.plot(road.longitudinal_lines[:,2*i], road.longitudinal_lines[:,2*i+1], color='green', linewidth=1.)
goal = State(road=road, r_s=90., r_l=0., v=10.,static=False)
start = State(time=0., length=0., road=road, r_s=5., r_l=0., v=10.,cost=0.,heuristic_map=heuristic_maps, static=False)
veh = Vehicle(trajectory=np.array([[-1.,-1.,start.x, start.y, start.theta, start.k, 0.,0.,0.]]))
# # weights: weights for (k, dk, v, a, a_c, l, env, j, t, s)
weights = np.array([5., 10., -0.1, 10., 0.1, 1., 50., 5, 30., -2.])
starttime = datetime.datetime.now()
res, state_dict, traj_dict = Astar(start, goal, road, cost_maps, veh, heuristic_maps, cursor, static=False, weights=weights)
endtime = datetime.datetime.now()
print((endtime - starttime).total_seconds()*1000) # 4.8s
print(res)
print(len(state_dict)) #96
print(len(traj_dict))
# for _ , traj in traj_dict.items():
# ax.plot(traj[:,2], traj[:,3], traj[:,0], color='navy', linewidth=0.3)
# ax.plot(traj[:,2], traj[:,3], color='blue', linewidth=1.)
# for _, state in state_dict.items():
# if state != start and state != goal:
# ax.plot(state.x, state.y, 'go')
# ax.text(state.x, state.y,'{0:.2f}'.format(state.cost))
state = goal
rows = 0
while state.parent is not None:
traj = traj_dict[(state.parent, state)]
ax.plot(traj[:,2], traj[:,3], color='magenta', linewidth=3.)
rows += traj.shape[0]
ax.plot(state.x, state.y, 'go')
ax.plot(state.parent.x, state.parent.y, 'go')
state = state.parent
# ax.plot(traj[:,2], traj[:,3], traj[:,0], color='teal', linewidth=1.)
# ax.plot(traj[:,0], traj[:,7], color='black', linewidth=0.5)
# print(rows)
final_traj=np.zeros((rows,9))
state = goal
# row = 0
while state.parent is not None:
traj = traj_dict[(state.parent, state)]
final_traj[(rows-traj.shape[0]):rows,:] = traj
rows -= traj.shape[0]
# row += traj.shape[0]
state = state.parent
with open('scenario_4/final_traj.pickle','wb') as f3:
pickle.dump(final_traj, f3)
#
#################
s1 = State(time=0.,length=0.,road=road,r_s=25.,r_l=road.lane_width, v=15.)
g1 = State(road=road,r_s=95.,r_l=road.lane_width,v=15.)
traj1 = trajectory_forward(s1,g1,cursor)
# print(traj1[-1,:])
# ax.plot(traj1[:,2], traj1[:,3], color='navy', linewidth=2.)
s2 = State(time=0.,length=0.,road=road,r_s=20.,r_l=0.,v=6.)
g2 = State(road=road,r_s=95.,r_l=0.,v=6.)
traj2 = trajectory_forward(s2,g2,cursor)
# print(traj2[-1,:])
# ax.plot(traj2[:,2], traj2[:,3], color='navy', linewidth=2.)
cfg3 = road.sl2xy(30.,-road.lane_width)
obst_s = Vehicle(trajectory=np.array([[-1.,-1.,cfg3[0], cfg3[1], cfg3[2], cfg3[3], 0.,0.,0.]]))
codes6 = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
verts_s = [tuple(obst_s.vertex[i]) for i in range(6)]
verts_s.append(verts_s[0])
ax.add_patch(patches.PathPatch(Path(verts_s, codes6), facecolor='cyan'))
for i in range(31):
state1 = trajectory_interp(traj1, i*goal.time/30)
state2 = trajectory_interp(traj2, i*goal.time/30)
state3 = trajectory_interp(final_traj, i*goal.time/30)
if state1 is not None:
obst_d1 = Vehicle(trajectory=np.array([[-1.,-1.,state1[2], state1[3], state1[4], 0., 0.,0.,0.]]))
verts_d1 = [tuple(obst_d1.vertex[i]) for i in range(6)]
verts_d1.append(verts_d1[0])
ax.add_patch(patches.PathPatch(Path(verts_d1, codes6), facecolor='cyan', alpha=0.1+0.03*i))
if state2 is not None:
obst_d2 = Vehicle(trajectory=np.array([[-1.,-1.,state2[2], state2[3], state2[4], 0., 0.,0.,0.]]))
verts_d2 = [tuple(obst_d2.vertex[i]) for i in range(6)]
verts_d2.append(verts_d2[0])
ax.add_patch(patches.PathPatch(Path(verts_d2, codes6), facecolor='cyan', alpha=0.1+0.03*i))
if state3 is not None:
obst_d3 = Vehicle(trajectory=np.array([[-1.,-1.,state3[2], state3[3], state3[4], 0., 0.,0.,0.]]))
verts_d3 = [tuple(obst_d3.vertex[i]) for i in range(6)]
verts_d3.append(verts_d3[0])
ax.add_patch(patches.PathPatch(Path(verts_d3, codes6), facecolor='blue', alpha=0.1+0.03*i))
plt.axis('equal')
# plt.axis('off')
# plt.savefig('scenario_4/obstacles2.png', dpi=600)
plt.show()
cursor.close()
conn.close()
max_a = 10
dt = 1 / 60.0
drive_base_width = .4
lookahead_at_max_vel = .3
minimum_lookahead = .05
ws = WaypointSequence()
ws.add_waypoint(Pose(0, 0, math.pi / 2))
# ws.add_waypoint(Pose(2, 2, 3 * math.pi/2))
ws.add_waypoint(Pose(-1, 1, math.pi))
config = TrajectoryConfig(max_v, max_a, dt)
trajectory = PathGeneration.generate_from_waypoints(ws, config, 0, 0)
left_right_traj = TrajectoryGeneration.make_left_right_trajectories(
trajectory, drive_base_width)
csv = CsvWriter("C:/Users/Stanl/PycharmProjects/PurePursuit/traj.csv")
time_list = []
left_list = []
right_list = []
for i in range(len(left_right_traj[0])):
left_list.append(left_right_traj[0][i].vel)
right_list.append(left_right_traj[1][i].vel)
time_list.append(left_right_traj[0][1].dt * i)
csv.write_to_csv(time_list, left_list, right_list)
robot = DriveBase(Pose(0, 0, 0), drive_base_width, max_v)
pp = PurePursuit(minimum_lookahead, lookahead_at_max_vel,
left_right_traj[2], drive_base_width, max_v)
controller = CombinedDriveController(pp, left_right_traj, max_v)
def senarios_1():
# database connection
conn = sqlite3.connect('InitialGuessTable.db')
cursor = conn.cursor()
# plot
fig = plt.figure()
# ax1 = fig.add_subplot(111, projection='3d')
ax1 = fig.add_subplot(111)
# ax2 = fig.add_subplot(212)
# road center line points
p = (0.01, 0.0070893847415232263, 0.0056488099243383414, -0.01, 109.61234595301809)
center_line = TG.spiral3_calc(p, s=100.,q=(3.,25.,0.))
# p = (0.,0.,0.,0.,90.) # (p0~p3, sg)
# center_line = TG.spiral3_calc(p, q=(5.,50.,0.))
# np.savetxt('scenario_1/road_center_line.txt', center_line, delimiter='\t')
# print(center_line)
# road
road = Road(center_line)
ax1.plot(center_line[:,1], center_line[:,2], color='maroon', linestyle='--', linewidth=1.)
ax1.plot(road.lateral_lines[:,0], road.lateral_lines[:,1],color='green', linewidth=1.)
ax1.plot(road.lateral_lines[:,-2], road.lateral_lines[:,-1],color='green', linewidth=1.)
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.)
# 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.)
cfg5 = road.sl2xy(90.,0.)
veh = Vehicle(trajectory=np.array([[-1.,-1.,cfg0[0], cfg0[1], cfg0[2], cfg0[3], 0.,5.,0.]]))
veh5 = Vehicle(trajectory=np.array([[-1.,-1.,cfg5[0], cfg5[1], cfg5[2], cfg5[3], 0.,5.,0.]]))
# ax1.plot(cfg0[0], cfg0[1], 'ko')
# ax1.text(cfg0[0], cfg0[1]+0.4, 'Start')
# # ax1.plot(cfg5[0], cfg5[1], 'ko')
# ax1.text(cfg5[0], cfg5[1]+0.4, 'Goal')
codes6 = [Path.MOVETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.LINETO,
Path.CLOSEPOLY,
]
# verts0 = [tuple(veh.vertex[i]) for i in range(6)]
# verts0.append(verts0[0])
# ax1.add_patch(patches.PathPatch(Path(verts0, codes6), facecolor='green', alpha=0.5))
# verts5 = [tuple(veh5.vertex[i]) for i in range(6)]
# verts5.append(verts5[0])
# ax1.add_patch(patches.PathPatch(Path(verts5, codes6), facecolor='red', alpha=0.5))
# 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(30., 0.)
cfg2 = road.sl2xy(30., -road.lane_width)
cfg3 = road.sl2xy(65.,0.)
cfg4 = road.sl2xy(65., 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.]]))
verts1 = [tuple(obst1.vertex[i]) for i in range(6)]
verts1.append(verts1[0])
ax1.add_patch(patches.PathPatch(Path(verts1, codes6), facecolor='cyan'))
verts2 = [tuple(obst2.vertex[i]) for i in range(6)]
verts2.append(verts2[0])
ax1.add_patch(patches.PathPatch(Path(verts2, codes6), facecolor='cyan'))
verts3 = [tuple(obst3.vertex[i]) for i in range(6)]
verts3.append(verts3[0])
ax1.add_patch(patches.PathPatch(Path(verts3, codes6), facecolor='cyan'))
verts4 = [tuple(obst4.vertex[i]) for i in range(6)]
verts4.append(verts4[0])
ax1.add_patch(patches.PathPatch(Path(verts4, codes6), facecolor='cyan'))
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-8, 0., cost_map)
# costmap_save = np.where( cost_map >1., -1., cost_map)
# np.savetxt('scenario_1/cost_map.txt', costmap_save, delimiter='\t')
# plot
# 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))
# heuristic map
goal_state = State(road=road, r_s=90., r_l=0., v=10.)
# ax1.scatter(goal_state.x, goal_state.y, c='r')
ax1.plot(goal_state.x, goal_state.y, 'rs')
heuristic_map = heuristic_map_constructor(goal_state, cost_map)
# hm_save = np.where(heuristic_map > np.finfo('d').max, -1., heuristic_map)
# np.savetxt('scenario_1/heuristic_map.txt', hm_save, delimiter='\t')
start_state = State(time=0., length=0., road=road, r_s=5., r_l=0., v=10.,cost=0., heuristic_map=heuristic_map)
# ax1.scatter(start_state.x, start_state.y, c='r')
ax1.plot(start_state.x, start_state.y, 'rs')
# ax1.imshow(heuristic_map, cmap=plt.cm.Reds, origin="lower",extent=(0.,ws.resolution*ws.row,0.,ws.resolution*ws.column))
# # weights: weights for (k, dk, v, a, a_c, l, env, j, t, s)
weights = np.array([5., 10., -0.1, 10., 0.1, 0.1, 50., 5, 40., -4.])
starttime = datetime.datetime.now()
res, state_dict, traj_dict = Astar(start_state, goal_state, road, cost_map, veh, heuristic_map, cursor, weights=weights)
endtime = datetime.datetime.now()
print((endtime - starttime).total_seconds()*1000) # 2.6s
print(res)
print(len(state_dict)) #66
print(len(traj_dict))
# print(goal_state.time, goal_state.length, goal_state.cost, start_state.heuristic, goal_state.heuristic)
# # True
# 168
# 175
# 8.78814826688 76.409797813 2701.06684421 1559.33663366 0.0
# for _ , traj in traj_dict.items():
# ax1.plot(traj[:,2], traj[:,3], traj[:,0], color='navy', linewidth=0.3)
# ax1.plot(traj[:,2], traj[:,3], color='navy', linewidth=1.)
# for _, state in state_dict.items():
# if state != start_state and state != goal_state:
# ax1.plot(state.x, state.y, 'go')
# ax1.text(state.x, state.y,'{0:.2f}'.format(state.cost))
state = goal_state
rows = 0
while state.parent is not None:
traj = traj_dict[(state.parent, state)]
ax1.plot(traj[:,2], traj[:,3], color='magenta', linewidth=3.)
rows += traj.shape[0]
ax1.plot(state.x, state.y, 'go')
ax1.plot(state.parent.x, state.parent.y, 'go')
state = state.parent
final_traj=np.zeros((rows,9))
state = goal_state
while state.parent is not None:
traj = traj_dict[(state.parent, state)]
final_traj[(rows-traj.shape[0]):rows,:] = traj
rows -= traj.shape[0]
state = state.parent
# with open('scenario_1/final_traj.pickle','wb') as f3:
# pickle.dump(final_traj, f3)
for i in range(31):
state1 = trajectory_interp(final_traj, i*goal_state.time/30)
if state1 is not None:
obst_d1 = Vehicle(trajectory=np.array([[-1.,-1.,state1[2], state1[3], state1[4], 0., 0.,0.,0.]]))
verts_d1 = [tuple(obst_d1.vertex[i]) for i in range(6)]
verts_d1.append(verts_d1[0])
ax1.add_patch(patches.PathPatch(Path(verts_d1, codes6), facecolor='blue', alpha=0.1+0.03*i))
# close database connection
cursor.close()
conn.close()
#
# plt.legend()
plt.axis('equal')
# plt.savefig('scenario_1/astar_3.png', dpi=600)
plt.show()
def path_forward(q0, q1, cursor):
p, r = TG.calc_path(cursor, q0, q1)
if r is not None and p[4] > 0.:
path = TG.spiral3_calc(p, r=r, q=q0)
return path, p, r
return None, None, None
