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()