vato007/breezyslam
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Display map and vehicle, slowly, in pixel coordinates

Browse Source
This commit is contained in:
simondlevy
2016-08-23 12:07:49 -04:00
parent 74d93af6d1
commit 81dfdadc43
2 changed files with 16 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/logdemoplt.py
View File

@@ -100,12 +100,12 @@ def main():
x_mm, y_mm, theta_degrees = slam.getpos() x_mm, y_mm, theta_degrees = slam.getpos()
# Get new map # Get new map
#slam.getmap(mapbytes) slam.getmap(mapbytes)
# Display map and robot pose # 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 # Exit gracefully if user closes display
if not display.refresh(): if not display.refresh():

36
examples/pltslamshow.py
View File

@@ -76,11 +76,14 @@ class SlamShow(object):
map_size_mm = map_scale_mm_per_pixel * map_size_pixels map_size_mm = map_scale_mm_per_pixel * map_size_pixels
self.ax.set_xlim([0, map_size_mm]) #self.ax.set_xlim([0, map_size_mm])
self.ax.set_ylim([0, map_size_mm]) #self.ax.set_ylim([0, map_size_mm])
self.ax.set_xlabel('X (mm)') self.ax.set_xlim([0, map_size_pixels])
self.ax.set_ylabel('Y (mm)') self.ax.set_ylim([0, map_size_pixels])
#self.ax.set_xlabel('X (mm)')
#self.ax.set_ylabel('Y (mm)')
self.ax.grid(False) 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)) 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') plt.set_cmap('gray')
# Interleave the grayscale map bytes into the color bytes # Interleave the grayscale map bytes into the color bytes
@@ -99,22 +102,6 @@ class SlamShow(object):
self.bgrbytes[1::3] = mapbytes self.bgrbytes[1::3] = mapbytes
self.bgrbytes[2::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): def setPose(self, x_mm, y_mm, theta_deg):
''' '''
Sets vehicle pose: Sets vehicle pose:
@@ -133,7 +120,10 @@ class SlamShow(object):
#Use a very short arrow shaft to orient the head of the arrow #Use a very short arrow shaft to orient the head of the arrow
dx, dy = plt_rotate(0, 0, 0.1, theta_deg) 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): def refresh(self):
@@ -142,7 +132,7 @@ class SlamShow(object):
return False return False
# Redraw current objects without blocking # Redraw current objects without blocking
#plt.draw() plt.draw()
# Refresh display, setting flag on window close or keyboard interrupt # Refresh display, setting flag on window close or keyboard interrupt
try: try: