From 2dcce2bc909a8e90ea4f8ca2dd23fee697d1e94b Mon Sep 17 00:00:00 2001
From: William Woodall <wjwwood@gmail.com>
Date: Wed, 12 Oct 2011 18:42:33 -0500
Subject: [PATCH] The Costmap2DWidget is done.

---
 hw4/costmapwidget.py | 87 +++++++++++++++++++++++++-------------------
 1 file changed, 50 insertions(+), 37 deletions(-)

diff --git a/hw4/costmapwidget.py b/hw4/costmapwidget.py
index 36c1dca..40f0dc7 100644
--- a/hw4/costmapwidget.py
+++ b/hw4/costmapwidget.py
@@ -5,31 +5,38 @@
 
 from PySide import QtCore, QtGui
 
-import matplotlib
-matplotlib.use('AGG')
-from matplotlib.pylab import gcf, imshow, draw
+# Force PySide
+import os; os.environ['QT_API'] = 'pyside'
+from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas
+from matplotlib.figure import Figure
+from matplotlib.backends.backend_qt4agg import NavigationToolbar2QTAgg as NavigationToolbar
+from matplotlib.pylab import imshow
 
 from costmap import Costmap2D
 from obstacle import Obstacle
 
-class Costmap2DWidget(QtGui.QWidget):
+class Costmap2DFigure(FigureCanvas):
     costmap_changed = QtCore.Signal()
     
-    """Implements a widget that will display a costmap"""
-    def __init__(self, costmap, parent = None):
-        QtGui.QWidget.__init__(self, parent)
+    """Implements an imshow figure for showing the costmap2d"""
+    def __init__(self, costmap, parent=None, width=5.0, height=4.0, dpi=100, interpolation='nearest'):
         self.costmap = costmap
+        self.interpolation = interpolation
+        fig = Figure(figsize=(width, height), dpi=dpi)
+        self.axes = fig.add_subplot(111)
+        # We want the axes cleared every time plot() is called
+        self.axes.hold(False)
         
-        # Setup display
-        self.imshow = imshow(self.costmap.data.T, interpolation='nearest')
-        self.figure = gcf()
-        self.image_label = QtGui.QLabel()
-        self.image_label.setBackgroundRole(QtGui.QPalette.Base)
+        self.compute_initial_figure()
         
-        # Setup layout
-        layout = QtGui.QVBoxLayout()
-        layout.addWidget(self.image_label)
-        self.setLayout(layout)
+        FigureCanvas.__init__(self, fig)
+        self.setParent(parent)
+        
+        FigureCanvas.setSizePolicy(self,
+                                   QtGui.QSizePolicy.Expanding,
+                                   QtGui.QSizePolicy.Expanding)
+        
+        FigureCanvas.updateGeometry(self)
         
         # Idle control
         self.idle = True
@@ -40,19 +47,6 @@ def __init__(self, costmap, parent = None):
         # Override costmap on change
         self.costmap.on_update = self.costmap_update_callback
     
-    def paintEvent(self, vent):
-        """Overrides the default paint event"""
-        self.figure = gcf()
-        # Render the canvas
-        self.figure.canvas.draw()
-        # Convert the render into a string buffer
-        str_buffer = self.figure.canvas.buffer_rgba(0,0)
-        # Get the bounding box of the render
-        l,b,w,h = self.figure.bbox.bounds
-        # Create and set the Pixmap from the render
-        image = QtGui.QImage(str_buffer, w, h, QtGui.QImage.Format_ARGB32)
-        self.image_label.setPixmap(QtGui.QPixmap.fromImage(image))
-    
     def connect_stuff(self):
         """Make Qt connections"""
         self.costmap_changed.connect(self.on_map_update)
@@ -63,37 +57,56 @@ def costmap_update_callback(self, key, val):
             self.idle = False
             self.costmap_changed.emit()
     
+    def compute_initial_figure(self):
+        """Plot the imshow"""
+        self.axes.imshow(self.costmap.data.T, interpolation=self.interpolation)
+    
     def on_map_update(self):
         """Slot to handle the costmap_changed signal"""
-        self.imshow.set_array(self.costmap.data.T)
-        print type(self.imshow)
-        self.repaint()
-        print('Should be repainting')
+        self.imshow = self.axes.imshow(self.costmap.data.T, interpolation=self.interpolation)
+        self.draw()
         self.idle = True
     
 
+class Costmap2DWidget(QtGui.QWidget):
+    """Implements a widget that will display a costmap figure with a toolbar"""
+    def __init__(self, costmap, parent = None):
+        QtGui.QWidget.__init__(self, parent)
+        self.costmap = costmap
+        
+        # Setup display
+        self.canvas = Costmap2DFigure(costmap)
+        self.toolbar = NavigationToolbar(self.canvas, self)
+        
+        # Setup layout
+        layout = QtGui.QVBoxLayout()
+        layout.addWidget(self.toolbar)
+        layout.addWidget(self.canvas)
+        self.setLayout(layout)
+    
+
 def _run_brushfire(c):
     """Runs the brushfire"""
+    import time; time.sleep(3)
     from brushfire import BrushfireExpansion
     be = BrushfireExpansion(c)
     be.set_ignition_cells([(0,0)])
-    import time
     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)
-    import time
     while ve.step_solution():
         pass
-        time.sleep(0.1)
+        # time.sleep(0.1)
 
 if __name__ == '__main__':
     try:
-        c = Costmap2D(10,20,resolution=0.1)
+        c = Costmap2D(20,20,resolution=0.1)
         Obstacle(3,3,3,3).draw(c)
         Obstacle(5,9,3,3).draw(c)
         Obstacle(4,16,3,3).draw(c)