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

Fixed timestamps and used them in logdemocv

Browse Source
This commit is contained in:
Matt Lubas
2016-08-15 15:31:27 -04:00
parent 2cb34233b3
commit 8ec235baf3
2 changed files with 16 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
examples/logdemocv.py
View File

@@ -43,6 +43,7 @@ from mines import MinesLaser, Rover, load_data
from cvslamshow import SlamShow from cvslamshow import SlamShow
from sys import argv, exit from sys import argv, exit
from time import sleep
def main(): def main():
@@ -58,7 +59,7 @@ def main():
seed = int(argv[3]) if len(argv) > 3 else 0 seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file # Load the data from the file
timestamp, lidars, odometries = load_data('.', dataset) timestamps, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry # Build a robot model if we want odometry
robot = Rover() if use_odometry else None robot = Rover() if use_odometry else None
@@ -77,6 +78,9 @@ def main():
# Set up a SLAM display # Set up a SLAM display
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM') display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
# Store previous timestamp to create delay for realistic movie
prevtime = 0
# Loop over scans # Loop over scans
for scanno in range(nscans): for scanno in range(nscans):
@@ -96,7 +100,8 @@ def main():
# Get new position # Get new position
x_mm, y_mm, theta_degrees = slam.getpos() x_mm, y_mm, theta_degrees = slam.getpos()
print(scanno, x_mm, y_mm, theta_degrees)
#print(scanno, x_mm, y_mm, theta_degrees)
# Get current map # Get current map
slam.getmap(mapbytes) slam.getmap(mapbytes)
@@ -110,7 +115,11 @@ def main():
if key != None and (key&0x1A): if key != None and (key&0x1A):
exit(0) exit(0)
# XXX Add delay for real-time plot # Add delay for real-time plot
currtime = timestamps[scanno] / 1.e6 # Convert usec to sec
if prevtime > 0:
sleep(currtime-prevtime)
prevtime = currtime
# Helpers --------------------------------------------------------- # Helpers ---------------------------------------------------------

5
examples/mines.py
View File

@@ -65,10 +65,13 @@ def load_data(datadir, dataset):
toks = s.split()[0:-1] # ignore '' toks = s.split()[0:-1] # ignore ''
odometry = (int(toks[0]), int(toks[2]), int(toks[3])) timestamp = int(toks[0])
odometry = timestamp, int(toks[2]), int(toks[3])
lidar = [int(tok) for tok in toks[24:]] lidar = [int(tok) for tok in toks[24:]]
timestamps.append(timestamp)
scans.append(lidar) scans.append(lidar)
odometries.append(odometry) odometries.append(odometry)