Cleanup
This commit is contained in:
@@ -47,7 +47,7 @@ from time import sleep
|
|||||||
from threading import Thread
|
from threading import Thread
|
||||||
import pickle
|
import pickle
|
||||||
|
|
||||||
def threadfunc(robot, slam, timestamps, lidars, odometries, use_odometry, mapbytes, pose):
|
def threadfunc(robot, slam, timestamps, lidars, odometries, mapbytes, pose):
|
||||||
'''
|
'''
|
||||||
Threaded function runs SLAM, setting the map bytes and robot pose for display
|
Threaded function runs SLAM, setting the map bytes and robot pose for display
|
||||||
on the main thread.
|
on the main thread.
|
||||||
@@ -59,7 +59,12 @@ def threadfunc(robot, slam, timestamps, lidars, odometries, use_odometry, mapbyt
|
|||||||
# Loop over scans
|
# Loop over scans
|
||||||
for scanno in range(len(lidars)):
|
for scanno in range(len(lidars)):
|
||||||
|
|
||||||
if use_odometry:
|
if odometries is None:
|
||||||
|
|
||||||
|
# Update SLAM with lidar alone
|
||||||
|
slam.update(lidars[scanno])
|
||||||
|
|
||||||
|
else:
|
||||||
|
|
||||||
# Convert odometry to velocities
|
# Convert odometry to velocities
|
||||||
velocities = robot.computeVelocities(odometries[scanno])
|
velocities = robot.computeVelocities(odometries[scanno])
|
||||||
@@ -67,11 +72,6 @@ def threadfunc(robot, slam, timestamps, lidars, odometries, use_odometry, mapbyt
|
|||||||
# Update SLAM with lidar and velocities
|
# Update SLAM with lidar and velocities
|
||||||
slam.update(lidars[scanno], velocities)
|
slam.update(lidars[scanno], velocities)
|
||||||
|
|
||||||
else:
|
|
||||||
|
|
||||||
# Update SLAM with lidar alone
|
|
||||||
slam.update(lidars[scanno])
|
|
||||||
|
|
||||||
# Get new position
|
# Get new position
|
||||||
pose[0],pose[1],pose[2] = slam.getpos()
|
pose[0],pose[1],pose[2] = slam.getpos()
|
||||||
|
|
||||||
@@ -118,7 +118,7 @@ def main():
|
|||||||
pose = [0,0,0]
|
pose = [0,0,0]
|
||||||
|
|
||||||
# Launch the data-collection / update thread
|
# Launch the data-collection / update thread
|
||||||
thread = Thread(target=threadfunc, args=(robot, slam, timestamps, lidars, odometries, use_odometry, mapbytes, pose))
|
thread = Thread(target=threadfunc, args=(robot, slam, timestamps, lidars, odometries if use_odometry else None, mapbytes, pose))
|
||||||
thread.daemon = True
|
thread.daemon = True
|
||||||
thread.start()
|
thread.start()
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user