From 81dfdadc438963920e202010a5d33997c37ffe99 Mon Sep 17 00:00:00 2001
From: simondlevy <simon.d.levy@gmail.com>
Date: Tue, 23 Aug 2016 12:07:49 -0400
Subject: [PATCH] Display map and vehicle, slowly, in pixel coordinates

---
 examples/logdemoplt.py  |  6 +++---
 examples/pltslamshow.py | 36 +++++++++++++-----------------------
 2 files changed, 16 insertions(+), 26 deletions(-)

diff --git a/examples/logdemoplt.py b/examples/logdemoplt.py
index d008655..3292ba0 100755
--- a/examples/logdemoplt.py
+++ b/examples/logdemoplt.py
@@ -100,12 +100,12 @@ def main():
         x_mm, y_mm, theta_degrees = slam.getpos()    
 
         # Get new map
-        #slam.getmap(mapbytes)
+        slam.getmap(mapbytes)
 
         # Display map and robot pose
-        #display.displayMap(mapbytes)
+        display.displayMap(mapbytes)
 
-        #display.setPose(x_mm, y_mm, theta_degrees)   
+        display.setPose(x_mm, y_mm, theta_degrees)   
 
         # Exit gracefully if user closes display
         if not display.refresh():
diff --git a/examples/pltslamshow.py b/examples/pltslamshow.py
index 26625b1..cce9d16 100644
--- a/examples/pltslamshow.py
+++ b/examples/pltslamshow.py
@@ -76,11 +76,14 @@ class SlamShow(object):
 
         map_size_mm = map_scale_mm_per_pixel * map_size_pixels
 
-        self.ax.set_xlim([0, map_size_mm])
-        self.ax.set_ylim([0, map_size_mm])
+        #self.ax.set_xlim([0, map_size_mm])
+        #self.ax.set_ylim([0, map_size_mm])
 
-        self.ax.set_xlabel('X (mm)')
-        self.ax.set_ylabel('Y (mm)')
+        self.ax.set_xlim([0, map_size_pixels])
+        self.ax.set_ylim([0, map_size_pixels])
+
+        #self.ax.set_xlabel('X (mm)')
+        #self.ax.set_ylabel('Y (mm)')
 
         self.ax.grid(False)
 
@@ -91,7 +94,7 @@ class SlamShow(object):
 
         mapimg = np.reshape(np.frombuffer(mapbytes, dtype=np.uint8), (self.map_size_pixels, self.map_size_pixels))
  
-        #plt.imshow(mapimg)
+        plt.imshow(mapimg)
         plt.set_cmap('gray')
 
         # Interleave the grayscale map bytes into the color bytes
@@ -99,22 +102,6 @@ class SlamShow(object):
         self.bgrbytes[1::3] = mapbytes
         self.bgrbytes[2::3] = mapbytes
 
-        # Put color bytes into image - this line causes error
-        #plt.set_cmap('BlueRedAlpha')
-
-    def displayVelocities(self, dxy_mm, dtheta_deg):
-        
-        # Add velocity bars
-        self.show_velocity(dxy_mm,      SENSOR_V_MAX_MM,      '   dXY', SENSOR_V_Y)
-        self.show_velocity(dtheta_deg,  SENSOR_THETA_MAX_DEG, 'dTheta', SENSOR_THETA_Y)
-                       
-    def displayTrajectory(self, trajectory):
-        
-        for k in range(1, len(trajectory)):
-            
-            x1_mm, y1_mm = trajectory[k-1]
-            x2_mm, y2_mm = trajectory[k]
-
     def setPose(self, x_mm, y_mm, theta_deg):
         '''
         Sets vehicle pose:
@@ -133,7 +120,10 @@ class SlamShow(object):
         #Use a very short arrow shaft to orient the head of the arrow
         dx, dy = plt_rotate(0, 0, 0.1, theta_deg)
 
-        self.vehicle=self.ax.arrow(x_mm, y_mm, dx, dy, head_width=ROBOT_WIDTH_MM, head_length=ROBOT_HEIGHT_MM, fc='r', ec='r')
+        s = self.map_scale_mm_per_pixel
+
+        self.vehicle=self.ax.arrow(x_mm/s, y_mm/s, 
+                dx, dy, head_width=ROBOT_WIDTH_MM/s, head_length=ROBOT_HEIGHT_MM/s, fc='r', ec='r')
 
     def refresh(self):                   
 
@@ -142,7 +132,7 @@ class SlamShow(object):
             return False
 
         # Redraw current objects without blocking
-        #plt.draw()
+        plt.draw()
 
         # Refresh display, setting flag on window close or keyboard interrupt
         try: