def setup_algorithm(self): """Override this""" print '(setup_algorithm) Implement me!' from costmap import Costmap2D from obstacle import Obstacle from costmapwidget import Costmap2DWidget self.costmap = Costmap2D(DEFAULT_WIDTH, DEFAULT_HEIGHT, resolution=DEFAULT_RESOLUTION) Obstacle(3, 3, 3, 3).draw(self.costmap) Obstacle(5, 9, 3, 3).draw(self.costmap) Obstacle(4, 16, 3, 3).draw(self.costmap) self.costmap_widget = Costmap2DWidget(self.costmap, parent=self)
def setup_algorithm(self): """Sets up the algorithm""" self.costmap = Costmap2D(DEFAULT_WIDTH, DEFAULT_HEIGHT, resolution=DEFAULT_RESOLUTION) Obstacle(3, 3, 3, 3).draw(self.costmap) Obstacle(9, 5, 3, 3).draw(self.costmap) Obstacle(16, 4, 3, 3).draw(self.costmap) self.costmap_widget = Costmap2DWidget(self.costmap, parent=self, show_goal=False, show_start=False, show_colorbar=self.colorbar) self.pf = PotentialField(self.costmap)
def setup_algorithm(self): """Sets up the algorithm""" self.costmap = Costmap2D(DEFAULT_WIDTH, DEFAULT_HEIGHT, resolution=DEFAULT_RESOLUTION) Obstacle(3, 3, 3, 3).draw(self.costmap) Obstacle(9, 5, 3, 3).draw(self.costmap) Obstacle(16, 4, 3, 3).draw(self.costmap) self.costmap_widget = Costmap2DWidget(self.costmap, parent=self, show_goal=False, show_colorbar=self.colorbar) self.costmap_widget.canvas.show_start = False self.costmap_widget.canvas.show_goal = False self.vo = VoronoiExpansion(self.costmap)
def setup_algorithm(self): """Sets up the algorithm""" self.costmap = Costmap2D(DEFAULT_WIDTH, DEFAULT_HEIGHT, resolution=DEFAULT_RESOLUTION) Obstacle(3, 3, 3, 3).draw(self.costmap) Obstacle(9, 5, 3, 3).draw(self.costmap) Obstacle(16, 4, 3, 3).draw(self.costmap) self.costmap_widget = Costmap2DWidget(self.costmap, parent=self, show_goal=False, show_colorbar=self.colorbar) self.costmap_widget.canvas.show_start = True self.costmap_widget.canvas.show_goal = False self.be = BrushfireExpansion(self.costmap) temp = self.costmap_widget.canvas.start_coord self.start_coord = (floor(temp[0] + 0.5), floor(temp[1] + 0.5)) self.be.set_ignition_cells([self.start_coord])
def setup_algorithm(self): """Sets up the algorithm""" self.costmap = Costmap2D(DEFAULT_WIDTH, DEFAULT_HEIGHT, resolution=DEFAULT_RESOLUTION) Obstacle(3, 3, 3, 3).draw(self.costmap) Obstacle(9, 5, 3, 3).draw(self.costmap) Obstacle(16, 4, 3, 3).draw(self.costmap) self.costmap_widget = Costmap2DWidget(self.costmap, parent=self, show_goal=False, show_colorbar=True) self.costmap_widget.canvas.show_start = True self.costmap_widget.canvas.show_goal = True temp = self.costmap_widget.canvas.start_coord self.start_coord = (floor(temp[0] + 0.5), floor(temp[1] + 0.5)) temp = self.costmap_widget.canvas.goal_coord self.goal_coord = (floor(temp[0] + 0.5), floor(temp[1] + 0.5)) self.a_star = AStar(self.costmap, self.start_coord, self.goal_coord, self.heuristic)
if came_from[current_node]: p = reconstruct_path(came_from, came_from[current_node]) p.append(current_node) return p else: return [current_node] if __name__ == '__main__': from costmap import Costmap2D from obstacle import Obstacle from matplotlib.pyplot import plot, show from matplotlib.pylab import imshow, show, figure import time c = Costmap2D(10, 20, resolution=0.5) c.goal = (0, 0) c.start = (c.width - 1, c.height - 1) Obstacle(4, 3, 3, 3).draw(c) Obstacle(5, 9, 3, 3).draw(c) Obstacle(4, 16, 3, 3).draw(c) a_star = AStar(c, c.start, c.goal, naive) start = time.time() a_star.solve() end = time.time() print 'Naive:', end - start imshow(a_star.field.data, interpolation='nearest')
while be.step_solution(): pass # time.sleep(0.1) def _run_voronoi_expansion(c): """Runs the voronoi expansion""" import time; time.sleep(3) from voronoi import VoronoiExpansion ve = VoronoiExpansion(c) while ve.step_solution(): pass # time.sleep(0.1) if __name__ == '__main__': try: c = Costmap2D(10,20,resolution=1.0) Obstacle(3,3,3,3).draw(c) Obstacle(5,9,3,3).draw(c) Obstacle(4,16,3,3).draw(c) app = QtGui.QApplication(sys.argv) cw = Costmap2DWidget(c) import threading threading.Thread(target=_run_brushfire, args=(c,)).start() # threading.Thread(target=_run_voronoi_expansion, args=(c,)).start() cw.show() sys.exit(app.exec_()) except SystemExit: pass