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 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_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 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_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 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_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 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_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_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()
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 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
