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

Switched to PyRoboViz for visualization

Browse Source
This commit is contained in:
simondlevy
2019-01-03 15:27:30 -05:00
parent 5d20b17e7d
commit 8b3e13f09b
7 changed files with 12 additions and 162 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
README.md
View File

@@ -83,8 +83,8 @@ map and robot trajctory for the Lidar scan and odometry data in the log file
you can also try the <b><tt>log2png.py</tt></b> script to generate a
a PNG file instead.
If you have installed Matplotlib, you can see a &ldquo;live&rdquo; animation
by doing
If you have installed [PyRoboViz]()https://github.com/simondlevy/PyRoboViz),
you can see a &ldquo;live&rdquo; animation by doing
<pre>
make movie

4
examples/logmovie.py
View File

@@ -40,7 +40,7 @@ MAP_SIZE_METERS = 32
from breezyslam.algorithms import Deterministic_SLAM, RMHC_SLAM
from mines import MinesLaser, Rover, load_data
from pltslamshow import SlamShow
from roboviz import Visualizer
from sys import argv, exit
from time import sleep
@@ -112,7 +112,7 @@ def main():
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display, named by dataset
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, dataset)
display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, dataset)
# Pose will be modified in our threaded code
pose = [0,0,0]

150
examples/pltslamshow.py
View File

@@ -1,150 +0,0 @@
'''
pltslamshow.py - Pyplot classes for displaying maps and robots in SLAM projects
Copyright (C) 2016 Simon D. Levy, Matt Lubas, and Alfredo Rwagaju
This code is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation, either version 3 of the
License, or (at your option) any later version.
This code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this code. If not, see <http://www.gnu.org/licenses/>.
'''
# Robot display params
ROBOT_HEIGHT_MM = 500
ROBOT_WIDTH_MM = 300
# This helps with Raspberry Pi
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
import matplotlib.cm as colormap
from math import sin, cos, radians
import numpy as np
from time import sleep
class SlamShow(object):
def __init__(self, map_size_pixels, map_scale_mm_per_pixel, title):
# Store constants for update
self.map_size_pixels = map_size_pixels
self.map_scale_mm_per_pixel = map_scale_mm_per_pixel
# Create a byte array to display the map with a color overlay
self.bgrbytes = bytearray(map_size_pixels * map_size_pixels * 3)
# Make a nice big (10"x10") figure
fig = plt.figure(figsize=(10,10))
# Store Python ID of figure to detect window close
self.figid = id(fig)
fig.canvas.set_window_title('SLAM')
plt.title(title)
self.ax = fig.gca()
self.ax.set_aspect("auto")
self.ax.set_autoscale_on(True)
# Use an "artist" to speed up map drawing
self.img_artist = None
# We base the axis on pixels, to support displaying the map
self.ax.set_xlim([0, map_size_pixels])
self.ax.set_ylim([0, map_size_pixels])
# Hence we must relabel the axis ticks to show millimeters
ticks = np.arange(0,self.map_size_pixels+100,100)
labels = [str(self.map_scale_mm_per_pixel * tick) for tick in ticks]
self.ax.xaxis.set_ticks(ticks)
self.ax.set_xticklabels(labels)
self.ax.yaxis.set_ticks(ticks)
self.ax.set_yticklabels(labels)
self.ax.set_xlabel('X (mm)')
self.ax.set_ylabel('Y (mm)')
self.ax.grid(False)
# Start vehicle at center
map_center_mm = map_scale_mm_per_pixel * map_size_pixels
self._add_vehicle(map_center_mm,map_center_mm,0)
def displayMap(self, mapbytes):
mapimg = np.reshape(np.frombuffer(mapbytes, dtype=np.uint8), (self.map_size_pixels, self.map_size_pixels))
# Pause to allow display to refresh
sleep(.001)
if self.img_artist is None:
self.img_artist = self.ax.imshow(mapimg, cmap=colormap.gray)
else:
self.img_artist.set_data(mapimg)
def setPose(self, x_mm, y_mm, theta_deg):
'''
Sets vehicle pose:
X: left/right (cm)
Y: forward/back (cm)
theta: rotation (degrees)
'''
#remove old arrow
self.vehicle.remove()
#create a new arrow
self._add_vehicle(x_mm, y_mm, theta_deg)
def _add_vehicle(self, x_mm, y_mm, theta_deg):
#Use a very short arrow shaft to orient the head of the arrow
dx, dy = plt_rotate(0, 0, 0.1, theta_deg)
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):
# If we have a new figure, something went wrong (closing figure failed)
if self.figid != id(plt.gcf()):
return False
# Redraw current objects without blocking
plt.draw()
# Refresh display, setting flag on window close or keyboard interrupt
try:
plt.pause(.01) # Arbitrary pause to force redraw
return True
except:
return False
return True
# Converts millimeters to pixels
def mm2pix(self, mm):
return int(mm / float(self.map_scale_mm_per_pixel))
# Helpers -------------------------------------------------------------
def plt_rotate(x, y, r, deg):
rad = radians(deg)
c = cos(rad)
s = sin(rad)
dx = r * c
dy = r * s
return x+dx, y+dy

4
examples/rpslam.py
View File

@@ -32,7 +32,7 @@ MIN_SAMPLES = 200
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel
from rplidar import RPLidar as Lidar
from pltslamshow import SlamShow
from roboviz import Visualizer
if __name__ == '__main__':
@@ -43,7 +43,7 @@ if __name__ == '__main__':
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
# Initialize an empty trajectory
trajectory = []

4
examples/rpslam_scipy.py
View File

@@ -33,7 +33,7 @@ from breezyslam.sensors import RPLidarA1 as LaserModel
from rplidar import RPLidar as Lidar
from pltslamshow import SlamShow
from roboviz import Visualizer
from scipy.interpolate import interp1d
import numpy as np
@@ -47,7 +47,7 @@ if __name__ == '__main__':
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
# Initialize an empty trajectory
trajectory = []

4
examples/urgslam.py
View File

@@ -28,7 +28,7 @@ from breezyslam.sensors import URG04LX as LaserModel
from breezylidar import URG04LX as Lidar
from pltslamshow import SlamShow
from roboviz import Visualizer
if __name__ == '__main__':
@@ -39,7 +39,7 @@ if __name__ == '__main__':
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)

4
examples/xvslam.py
View File

@@ -28,7 +28,7 @@ from breezyslam.sensors import XVLidar as LaserModel
from xvlidar import XVLidar as Lidar
from pltslamshow import SlamShow
from roboviz import Visualizer
from sys import stdout
@@ -41,7 +41,7 @@ if __name__ == '__main__':
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
display = Visualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
# Initialize an empty trajectory
trajectory = []