def testThetaStar(self):
_, _, testMap = self.getSampleMapRRT(3)
robotOutline = Polygon.Polygon([(-.25,0), (.25,0), (0,.5)])
robot = RRTRobot(np.array([0,.2,np.pi/2]), robotOutline, .5)
plotter = RRTPlotter()
planner = DipolarRRT(testMap, robot, plotter)
planner.DEBUGER = self.DEBUGER
planner.PLOT_TREE = self.PLOT_TREE
planner.PLOT_TREE_FAIL = self.PLOT_TREE_FAIL
currDipole = np.array([1, 1, 0])
dirV = np.array([.5, 0, 0])
rrtPath = []
for i in range(10):
rrtPath.append(currDipole)
currDipole = currDipole + dirV
rrtPath = planner.dipolesToNodes(rrtPath)
shortPath = planner.getThetaStarPath(rrtPath)
if self.PLOT_SHORT_PATH:
pathT = planner.get2DPathRepresent(shortPath)
plotter.drawDipolePath2D(pathT, color='r', width=3)
def runRRTDipoleControlAndThetaStar(self):
""" Run the dipolar RRT/shortcut and plot according to parameters
specified in this class' init function.
"""
if not self.DEBUGER:
plt.ion()
startPose, endPose, testMap = self.getSampleMapRRT(3)
robot = RRTRobot.circularRobot((0,0,0), .5)
plotter = RRTPlotter()
plotter.drawMap(testMap)
plotter.drawStartAndGoalRobotShape(startPose, endPose, robot)
planner = DipolarRRT(testMap, robot, plotter=plotter)
planner.DEBUGER = self.DEBUGER
planner.PLOT_TREE = self.PLOT_TREE
planner.PLOT_TREE_FAIL = self.PLOT_TREE_FAIL
rrtPath = planner.getRRTDipoleControlPath(startPose, [endPose])
if rrtPath == None:
print "Did not find path in given time."
return
if self.PLOT_RRT_PATH:
pathT = planner.get2DPathRepresent(rrtPath)
plotter.drawDipolePath2D(pathT, color='g', width=3)
print "Getting theta star Path"
shortPath = planner.getThetaStarPath(rrtPath)
print "Done Smoothing Path"
if self.PLOT_SHORT_PATH:
pathT = planner.get2DPathRepresent(shortPath)
plotter.drawDipolePath2D(pathT, color='r', width=3)
shortPath = planner.getThetaStarPath(rrtPath)
print "Showing Final Results"
plt.ioff()
plt.show()
