diff --git a/PathPlanning/RRT/oficina.png b/PathPlanning/RRT/oficina.png new file mode 100644 index 0000000000..0c6fb93a82 Binary files /dev/null and b/PathPlanning/RRT/oficina.png differ diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py index 8031c6d3af..884c9a7424 100644 --- a/PathPlanning/RRT/rrt_with_pathsmoothing.py +++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py @@ -12,13 +12,17 @@ import random import math import copy +from scipy import misc +import glob +import numpy as np +import matplotlib class RRT(): u""" Class for RRT Planning """ - def __init__(self, start, goal, obstacleList,randArea,expandDis=1.0,goalSampleRate=5,maxIter=500): + def __init__(self, start, goal, obstacleList,randArea,expandDis=30,goalSampleRate=10,maxIter=100): u""" Setting Parameter @@ -99,14 +103,16 @@ def DrawGraph(self,rnd=None): for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [node.y, self.nodeList[node.parent].y], "-g") + plt.axis([0, 800, 0, 800]) ####################################### aqui + plt.grid(True) for (x,y,size) in obstacleList: self.PlotCircle(x,y,size) plt.plot(self.start.x, self.start.y, "xr") plt.plot(self.end.x, self.end.y, "xr") - plt.axis([-2, 15, -2, 15]) + #plt.axis([0, 800, 0, 800]) plt.grid(True) - plt.pause(0.01) + plt.pause(0.001) def PlotCircle(self,x,y,size): deg=range(0,360,5) @@ -252,26 +258,60 @@ def PathSmoothing(path, maxIter, obstacleList): import matplotlib.pyplot as plt #====Search Path with RRT==== # Parameter - obstacleList = [ - (5, 5, 1), - (3, 6, 2), - (3, 8, 2), - (3, 10, 2), - (7, 5, 2), - (9, 5, 2) - ] # [x,y,size] - rrt=RRT(start=[0,0],goal=[5,10],randArea=[-2,15],obstacleList=obstacleList) + + for image_path in glob.glob("/home/osmaralg/Desktop/PythonRobotics/PathPlanning/RRT/testfile.png"): + image = misc.imread(image_path) + print (image.shape) + print (image.dtype) + + + + png = [] + for image_path in glob.glob("/home/osmaralg/Desktop/PythonRobotics/PathPlanning/RRT/testfile.png"): + png.append(misc.imread(image_path)) + + im = np.asarray(png) + + ocupado = [] + print 'Importing done...', im.shape + + print im + size = 800 # number of the pixels of the image + resolution = 12 # step size pixel, if 1 evaluates every pixel if 4 every 4 pixels obstacle sambple + obstaclesize = 30 # radious of the obstacle circle + darkness = 50 # gray scale if 127 gray equals obstacle if 0 only black equals obstacle + for i in range(0,size,resolution): + for j in range(0,size,resolution): + if im[0,i,j]