def DrawGraph(self, rnd=None): # pragma: no cover
plt.clf()
ax = plt.axes()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
# plt.plot([node.x, self.nodeList[node.parent].x], [
# node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, oyaw, h, w) in self.obstacleList:
block = Rectangle((ox - 0.5 * w, oy - 0.5 * h),
w,
h,
angle=np.rad2deg(oyaw),
color='k')
ax.add_patch(block)
reeds_shepp_path_planning.plot_arrow(self.start.x, self.start.y,
self.start.yaw)
reeds_shepp_path_planning.plot_arrow(self.end.x, self.end.y,
self.end.yaw)
plt.axis([-2, 22, -2, 22])
plt.gca().set_aspect('equal', adjustable='box')
plt.grid(True)
plt.pause(0.01)
def DrawGraph(self, rnd=None):
"""
Draw Graph
"""
plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
# plt.plot(node.path_x, node.path_y, "-g")
pNode = self.nodeList[node.parent]
plt.plot([pNode.x, node.x], [pNode.y, node.y], "-g")
# plt.plot([node.x, self.nodeList[node.parent].x], [
# node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
reeds_shepp_path_planning.plot_arrow(self.start.x, self.start.y,
self.start.yaw)
reeds_shepp_path_planning.plot_arrow(self.end.x, self.end.y,
self.end.yaw)
plt.axis(
[self.minrand_x, self.maxrand_x, self.minrand_y, self.maxrand_y])
plt.grid(True)
plt.pause(0.01)
def DrawGraph(self, rnd=None):
u"""
Draw Graph
"""
import matplotlib.pyplot as plt
plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
# plt.plot([node.x, self.nodeList[node.parent].x], [
# node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, size) in obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
reeds_shepp_path_planning.plot_arrow(self.start.x, self.start.y,
self.start.yaw)
reeds_shepp_path_planning.plot_arrow(self.end.x, self.end.y,
self.end.yaw)
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def main():
print("Start Hybrid A* planning")
ox, oy = [], []
for i in range(3):
ox.append(18 - 0.9 * i)
oy.append(17 + 0.5 * i)
# for i in range(60):
# ox.append(i)
# oy.append(0.0)
# for i in range(60):
# ox.append(60.0)
# oy.append(i)
# for i in range(61):
# ox.append(i)
# oy.append(60.0)
# for i in range(61):
# ox.append(0.0)
# oy.append(i)
# for i in range(40):
# ox.append(20.0)
# oy.append(i)
# for i in range(40):
# ox.append(40.0)
# oy.append(60.0 - i)
# Set Initial parameters
start = [10.0, 10.0, np.deg2rad(90.0)]
goal = [50.0, 50.0, np.deg2rad(90.0)]
# print("start : ", start)
# print("goal : ", goal)
if show_animation:
plt.plot(ox, oy, ".k")
rs.plot_arrow(start[0], start[1], start[2], fc='g')
rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True)
plt.axis("equal")
path = hybrid_a_star_planning(start, goal, ox, oy, XY_GRID_RESOLUTION,
YAW_GRID_RESOLUTION)
x = path.x_list
y = path.y_list
yaw = path.yaw_list
if show_animation:
for i_x, i_y, i_yaw in zip(x, y, yaw):
plt.cla()
plt.plot(ox, oy, ".k")
plt.plot(x, y, "-r", label="Hybrid A* path")
plt.grid(True)
plt.axis("equal")
plot_car(i_x, i_y, i_yaw)
plt.pause(0.0001)
print(__file__ + " done!!")
def DrawGraph(self, rnd=None):
u"""
Draw Graph
"""
import matplotlib.pyplot as plt
# plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
pass
# plt.plot([node.x, self.nodeList[node.parent].x], [
# node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, size) in obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
reeds_shepp_path_planning.plot_arro/Users/atsushisakai/Dropbox/Program/berkeley/RacerCourseOptimization/cvxpy/stanford/mpc_path_planner_common.pyw(
self.start.x, self.start.y, self.start.yaw)
reeds_shepp_path_planning.plot_arrow(
self.end.x, self.end.y, self.end.yaw)
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def main():
print("Start Hybrid A* planning")
ox, oy = [], []
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
# Set Initial parameters
start = [10.0, 10.0, np.deg2rad(90.0)]
goal = [50.0, 50.0, np.deg2rad(-90.0)]
plt.plot(ox, oy, ".k")
rs.plot_arrow(start[0], start[1], start[2], fc='g')
rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True)
plt.axis("equal")
path = hybrid_a_star_planning(start, goal, ox, oy, XY_GRID_RESOLUTION,
YAW_GRID_RESOLUTION)
# plot_car(*start)
# plot_car(*goal)
x = path.xlist
y = path.ylist
yaw = path.yawlist
# direction = path.directionlist
for ix, iy, iyaw in zip(x, y, yaw):
plt.cla()
plt.plot(ox, oy, ".k")
plt.plot(x, y, "-r", label="Hybrid A* path")
plt.grid(True)
plt.axis("equal")
plot_car(ix, iy, iyaw)
plt.pause(0.0001)
plt.show()
print(__file__ + " start!!")
def main():
start_time = time.clock()
print("Start Hybrid A* planning")
minx = min(ox_all) + 4000
maxx = max(ox_all) - 4000
miny = min(oy_all) + 5000
maxy = max(oy_all) - 15000
point_start = [random.uniform(minx, maxx), random.uniform(miny, maxy)]
point_goal = [random.uniform(minx, maxx), random.uniform(miny, maxy)]
degree = math.atan2(point_goal[1] - point_start[1],
point_goal[0] - point_start[0]) * 180 / PI
start = [point_start[0], point_start[1], degree]
goal = [point_goal[0], point_goal[1], degree]
plt.plot(point_start[0], point_start[1], 'o')
plt.plot(point_goal[0], point_goal[1], 'o')
rs.plot_arrow(start[0], start[1], start[2], fc='g')
rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True)
plt.axis("equal")
obs_temp = []
path = hybrid_a_star_planning(start, goal, ox, oy, XY_GRID_RESOLUTION,
YAW_GRID_RESOLUTION)
x = path.xlist
y = path.ylist
yaw = path.yawlist
for i in range(len(x)):
millerToLonLat(x[i], y[i])
json_item = {}
json_item["name"] = "UAV Path"
json_item["path"] = lonlat_coordinate
#PathJson = json.dumps(json_item)
with open('path.json', 'w') as json_file:
json.dump(json_item, json_file)
for ix, iy, iyaw in zip(x, y, yaw):
plt.cla()
plt.plot(ox, oy, ".k")
plt.plot(x, y, "-r", label="Hybrid A* path", lw=2)
plt.grid(True)
plt.axis("equal")
plot_uav(ix, iy, iyaw)
plt.pause(0.0001)
print(__file__ + " done!!")
end_time = time.clock()
print(end_time - start_time)
plt.show()
def main(max_iter=40):
print("Start " + __file__)
# ====Search Path with RRT====
obstacleList = [
# (5, 5, 1),
# (4, 6, 1),
# (4, 8, 1),
# (4, 10, 1),
# (6, 5, 1),
# (7, 5, 1),
# (8, 6, 1),
# (8, 8, 1),
# (8, 10, 1)
(2, 0, 0.5),
(2, 1, 0.5),
(2, 2, 0.5),
(3, 0, 0.5),
(4, 0, 0.5),
(4, 1, 0.5),
(4, 2, 0.5),
(5, 0, 0.5),
(6, 0, 0.5),
(6, 1, 0.5),
(6, 2, 0.5),
(7, 0, 0.5),
(8, 0, 0.5),
(8, 1, 0.5),
(8, 2, 0.5)
] # [x,y,size(radius)]
# Set Initial parameters
start = [0.0, 0.0, np.deg2rad(90.0)]
goal = [5.0, 1.0, np.deg2rad(90.0)]
rrt_star_reeds_shepp = RRTStarReedsShepp(start,
goal,
obstacleList, [-2.0, 15.0],
max_iter=max_iter)
path = rrt_star_reeds_shepp.planning(animation=show_animation)
# Draw final path
if path and show_animation: # pragma: no cover
for point in reversed(path):
reeds_shepp_path_planning.plot_arrow(point[0], point[1], point[2])
plt.pause(0.01)
#rrt_star_reeds_shepp.draw_graph()
#plt.plot([x for (x, y, yaw) in path], [y for (x, y, yaw) in path], '-r')
#plt.grid(True)
#plt.pause(0.001)
plt.show()
def main():
print("Start rrt start planning")
# ====Search Path with RRT====
ox, oy = [], []
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
# Set Initial parameters
start = [10.0, 10.0, math.radians(90.0)]
goal = [50.0, 50.0, math.radians(-90.0)]
xyreso = 2.0
yawreso = math.radians(15.0)
rx, ry, ryaw = hybrid_a_star_planning(
start, goal, ox, oy, xyreso, yawreso)
plt.plot(ox, oy, ".k")
rs.plot_arrow(start[0], start[1], start[2])
rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True)
plt.axis("equal")
plt.show()
print(__file__ + " start!!")
def main():
print("Start Hybrid A* planning")
ox, oy = [], []
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
# Set Initial parameters
start = [10.0, 10.0, math.radians(90.0)]
goal = [50.0, 50.0, math.radians(-90.0)]
xyreso = 2.0
yawreso = math.radians(15.0)
rx, ry, ryaw = hybrid_a_star_planning(
start, goal, ox, oy, xyreso, yawreso)
plt.plot(ox, oy, ".k")
rs.plot_arrow(start[0], start[1], start[2])
rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True)
plt.axis("equal")
plt.show()
print(__file__ + " start!!")
def DrawGraph(self, rnd=None):
"""
Draw Graph
"""
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
reeds_shepp_path_planning.plot_arrow(self.start.x, self.start.y,
self.start.yaw)
reeds_shepp_path_planning.plot_arrow(self.end.x, self.end.y,
self.end.yaw)
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def DrawGraph(self, rnd=None):
"""
Draw Graph
"""
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
reeds_shepp_path_planning.plot_arrow(
self.start.x, self.start.y, self.start.yaw)
reeds_shepp_path_planning.plot_arrow(
self.end.x, self.end.y, self.end.yaw)
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def plot_start_goal_arrow(self):
reeds_shepp_path_planning.plot_arrow(self.start.x, self.start.y,
self.start.theta)
reeds_shepp_path_planning.plot_arrow(self.end.x, self.end.y,
self.end.theta)
def plan(startx,starty,startyaw,x,y,goalx,goaly,goalyaw,heightmap,anglemap,extended=True):
print("Start Hybrid A* planning")
print(heightmap.shape)
global h_map
h_map = heightmap
global a_map
a_map = anglemap
global extend
extend = extended
ox, oy = y,x
# Set Initial parameters
start = [starty, startx, startyaw]
goal = [goaly, goalx, goalyaw]
print("start : ", start)
print("goal : ", goal)
if show_animation:
plt.plot(ox, oy, ".k")
rs.plot_arrow(start[0], start[1], start[2], fc='g')
rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True)
plt.axis("equal")
path = hybrid_a_star_planning(
start, goal, ox, oy, XY_GRID_RESOLUTION, YAW_GRID_RESOLUTION)
x = path.x_list
y = path.y_list
yaw = path.yaw_list
directions = path.direction_list
coords = np.column_stack((np.array(x),np.array(y)))
grad = np.gradient(coords,axis=0)
# for i in range(0,len(x)):
# print(str(i) + ' ' + str(x[i]) + ' ' + str(y[i]) + ' ' + str(grad[i]))
###OPTIMIZE THIS LATER
locs = [0]
for i in range(0,len(grad)):
if abs(grad[i,0]) < .1:
if abs(grad[i,1]) < .1:
locs.append(i)
locs.append(len(x) - 1)
if show_animation:
for i_x, i_y, i_yaw in zip(x, y, yaw):
plt.cla()
plt.gca().invert_yaxis()
plt.plot(ox, oy, ".k")
plt.plot(x, y, "-r", label="Hybrid A* path")
plt.grid(True)
plt.axis("equal")
plt.imshow(heightmap)
plot_car(i_x, i_y, i_yaw)
plt.pause(0.00001)
# plt.show()
print(__file__ + " done!!")
return x,y,yaw,locs,directions
def main():
print("Start Hybrid A* planning")
# ox, oy = [], []
# ox, oy = obs_map.map_maze(ox, oy)
# start = [17.0, 20.0, np.deg2rad(-90.0)]
# goal = [20.0, 42.0, np.deg2rad(0.0)]
#goal = [17.0, 20.0, np.deg2rad(90.0)]
#start = [20.0, 42.0, np.deg2rad(0.0)]
#goal = [55.0, 50.0, np.deg2rad(0.0)]
start = [0.0, 0.0, np.deg2rad(90.0)]
goal = [5.0, 20.0, np.deg2rad(90.0)]
ox, oy = [], []
pos_cars = [(0, 15), (5, 2)]
ox, oy = obs_map.map_road(ox, oy, pos_cars)
plt.plot(ox, oy, ".k")
rs.plot_arrow(start[0], start[1], start[2], fc='g')
rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True)
plt.axis("equal")
# Path Planning - Hybrid A*
tic = time.time()
path = hybrid_a_star_planning(start, goal, ox, oy, XY_GRID_RESOLUTION,
YAW_GRID_RESOLUTION)
toc = time.time()
print("Process time (hybrid A*):", (toc - tic))
# Constraint Search
tic = time.time()
lb_x, ub_x, lb_y, ub_y = constraint_search(ox, oy, path)
toc = time.time()
print("Process time (constraint_search):", (toc - tic))
x = path.xlist
y = path.ylist
yaw = path.yawlist
ind = 0 # index for lower/upper bounds.
for ix, iy, iyaw in zip(x, y, yaw):
# plotting during animation.
plt.cla()
plt.plot(ox, oy, ".k")
plt.plot(lb_x[ind], iy, "xr", markersize=10) # Plot Constraints
plt.plot(ub_x[ind], iy, "xr", markersize=10) # Plot Constraints
plt.plot(ix, lb_y[ind], "xb", markersize=10) # Plot Constraints
plt.plot(ix, ub_y[ind], "xb", markersize=10) # Plot Constraints
ind = ind + 1
plt.plot(x, y, "-r", label="Hybrid A* path")
plt.grid(True)
plt.axis("equal")
plot_car(ix, iy, iyaw)
plt.pause(0.0001)
print(__file__ + " done!!")
plt.show()
def main():
print("Start Hybrid A* planning")
ox, oy = [], []
for i in range(100):
ox.append(i)
oy.append(0.0)
for i in range(100):
ox.append(100.0)
oy.append(i)
for i in range(101):
ox.append(i)
oy.append(100.0)
for i in range(101):
ox.append(0.0)
oy.append(i)
for i in range(50):
ox.append(20.0)
oy.append(i)
for i in range(60):
ox.append(40.0)
oy.append(i)
for i in range(30):
ox.append(40.0)
oy.append(100.0 - i)
for i in range(20):
oy.append(50)
ox.append(40 + i)
for i in range(35):
oy.append(40)
ox.append(65 + i)
for i in range(25):
oy.append(40 - i)
ox.append(65)
# Set Initial parameters
start = [10.0, 10.0, np.deg2rad(90.0)]
goal = [80.0, 20.0, np.deg2rad(180.0)]
plt.plot(ox, oy, ".k")
rs.plot_arrow(start[0], start[1], start[2], fc='g')
rs.plot_arrow(goal[0], goal[1], goal[2])
plt.grid(True)
plt.axis("equal")
path = hybridastar(start, goal, ox, oy, XY_GRID_RESOLUTION,
yaw_GRID_RESOLUTION)
x = path.xlist
l = len(list(x))
y = path.ylist
print('ty', len(list(y)))
yaw = path.yawlist
count = 0
for ix, iy, iyaw in zip(x, y, yaw):
plt.cla()
print('type', type(ix))
plt.plot(ox, oy, ".k") #display obstacles and boundary
plt.plot(x, y, "-y", label="Hybrid A* path") #display path
plt.grid(True)
plt.axis("equal")
#print('ix',ix)
#print('x',x)
plot_car(ix, iy, iyaw) #display car pic
#plt.savefig(r'D:\Games\planningp5\pix\{}.png'.format(count))
count = count + 1
#plt.savefig(r'C:\Users\Vamshi\Downloads\planningp5\*.png',transparent=False,bbox_inches='tight')
plt.pause(0.0001)
