def main(argv=None):
if (argv == None):
argv = sys.argv[1:]
# Specify use of global threads
global thread1
global thread2
# Random num obstacles (Could be 1-5 OBSTACLES CAN OVERLAP)
num_obstacles = randint(1, 5)
# Dimensions of grid, num objects, max dimensions of objects
pp = PathPlanningProblem(MAX_FIELD_WIDTH, MAX_FIELD_HEIGHT, num_obstacles,
MAX_OBSTACLE_WIDTH, MAX_OBSTACLE_HEIGHT,
MIN_OBSTACLE_WIDTH, MIN_OBSTACLE_HEIGHT)
# Call problem creator (SEE PART 2 Path Planning Simulation Domain)
initial, goals = pp.CreateProblemInstance()
# Plot the starting point
pygame.draw.rect(screen, RED,
(initial[0], initial[1], 1 * SCALE, 1 * SCALE))
# Plot the obstacles
for start in pp.obstacles:
pygame.draw.rect(screen, BLACK,
(start.x, start.y, start.width, start.height))
# Plot the goal(s)
for goal in goals:
pygame.draw.rect(screen, GREEN,
(goal[0], goal[1], 1 * SCALE, 1 * SCALE))
# Extract single goal
goal = [goals[0][0], goals[0][1]]
# Run the dual RRT algorithm :)
thread1 = threading.Thread(target=ExploreDomain,
args=(pp, initial, MAX_ITERATIONS, goal,
"Thread 1"))
thread2 = threading.Thread(target=ExploreDomain,
args=(pp, goal, MAX_ITERATIONS, initial,
"Thread 2"))
thread1.start()
thread2.start()
# Allow window to persist after execution, ESCAPE to quit
while 1:
pygame.display.update()
if checkExitSimulation() is True:
print("The marker has left the building...")
exit(1)
def main(argv=None):
if (argv == None):
argv = sys.argv[1:]
width = 10.0
height = 10.0
pp = PathPlanningProblem(width, height, 5, 4.0, 4.0)
#pp.obstacles = [ Obstacle(0.0, 0.0, pp.width, pp.height / 2.2, '#555555' ) ]
pp.obstacles = []
initial, goals = pp.CreateProblemInstance()
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(o.patch)
# ip = plt.Rectangle((initial[0],initial[1]), 0.1, 0.1, facecolor='#ff0000')
# ax.add_patch(ip)
for g in goals:
g = plt.Rectangle((g[0], g[1]), 0.1, 0.1, facecolor='#00ff00')
# ax.add_patch(g)
path = ExploreDomain(pp, initial, 50000)
ax.set_title('Vacuuming Domain')
plt.plot(path[:, 0], path[:, 1], 'b-')
ax = fig.add_subplot(1, 2, 2)
# x,y,z = pp.CalculateCoverage(path, 0.5)
# X,Y = np.meshgrid(x,y)
# Z = z
# ax.plot_surface(X,Y,Z, rstride=1, cstride=1, cmap=cm.coolwarm)
heatmap, x, y = np.histogram2d(path[:, 0],
path[:, 1],
bins=50,
range=[[0.0, pp.width], [0.0, pp.height]])
coverage = float(np.count_nonzero(heatmap)) / float(
len(heatmap) * len(heatmap[0]))
extent = [x[0], x[-1], y[0], y[-1]]
ax.set_title('Random Walk\nCoverage {0}'.format(coverage))
plt.imshow(np.rot90(heatmap))
plt.colorbar()
plt.show()
def main(argv=None):
if (argv == None):
argv = sys.argv[1:]
width = 100.0
height = 100.0
maxOWidth = 50.0
maxOHeight = 50.0
numObstacles = 10
if len(argv) > 0:
numObstacles = int(argv[0])
pp = PathPlanningProblem(width, height, numObstacles, maxOWidth,
maxOHeight)
#pp.obstacles = [ Obstacle(0.0, 0.0, pp.width, pp.height / 2.2, '#555555' ) ]
initial, goals = pp.CreateProblemInstance()
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0], initial[1]), 0.1, 0.1, facecolor='#ff0000')
ax.add_patch(ip)
for g in goals:
g = plt.Rectangle((g[0], g[1]), 0.1, 0.1, facecolor='#00ff00')
ax.add_patch(g)
start_time = time.time()
qtd = QuadTreeDecomposition(pp, 0.1)
markAdjacentNodes()
assignHeuristics(goals[0])
length, found = aStart(initial, goals[0])
total_time = time.time() - start_time
if found == False:
print("Did not find goal.")
else:
print("Found goal in: ", total_time, ". Path length: ", length)
qtd.Draw(ax)
n = qtd.CountCells()
ax.set_title('Quadtree Decomposition\n{0} cells'.format(n))
plt.show()
def main(argv=None):
if (argv == None):
argv = sys.argv[1:]
# Random num obstacles (Could be 1-5 OBSTACLES CAN OVERLAP)
num_obstacles = randint(1, 5)
# Dimensions of grid, num objects, max dimensions of objects
pp = PathPlanningProblem(MAX_FIELD_WIDTH, MAX_FIELD_HEIGHT, num_obstacles,
MAX_OBSTACLE_WIDTH, MAX_OBSTACLE_HEIGHT,
MIN_OBSTACLE_WIDTH, MIN_OBSTACLE_HEIGHT)
# Call problem creator (SEE PART 2 Path Planning Simulation Domain)
initial, goals = pp.CreateProblemInstance()
# Plot the starting point
pygame.draw.rect(screen, RED,
(initial[0], initial[1], 1 * SCALE, 1 * SCALE))
# Plot the obstacles
for start in pp.obstacles:
pygame.draw.rect(screen, BLACK,
(start.x, start.y, start.width, start.height))
# Plot the goal(s)
for goal in goals:
pygame.draw.rect(screen, GREEN,
(goal[0], goal[1], 1 * SCALE, 1 * SCALE))
# Run the RRT algorithm :)
startTime = time.process_time()
foundGoal, timeToGoal = ExploreDomain(pp, initial, MAX_ITERATIONS, goals)
if foundGoal is True:
print("Agent took the following length of time to find the goal: ",
(timeToGoal - startTime), "s")
# Allow window to persist after execution, ESCAPE to quit
while 1:
pygame.display.update()
checkExitSimulation("The marker has left the building...")
def main( argv = None, showPlot = True, problemSpace = None, initial = None, goals = None):
if ( argv == None ):
argv = sys.argv[1:]
width = 10.0
height = 10.0
obstacleCount = 15
obstacleWidth = 5.0
obstacleHeight = 5.0
##############
# Create path planning problem
if problemSpace == None:
problemSpace = PathPlanningProblem( width, height, obstacleCount, obstacleWidth, obstacleHeight)
if initial == None or goals == None:
initial, goals = problemSpace.CreateProblemInstance()
goal = goals[0]
##############
# Quad tree decomposition
fig = plt.figure()
ax = fig.add_subplot(1,2,1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
# Draw obstacles
for o in problemSpace.obstacles:
ax.add_patch(copy.copy(o.patch) )
# Do the space partitioning
qtd = QuadTreeDecomposition(problemSpace, 0.2)
qtd.Draw(ax)
# Draw initial position
ax.add_patch(plt.Circle((initial[0], initial[1]), 0.05, color="#ff0000"))
# Draw goal
ax.add_patch(plt.Circle((goal[0], goal[1]), 0.05, color="#00ff00"))
# Do pathfinding
pathDist = math.inf
timeTaken = 0
path, pathDist, timeTaken, success = pathfinding(initial, goal, qtd)
if success:
prevPoint = goal
for point in path:
plt.plot([point[0], prevPoint[0]], [point[1], prevPoint[1]], '#00aaff', lw=2)
ax.add_patch(plt.Circle((point[0], point[1]), 0.05, color="#00aaff"))
prevPoint = point
plt.plot([initial[0], prevPoint[0]], [initial[1], prevPoint[1]], '#00aaff', lw=2)
# Title plot
n = qtd.CountCells()
ax.set_title('Quadtree Decomposition\n{0} cells\nTime taken: {1:.5f}\nDistance: {2:.5f}'.format(n, timeTaken, pathDist))
##############
# Binary space partiioning
ax = fig.add_subplot(1,2,2, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
# Draw obstacles
for o in problemSpace.obstacles:
ax.add_patch(copy.copy(o.patch))
# Do the space partitioning
bsp = BinarySpacePartitioning(problemSpace, 0.2)
bsp.Draw(ax)
# Draw initial position
ax.add_patch(plt.Circle((initial[0], initial[1]), 0.05, color="#ff0000"))
# Draw goal
ax.add_patch(plt.Circle((goal[0], goal[1]), 0.05, color="#00ff00"))
# Do pathfinding
path, pathDist, timeTaken, success = pathfinding(initial, goal, bsp)
if success:
prevPoint = goal
for point in path:
plt.plot([point[0], prevPoint[0]], [point[1], prevPoint[1]], '#00aaff', lw=2)
ax.add_patch(plt.Circle((point[0], point[1]), 0.05, color="#00aaff"))
prevPoint = point
plt.plot([initial[0], prevPoint[0]], [initial[1], prevPoint[1]], '#00aaff', lw=2)
# Title plot
n = bsp.CountCells()
ax.set_title('BSP Decomposition\n{0} cells\nTime taken: {1:.4f}\nDistance: {2:.4f}'.format(n, timeTaken, pathDist))
##############
# Show plot
if showPlot:
plt.show()
def main(argv=None):
if (argv == None):
argv = sys.argv[1:]
width = 100.0
height = 100.0
pp = PathPlanningProblem(width, height, 60, 40, 40)
initial, goals = pp.CreateProblemInstance()
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0], initial[1]), 1.0, 1.0, facecolor='#ff0000')
ax.add_patch(ip)
for g in goals:
g = plt.Rectangle((g[0], g[1]), 1.0, 1.0, facecolor='#00ff00')
ax.add_patch(g)
qtd = QuadTreeDecomposition(pp, 0.2, initial, goals)
qtd.Draw(ax)
n = qtd.CountCells()
start = timeit.default_timer()
astar = AStarSearch(
qtd.domain, qtd.root,
Rectangle(qtd.initialCell[0], qtd.initialCell[1], 0.1, 0.1),
Rectangle(qtd.goalsCell[0][0], qtd.goalsCell[0][1], 0.1, 0.1))
stop = timeit.default_timer()
print("A* Running Time: ", stop - start)
print("A* Path Length : ", astar.path_length)
plt.plot([x for (x, y) in astar.path], [y for (x, y) in astar.path], '-')
ax.set_title('Quadtree Decomposition\n{0} cells'.format(n))
ax = fig.add_subplot(1, 2, 2, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0], initial[1]), 1, 1, facecolor='#ff0000')
ax.add_patch(ip)
goal = None
for g in goals:
goal = g
g = plt.Rectangle((g[0], g[1]), 1, 1, facecolor='#00ff00')
ax.add_patch(g)
start = timeit.default_timer()
spath = RRT.ExploreDomain(RRT(8), pp, initial, goal, 5000)
path = spath[0]
final = spath[1]
if len(final) == 0:
print("No path found")
RRT.draw(RRT(0), plt, path, final)
stop = timeit.default_timer()
print("RRT Running Time: ", stop - start)
if len(final) > 0:
print("RRT Path Length : ", RRT.pathLen(RRT(0), final))
ax.set_title('RRT')
plt.show()
def main( argv = None ):
if ( argv == None ):
argv = sys.argv[1:]
width = 100.0
height = 100.0
maxOWidth = 50.0
maxOHeight = 50.0
numObstacles = 5
if len(argv) > 0:
numObstacles = int(argv[0])
pp = PathPlanningProblem( width, height, numObstacles, maxOWidth, maxOHeight)
#pp.obstacles = [ Obstacle(0.0, 0.0, pp.width, pp.height / 2.2, '#555555' ) ]
# pp.obstacles = []
initial, goals = pp.CreateProblemInstance()
fig = plt.figure()
ax = fig.add_subplot(1,1,1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(o.patch)
# ax.add_patch(g)
start_time = time.time()
paths, found = ExploreDomain( pp, initial, 5, goals[0] )
ax.set_title('RRT Domain')
print( "Total paths traversed: ", len(paths) )
if found:
toGoal = []
toGoal.append( paths[len( paths ) - 1] )
paths.pop( len( paths ) - 1 )
atStart = False
if toGoal[0][0][0] == initial[0] and toGoal[0][0][1] == initial[1]:
atStart = True
while not atStart:
pathToConnect = toGoal[len( toGoal ) - 1]
i = 0
for path in paths:
if path[len( path ) - 1][0] == pathToConnect[0][0] and path[len( path ) - 1][1] == pathToConnect[0][1]:
toGoal.append( path )
paths.pop( i )
atStart = path[0][0] == initial[0] and path[0][1] == initial[1]
break
else:
i += 1
print( "Total steps to goal: ", len( toGoal ) )
total_time = time.time() - start_time
path_length = 0
#calculate the length.
for path in toGoal:
path_length += lineLength( path[0] - path[len( path ) - 1] )
print( "Found the goal in: ", total_time, "\nWith path of length: ", path_length )
for path in paths:
plt.plot(path[:,0], path[:,1], 'b-')
for path in toGoal:
plt.plot(path[:,0], path[:,1], 'y-')
else:
print("Did not find goal.")
for path in paths:
plt.plot(path[:,0], path[:,1], 'b-')
# These don't show up...
ip = plt.Rectangle((initial[0],initial[1]), .5, .5, facecolor='#ff0000')
ax.add_patch(ip)
for g in goals:
g = plt.Rectangle((g[0],g[1]), 0.5, 0.5, facecolor='#00ff00')
ax.add_patch(g)
plt.show()
def main(argv=None,
showPlot=True,
problemSpace=None,
initial=None,
goals=None):
if (argv == None):
argv = sys.argv[1:]
width = 10.0
height = 10.0
obstacleCount = 15
obstacleWidth = 5.0
obstacleHeight = 5.0
goalRadius = 0.15
biasToGoal = 0.5
stepDistance = 0.15
maxSteps = 50000
##############
# Create path planning problem
if problemSpace == None:
problemSpace = PathPlanningProblem(width, height, obstacleCount,
obstacleWidth, obstacleHeight)
if initial == None or goals == None:
initial, goals = problemSpace.CreateProblemInstance()
goal = goals[0]
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
##############
# RRT
startTime = time.time()
path, nodeNearGoal = ExploreDomain(problemSpace, initial, goal, maxSteps,
stepDistance, biasToGoal, goalRadius)
timeTaken = time.time() - startTime
ax.set_title('Vacuuming Domain')
# Draw the path
pathX = []
pathY = []
for node in path:
if node.parent:
plt.plot([node.pos[0], node.parent.pos[0]],
[node.pos[1], node.parent.pos[1]],
'#7777aa',
lw=2,
zorder=10000)
ax.add_patch(
plt.Circle((node.pos[0], node.pos[1]),
0.025,
color="#0000aa",
zorder=10002))
pathX.append(node.pos[0])
pathY.append(node.pos[1])
pathDistance = 0
walkNode = nodeNearGoal
while walkNode and walkNode.parent:
plt.plot([walkNode.pos[0], walkNode.parent.pos[0]],
[walkNode.pos[1], walkNode.parent.pos[1]],
'#00aaff',
lw=2,
zorder=10001)
ax.add_patch(
plt.Circle((walkNode.pos[0], walkNode.pos[1]),
0.025,
color="#99ccff",
zorder=10003))
pathDistance += getDistanceBetweenPoints(walkNode.pos,
walkNode.parent.pos)
walkNode = walkNode.parent
if nodeNearGoal:
plt.plot([nodeNearGoal.pos[0], goal[0]],
[nodeNearGoal.pos[1], goal[1]],
'#00aaff',
lw=2)
pathDistance += getDistanceBetweenPoints(nodeNearGoal.pos, goal)
##############
# Draw map
# Draw obstacles
for o in problemSpace.obstacles:
ax.add_patch(o.patch)
# Draw initial position
ax.add_patch(
plt.Circle((initial[0], initial[1]),
0.05,
color="#ff0000",
zorder=math.inf))
# Draw goal
ax.add_patch(plt.Circle((goal[0], goal[1]), goalRadius, color="#ffff00"))
ax.add_patch(
plt.Circle((goal[0], goal[1]), 0.05, color="#00ff00", zorder=math.inf))
# Output extra data
ax.set_title('Time taken: {0:.5f}\nDistance: {1:.5f}\nNodes: {2}'.format(
timeTaken, pathDistance, len(path)))
##############
# Draw heatmap
ax = fig.add_subplot(1, 2, 2)
# x,y,z = problemSpace.CalculateCoverage(path, 0.5)
# X,Y = np.meshgrid(x,y)
# Z = z
# ax.plot_surface(X,Y,Z, rstride=1, cstride=1, cmap=cm.coolwarm)
heatmap, x, y = np.histogram2d(pathX,
pathY,
bins=50,
range=[[0.0, problemSpace.width],
[0.0, problemSpace.height]])
coverage = float(np.count_nonzero(heatmap)) / float(
len(heatmap) * len(heatmap[0]))
extent = [x[0], x[-1], y[0], y[-1]]
ax.set_title('Random Walk\nCoverage {0}'.format(coverage))
plt.imshow(np.rot90(heatmap))
plt.colorbar()
##############
# Show plot
if showPlot:
plt.show()
def main(argv=None):
if (argv == None):
argv = sys.argv[1:]
width = 10.0
height = 10.0
obs_count = int(argv[0])
# obs_count = 40
iterations = 1 # specify # of times to run map generation & Quadtree, FBSP, RRT (used for comparison of run times)
# avg times
# TODO: add RRT averages
qtd_decomp_avg = 0.0
qtd_star_avg = 0.0
qtd_path_avg = 0.0
fbsp_decomp_avg = 0.0
fbsp_star_avg = 0.0
fbsp_path_avg = 0.0
if obs_count >= 30:
obs_width = obs_height = 2.0
else:
obs_width = obs_height = 5.0
for i in range(iterations):
pp = PathPlanningProblem(width, height, obs_count, obs_width,
obs_height)
qtd_start = time.perf_counter()
qtd = QuadTreeDecomposition(pp, 0.1)
qtd_end = time.perf_counter()
initial, goals = pp.CreateProblemInstance()
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial.x, initial.y),
initial.width,
initial.height,
facecolor='#ff0000')
ax.add_patch(ip)
# for g in goals:
g = plt.Rectangle((goals.x, goals.y),
goals.width,
goals.height,
facecolor='#00ff00')
ax.add_patch(g)
# print(initial.x, initial.y)
# print(goals.x, goals.y)
# qtd.Draw(ax)
# plt.show()
# run A*
qtd_star_start_time = time.perf_counter()
qtd_path = AStar(qtd, initial, goals).path_to_goal
qtd_star_end_time = time.perf_counter()
qtd_path_len = 0
x_cord = []
y_cord = []
for cord in qtd_path:
x_cord += [cord[0]]
y_cord += [cord[1]]
qtd_path_len += cord[2]
plt.plot(x_cord, y_cord, '-')
qtd.Draw(ax)
n = qtd.CountCells()
ax.set_title('Quadtree Decomposition\n{0} cells'.format(n))
ax = fig.add_subplot(1, 2, 2, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
fbsp_start = time.perf_counter()
bsp = BinarySpacePartitioning(pp, 0.1)
fbsp_end = time.perf_counter()
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial.x, initial.y),
initial.width,
initial.height,
facecolor='#ff0000')
ax.add_patch(ip)
# for g in goals:
g = plt.Rectangle((goals.x, goals.y),
goals.width,
goals.height,
facecolor='#00ff00')
ax.add_patch(g)
# run A*
fbsp_star_start_time = time.perf_counter()
fbsp_path = AStar(bsp, initial, goals).path_to_goal
fbsp_star_end_time = time.perf_counter()
fbsp_path_len = 0
x_cord = []
y_cord = []
for cord in fbsp_path:
x_cord += [cord[0]]
y_cord += [cord[1]]
fbsp_path_len += cord[2]
plt.plot(x_cord, y_cord, '-')
bsp.Draw(ax)
n = bsp.CountCells()
ax.set_title('BSP Decomposition\n{0} cells'.format(n))
plt.show()
qtd_decomp_time = qtd_end - qtd_start
qtd_star_time = qtd_star_end_time - qtd_star_start_time
qtd_decomp_avg += qtd_decomp_time
qtd_star_avg += qtd_star_time
qtd_path_avg += qtd_path_len
fbsp_decomp_time = fbsp_end - fbsp_start
fbsp_star_time = fbsp_star_end_time - fbsp_star_start_time
fbsp_decomp_avg += fbsp_decomp_time
fbsp_star_avg += fbsp_star_time
fbsp_path_avg += fbsp_path_len
print('\n')
if len(qtd_path) < 1:
print('No path found for Quadtree A*')
print("Quadtree Decom Runtime: ", qtd_decomp_time)
print('Quadtree A* Runtime: ', qtd_star_time)
print('Quadtree A* path length: ', qtd_path_len)
if len(fbsp_path) < 1:
print('No path found for FBSP A*')
print("FBSP Decomp Runtime: ", fbsp_decomp_time)
print('FBSP A* Runtime: ', fbsp_star_time)
print('FBSP A* path length: ', fbsp_path_len)
print('\n')
if iterations > 1:
print("Quadtree AVG Decom Runtime: ", qtd_decomp_avg / iterations)
print('Quadtree AVG A* Runtime: ', qtd_star_avg / iterations)
print('Quadtree AVG A* path length: ', qtd_path_avg / iterations)
print("FBSP AVG Decomp Runtime: ", fbsp_decomp_avg / iterations)
print('FBSP AVG A* Runtime: ', fbsp_star_avg / iterations)
print('FBSP AVG A* path length: ', fbsp_path_avg / iterations)
def main(argv=None):
if (argv == None):
argv = sys.argv[1:]
width = 100.0
height = 100.0
onum = 5
oheight = 50
oheight_min = 10
owidth = 50
owidth_min = 10
targeSize = 2
halfSize = targeSize / 2
pp = PathPlanningProblem(width, height, onum, owidth, oheight, owidth_min,
oheight_min)
#pp.obstacles = [ Obstacle(0.0, 0.0, pp.width, pp.height / 2.2, '#555555' ) ]
initial, goals = pp.CreateProblemInstance()
# goalN=QuadTreeDecomposition.get_neighborsAll(test.goalNode )
# startN=QuadTreeDecomposition.get_neighborsAll(test.startNode )
# for nb in startN:
# QuadTreeDecomposition.setColor(nb)
# for nb in goalN:
# QuadTreeDecomposition.setColor(nb)
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
# initial = test.initial
# goals = test.goals
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0] - halfSize, initial[1] - halfSize),
targeSize,
targeSize,
facecolor='#ff0000')
ax.add_patch(ip)
for g in goals:
g = plt.Rectangle((g[0] - halfSize, g[1] - halfSize),
targeSize,
targeSize,
facecolor='#00ff00')
ax.add_patch(g)
qtd = QuadTreeDecomposition(pp, 0.5)
test = ast.AStar(qtd, pp, initial, goals, ax)
test.findPath()
qtd.Draw(ax)
n = qtd.CountCells()
ax.set_title('Quadtree Decomposition\n{0} cells'.format(n))
ax = fig.add_subplot(1, 2, 2, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0] - halfSize, initial[1] - halfSize),
targeSize,
targeSize,
facecolor='#ff0000')
ax.add_patch(ip)
for g in goals:
g = plt.Rectangle((g[0] - halfSize, g[1] - halfSize),
targeSize,
targeSize,
facecolor='#00ff00')
ax.add_patch(g)
bsp = BinarySpacePartitioning(pp, 0.25)
test = ast.AStar(bsp, pp, initial, goals, ax)
test.findPath()
bsp.Draw(ax)
n = bsp.CountCells()
ax.set_title('BSP Decomposition\n{0} cells'.format(n))
plt.show()
import matplotlib.pyplot as plt from pathplanning import PathPlanningProblem from RRT import main as rrt from FBSP import main as fbsp width = 10.0 height = 10.0 obstacleCount = 10 obstacleWidth = 5.0 obstacleHeight = 5.0 problemSpace = PathPlanningProblem( width, height, obstacleCount, obstacleWidth, obstacleHeight) initial, goals = problemSpace.CreateProblemInstance() fbsp(showPlot=False, problemSpace=problemSpace, initial=initial, goals=goals) rrt(showPlot=False, problemSpace=problemSpace, initial=initial, goals=goals) plt.show()
def main(argv=None):
freeNode = []
if (argv == None):
argv = sys.argv[1:]
width = 100.0
height = 100.0
pp = PathPlanningProblem(width, height, 15, 50.0, 50.0)
# pp.obstacles = [ Obstacle(0.0, 0.0, pp.width, pp.height / 2.2, '#555555' ) ]
initial, goals = pp.CreateProblemInstance()
# print(initial[0][0])
# print(goals[0][1])
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0], initial[1]), 1.0, 1.0, facecolor='#ff0000')
ax.add_patch(ip)
for g in goals:
g = plt.Rectangle((g[0], g[1]), 1.0, 1.0, facecolor='#00ff00')
ax.add_patch(g)
qtd = QuadTreeDecomposition(pp, 1.0)
qtd.Draw(ax, freeNode)
n = qtd.CountCells()
ax.set_title('Quadtree Decomposition\n{0} cells'.format(n))
goalX = goals[0][0]
goalY = goals[0][1]
goalNode = findNode(goalX, goalY, freeNode)
initialX = initial[0]
initialY = initial[1]
resultSet = []
resultSet.append(goalNode[0])
while (len(resultSet) != 0):
rectangle = resultSet.pop(0)
findAroud(rectangle, freeNode, resultSet, ax)
availableNodes = []
qtd.findAllFreeNodes(availableNodes)
# for a in availableNodes :
# if(a[0].hValue==1):
# g = plt.Rectangle((a[0].x, a[0].y), a[0].width, a[0].height, facecolor='#FF0000')
# ax.add_patch(g)
# if (a[0].hValue == 2):
# g = plt.Rectangle((a[0].x, a[0].y), a[0].width, a[0].height, facecolor='#008000')
# ax.add_patch(g)
#run A* algorithm
initialNode = findNode(initialX, initialY, availableNodes)
astart = Astart()
astart.Astartprocessing(initialNode, goalNode, availableNodes, ax)
plt.show()
print("end")