Created on Jan 4, 2015
@author: daqing_yi
'''
from RRTstarPlanner import *
import time
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
MAP_FILE = './lab-map-inferior.png'
MAP_FILE = './lab-map-scaled.png'
planner = RRTstarPlanner([444, 989], 10, calcDist, MAP_FILE)
path = planner.findPath([40, 40], [320, 300], 1000)
print path
import pygame.image
pygame.image.save(planner.rrts_viz.screen, 'RRTstar01.png')
while True:
planner.rrts_viz.update()
'''
Created on Jan 4, 2015
@author: daqing_yi
'''
from RRTstarPlanner import *
import time
if __name__ == '__main__':
def calcDist(currentPos, referencePos):
dist = 0.0
if referencePos == None:
return dist
dist = np.sqrt((currentPos[0] - referencePos[0])**2 +
(currentPos[1] - referencePos[1])**2)
return dist
MAP_FILE = './map.png'
planner = RRTstarPlanner([600, 400], 10, calcDist, MAP_FILE)
path = planner.findPath([40, 40], [500, 40], 6000)
print path
import pygame.image
pygame.image.save(planner.rrts_viz.screen, 'RRTstar02.png')
while True:
planner.rrts_viz.update()
readInput = True
start_pos = mapMgrViz.convertedTrackingPosList[0]
end_pos = mapMgrViz.convertedTrackingPosList[
len(mapMgrViz.convertedTrackingPosList) - 1]
def calcDist(currentPos, referencePos):
dist = 0.0
if referencePos == None:
return dist
dist = np.sqrt((currentPos[0] - referencePos[0])**2 +
(currentPos[1] - referencePos[1])**2)
return dist
homoMgr = HomotopyMgr(mapMgrViz.world_map, mapMgrViz.reader)
homoMgr.init(mapMgrViz.refString)
planner = RRTstarPlanner([mapMgr.width, mapMgr.height], 10, calcDist,
MAP_FILE)
for subseg in mapMgr.subsegments:
planner.rrts_viz.refLines.append(
[subseg.line_seg.coords[0], subseg.line_seg.coords[1]])
path = planner.findPath(start_pos, end_pos, 1000, homoMgr)
print path
import pygame.image
pygame.image.save(planner.rrts_viz.screen, 'RRTstar02.png')
while True:
planner.rrts_viz.update()