class RRTstarPlanner(object):
def __init__(self,
planning_range,
segment_length,
cost_func=None,
map_file=None):
self.planningRange = planning_range
self.segmentLength = segment_length
self.mapFile = map_file
self.rrts = RRTstar(self.planningRange, self.segmentLength)
if self.mapFile != None:
self.rrts.loadMap(self.mapFile)
self.rrts_viz = RRTVisualizer(self.rrts)
self.cost_func = cost_func
def findPath(self, start, goal, iterationNum, homotopyMgr):
self.rrts.init(start, goal, self.cost_func, homotopyMgr)
for i in range(iterationNum):
print "Iter@" + str(i)
self.rrts.extend()
self.rrts_viz.update()
path = self.rrts.findPath()
self.rrts_viz.activePath = path
self.rrts_viz.update()
return path
class RRTstarPlanner(Planner):
def __init__(self,
planning_range,
segment_length,
cost_func=None,
map_file=None):
super(RRTstarPlanner, self).__init__(planning_range, segment_length,
map_file)
self.rrts = RRTstar(self.planningRange, self.segmentLength)
if self.mapFile != None:
self.rrts.loadMap(self.mapFile)
self.rrts_viz = RRTVisualizer(self.rrts)
self.cost_func = cost_func
def findPath(self, start, goal, iterationNum):
self.rrts.init(start, goal, self.cost_func)
for i in range(iterationNum):
self.rrts.extend()
self.rrts_viz.update()
path = self.rrts.findPath()
self.rrts_viz.activePath = path
self.rrts_viz.update()
return path
def __init__(self,
planning_range,
segment_length,
cost_func=None,
map_file=None):
super(RRTstarPlanner, self).__init__(planning_range, segment_length,
map_file)
self.rrts = RRTstar(self.planningRange, self.segmentLength)
if self.mapFile != None:
self.rrts.loadMap(self.mapFile)
self.rrts_viz = RRTVisualizer(self.rrts)
self.cost_func = cost_func
def __init__(self,
planning_range,
segment_length,
cost_func=None,
map_file=None):
super(kRRGPlanner, self).__init__(planning_range, segment_length,
map_file)
self.rrg = kRRG(self.planningRange, self.segmentLength)
if self.mapFile != None:
self.rrg.loadMap(self.mapFile)
self.krrg_viz = RRTVisualizer(self.rrg)
self.cost_func = cost_func
def __init__(self,
planning_range,
segment_length,
cost_func=None,
map_file=None):
self.planningRange = planning_range
self.segmentLength = segment_length
self.mapFile = map_file
self.krrts = kRRTstar(self.planningRange, self.segmentLength)
if self.mapFile != None:
self.rrts.loadMap(self.mapFile)
self.krrts_viz = RRTVisualizer(self.krrts)
self.cost_func = cost_func
'''
Created on Nov 12, 2014
@author: daqing_yi
'''
from RRG import *
from RRTVisualizer import *
if __name__ == '__main__':
rrt = RRG([400, 400], 10)
rrt_viz = RRTVisualizer(rrt)
rrt.init([40, 40], [300, 300])
for i in range(1000):
print i
rrt.expand()
rrt_viz.update()
'''
Created on Jan 3, 2015
@author: daqing_yi
'''
from kRRG import *
from RRTVisualizer import *
if __name__ == '__main__':
def calcDist(currentNode, referenceNode):
dist = 0.0
if referenceNode==None:
return dist
dist = np.sqrt((currentNode.pos[0]-referenceNode.pos[0])**2+(currentNode.pos[1]-referenceNode.pos[1])**2)
return dist
krrg = kRRG([600, 400], 10)
krrg_viz = RRTVisualizer(krrg)
krrg.init([40,40], [320, 300], calcDist)
for i in range(1000):
#print i
krrg.extend()
krrg_viz.update()
def __init__(self, planning_range, segment_length, map_file=None):
super(RRTPlanner, self).__init__(planning_range, segment_length, map_file)
self.rrt = RRT(self.planningRange, self.segmentLength)
if self.mapFile!= None:
self.rrt.loadMap(self.mapFile)
self.rrt_viz = RRTVisualizer(self.rrt)
from kRRTstar import *
from RRTVisualizer import *
if __name__ == '__main__':
MAP_FILE = './lab-map-inferior.png'
MAP_FILE = './lab-map-scaled.png'
start = [40, 40]
goal = [320, 300]
def calcDist(currentNode, referenceNode):
dist = 0.0
if referenceNode == None:
return dist
dist = np.sqrt((currentNode.pos[0] - referenceNode.pos[0])**2 +
(currentNode.pos[1] - referenceNode.pos[1])**2)
return dist
krrts = kRRTstar([444, 989], 10)
krrts.loadMap(MAP_FILE)
krrts_viz = RRTVisualizer(krrts)
krrts.init(start, goal, calcDist)
#rrt.init([40,40], [200, 200])
for i in range(2000):
krrts.extend()
krrts_viz.update()
'''
Created on Jan 3, 2015
@author: daqing_yi
'''
from RRG import *
from RRTVisualizer import *
if __name__ == '__main__':
def calcDist(currentNode, referenceNode):
dist = 0.0
if referenceNode == None:
return dist
dist = np.sqrt((currentNode.pos[0] - referenceNode.pos[0])**2 +
(currentNode.pos[1] - referenceNode.pos[1])**2)
return dist
rrg = RRG([600, 400], 10)
rrg_viz = RRTVisualizer(rrg)
rrg.init([40, 40], [320, 300], calcDist)
for i in range(1000):
#print i
rrg.extend()
rrg_viz.update()
from RRTstar import *
from RRTVisualizer import *
if __name__ == '__main__':
MAP_FILE = './lab-map-inferior.png'
MAP_FILE = './lab-map-scaled.png'
start = [40, 40]
goal = [320, 300]
def calcDist(currentNode, referenceNode):
dist = 0.0
if referenceNode == None:
return dist
dist = np.sqrt((currentNode.pos[0] - referenceNode.pos[0])**2 +
(currentNode.pos[1] - referenceNode.pos[1])**2)
return dist
rrts = RRTstar([444, 989], 10)
rrts.loadMap(MAP_FILE)
rrts_viz = RRTVisualizer(rrts)
rrts.init(start, goal, calcDist)
#rrt.init([40,40], [200, 200])
for i in range(2000):
rrts.extend()
rrts_viz.update()