diff --git a/examples/logdemoplt.py b/examples/logdemoplt.py index 3292ba0..9741d03 100755 --- a/examples/logdemoplt.py +++ b/examples/logdemoplt.py @@ -43,8 +43,39 @@ from mines import MinesLaser, Rover, load_data from pltslamshow import SlamShow from sys import argv, exit -from time import time +from time import time, sleep +from threading import Thread +def threadfunc(robot, slam, lidars, odometries, use_odometry, mapbytes, pose): + + # Loop over scans + for scanno in range(len(lidars)): + + if use_odometry: + + # Convert odometry to velocities + velocities = robot.computeVelocities(odometries[scanno]) + + # Update SLAM with lidar and velocities + slam.update(lidars[scanno], velocities) + + else: + + # Update SLAM with lidar alone + slam.update(lidars[scanno]) + + # Get new position + pose[0],pose[1],pose[2] = slam.getpos() + + # Get new map + slam.getmap(mapbytes) + + # XXX Add delay for real-time plot + sleep(.1) + + print(scanno) + + def main(): # Bozo filter for input args @@ -79,45 +110,31 @@ def main(): display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM') start_time = time() - - # Loop over scans - for scanno in range(nscans): - - if use_odometry: - - # Convert odometry to velocities - velocities = robot.computeVelocities(odometries[scanno]) - - # Update SLAM with lidar and velocities - slam.update(lidars[scanno], velocities) - - else: - - # Update SLAM with lidar alone - slam.update(lidars[scanno]) - - # Get new position - x_mm, y_mm, theta_degrees = slam.getpos() - # Get new map - slam.getmap(mapbytes) + pose = [0,0,0] + + # Launch the data-collection / update thread + thread = Thread(target=threadfunc, args=(robot, slam, lidars, odometries, use_odometry, mapbytes, pose)) + thread.daemon = True + thread.start() + + while True: # Display map and robot pose display.displayMap(mapbytes) + display.setPose(*pose) - display.setPose(x_mm, y_mm, theta_degrees) - - # Exit gracefully if user closes display + # Refresh the display, exiting gracefully if user closes it if not display.refresh(): exit(0) curr_time = time() print(curr_time - start_time) start_time = curr_time - - # XXX Add delay for real-time plot - - + + sleep(.01) + + # Helpers --------------------------------------------------------- def mm2pix(mm):