def main():
lidar = Lidar(device='/dev/ttyACM0')
slam = RMHC_SLAM(LaserModel(), map_size_pixels=MAP_SIZE_PIXELS, map_size_meters=MAP_SIZE_METERS)
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
#image_thread = threading.Thread(target=save_image, args=[display])
#image_thread.start()
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
slam.update(lidar.getScan())
x, y, theta = slam.getpos()
#print(x, y, theta)
slam.getmap(mapbytes)
display.displayMap(mapbytes)
display.setPose(x, y, theta)
#display.save_image()
display.save_pgm(mapbytes)
if not display.refresh():
exit(0)
MAP_SIZE_PIXELS = 500
MAP_SIZE_METERS = 10
LIDAR_DEVICE = '/dev/ttyACM0'
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import URG04LX as LaserModel
from breezylidar import URG04LX as Lidar
from pltslamshow import SlamShow
if __name__ == '__main__':
# Connect to Lidar unit
lidar = Lidar(LIDAR_DEVICE)
# Create an RMHC SLAM object with a laser model and optional robot model
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')
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
# Update SLAM with current Lidar scan
slam.update(lidar.getScan())