def main():
# Bozo filter for input args
if len(argv) < 4:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Allocate byte array to receive map updates
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Load the data from the file
timestamps, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
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)
# Pose will be modified in our threaded code
pose = [0, 0, 0]
# Launch the data-collection / update thread
thread = Thread(target=threadfunc,
args=(robot, slam, timestamps, lidars,
odometries if use_odometry else None, mapbytes,
pose))
thread.daemon = True
thread.start()
# Loop forever,displaying current map and pose
while True:
# Display map and robot pose
display.displayMap(mapbytes)
display.setPose(*pose)
# Refresh the display, exiting gracefully if user closes it
if not display.refresh():
exit(0)
# 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())
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
display.displayMap(mapbytes)
display.setPose(x, y, theta)
# Exit on ESCape
key = display.refresh()
if key != None and (key & 0x1A):
exit(0)
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances,
scan_angles_degrees=previous_angles)
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display the map
display.displayMap(mapbytes)
# Display the robot's pose in the map
display.setPose(x, y, theta)
# Break on window close
if not display.refresh():
break
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
display = SlamShow(MAP_SIZE_PIXELS,
MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, 'SLAM')
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
try:
# scans = [[data, timestamp], ....]
for pts, dt in scans:
# need to reverse the order for it to plot correctly
slam.update(list(reversed(pts)))
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# display map
display.displayMap(mapbytes)
display.setPose(x, y, theta)
display.refresh()
# scan is 5 Hz
time.sleep(1 / 5)
while True: # just leave it up and running for a while
continue
except KeyboardInterrupt:
print("got ctrl-C ...")
async def main():
#---------------------------------------
# Setup Lidar unit and SLAM
#---------------------------------------
# Connect to Lidar unit
lidar = nanoLidar.nanoLidar(host)
# get firmware info
print("Lidar Version:")
print(lidar.getInfo())
# 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 an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
#---------------------------------------
# Setup Robot unit
#---------------------------------------
# Connect to robot unit
robot = robotPosition.robotPosition(host)
robot.powerOn()
# get firmware info
print("Chassis Version:")
print(robot.getInfo())
#---------------------------------------
# main loop
#---------------------------------------
run = True
while run:
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
slam.update([pair[0] for pair in lidar.getScan()])
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
display.displayMap(mapbytes)
display.setPose(x, y, theta)
# Exit on ESCape
key = display.refresh()
if key != None and (key & 0x1A):
run = False
# Movement control
distance = int(input("Distance [mm]: "))
angle = int(input("Turn angle [degrees]: "))
(xp, yp, theta_deg, theta_rad) = robot.setPosition(distance, angle)
if int(input("Carry on (1/0): ")) == 0:
run = False
# power off stepper motors
robot.powerOff()
self.distance_no_detection_mm = 10000 #10m
self.detection_margin = 0
self.offset_mm = offset_mm
WINDOW_SIZE = 1200
MAP_SIZE_METERS = 10
lidar = DIYLidar(log = False)
lidar_properties = DIYLidar_Properties(lidar.getPointsPerLap())
mapbytes = bytearray(WINDOW_SIZE * WINDOW_SIZE)
slam = RMHC_SLAM(lidar_properties, WINDOW_SIZE, 35)
window = SlamShow(WINDOW_SIZE, MAP_SIZE_METERS*1000/WINDOW_SIZE, "scan")
pose = [0,0,0]
while True:
scan = lidar.waitForNextScan()
if scan != False:
slam.update(scan.getPointsDistances_mm())
x, y, theta = slam.getpos()
pose = [x /10.0, y / 10.0, theta]
slam.getmap(mapbytes)
window.displayMap(mapbytes)
window.setPose(*pose)
if not window.refresh():
lidar.Quit()
exit(0)