def __init__(self, laser, map_size_pixels, map_size_meters,
map_quality=_DEFAULT_MAP_QUALITY, hole_width_mm=_DEFAULT_HOLE_WIDTH_MM,
random_seed=None, sigma_xy_mm=_DEFAULT_SIGMA_XY_MM, sigma_theta_degrees=_DEFAULT_SIGMA_THETA_DEGREES,
max_search_iter=_DEFAULT_MAX_SEARCH_ITER):
self.myfilter = FilterHandler(standardGH(g, h, 20000, 0), standardGH(g, h, 20000, 0), standardGH(g, h))
RMHC_SLAM.__init__(self, laser, map_size_pixels, map_size_meters,
map_quality, hole_width_mm,
random_seed, sigma_xy_mm, sigma_theta_degrees,
max_search_iter)
def __init__(self, laser, MAP_SIZE_M=4.0, MAP_RES_PIX_PER_M=100, MAP_DEPTH=5, HOLE_WIDTH_MM = 200, RANDOM_SEED = 0xabcd, **unused):
MAP_SIZE_PIXELS = int(MAP_SIZE_M*MAP_RES_PIX_PER_M) # number of pixels across the entire map
RMHC_SLAM.__init__(self, \
laser, \
MAP_SIZE_PIXELS, \
MAP_SIZE_M, \
MAP_DEPTH, \
HOLE_WIDTH_MM, \
RANDOM_SEED)
self.scanSize = laser.SCAN_SIZE
self.breezyMap = bytearray(MAP_SIZE_PIXELS**2) # initialize map array for BreezySLAM's internal mapping
def __init__(self):
Thread.__init__(self)
#self.lidar = Lidar(LIDAR_DEVICE)
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, map_quality=50, hole_width_mm=200, max_search_iter=10000)
self.N = 0
self.bot_pos = [[1500, 1300]]
self.bot_ori = [0]
self.state = True
def __init__(self, robot, laser, logFile=None, MAP_SIZE_M=8.0, MAP_RES_PIX_PER_M=100, USE_ODOMETRY=True, MAP_QUALITY=5, **unused):
self.USE_ODOMETRY = USE_ODOMETRY
MAP_SIZE_PIXELS = int(MAP_SIZE_M*MAP_RES_PIX_PER_M) # number of pixels across the entire map
RMHC_SLAM.__init__(self, \
laser, \
MAP_SIZE_PIXELS, \
MAP_SIZE_M, \
MAP_QUALITY, \
HOLE_WIDTH_MM, \
RANDOM_SEED)
self.robot = robot
self.scanSize = laser.SCAN_SIZE
self.logFile = logFile
self.prevEncPos = () # robot encoder data
self.currEncPos = () # left wheel [ticks], right wheel [ticks], timestamp [ms]
self.breezyMap = bytearray(MAP_SIZE_PIXELS**2) # initialize map array for BreezySLAM's internal mapping
def update(self, scan_mm, velocities=None, command=0):
"""
Calculates the amount of usefull pixels perframe and stores the time.
scan_mm: new scan
velocities: velocitie data
command: next to perform command
:param velocities:
:param command:
:param scan_mm:
"""
errors = 0
self.values = len(scan_mm)
for x in range(0, len(scan_mm)):
if scan_mm[x] == 0:
errors += 1
self.error = float(errors) / len(scan_mm)
if velocities == None:
self.time = None
else:
self.time = velocities[2]
self.command = command
RMHC_SLAM.update(self, scan_mm, velocities)
def _getNewPosition(self, start_position):
"""
Filters the rmhc slam result.
start_position: estimated position based on odometry
"""
# RMHC search is implemented as a C extension for efficiency
slam_position = RMHC_SLAM._getNewPosition(self, start_position)
#check slam values for reliable turn.
#vtheta = (slam_position.theta_degrees - start_position.theta_degrees)/self.time
#if(math.fabs(vtheta) > MAX_DEGREE_PER_S):
# vtheta = vtheta/math.fabs(vtheta) * MAX_DEGREE_PER_S
# slam_position.theta_degrees = start_position.theta_degrees + vtheta * self.time
if not filtering: return slam_position
if self.time == None:
return slam_position
return self.myfilter(slam_position, start_position, self.error, self.time, self.command)
class SLAM_Engine:
def __init__(self, size_pixels, size_meters):
self.map_size_pixels = size_pixels
self.map_size_meters = size_meters
self.map_scale_meters_per_pixel = float(size_meters) / float(
size_pixels)
self.mapbytes = bytearray(self.map_size_pixels * self.map_size_pixels)
self.robot = None
self.control = False
self.lidarShow = True
self.routine = False
#slam算法初始化
self.slam = RMHC_SLAM(MinesLaser(),
self.map_size_pixels,
self.map_size_meters,
map_quality=_DEFAULT_MAP_QUALITY,
hole_width_mm=_DEFAULT_HOLE_WIDTH_MM,
random_seed=999,
sigma_xy_mm=_DEFAULT_SIGMA_XY_MM,
sigma_theta_degrees=_DEFAULT_SIGMA_THETA_DEGREES,
max_search_iter=_DEFAULT_MAX_SEARCH_ITER)
self.map_view = OpenMap(self.map_size_pixels, self.map_size_meters)
self.pose = [0, 0, 0]
def set_auto_control(self):
self.control = True
def reset_auto_control(self):
self.control = False
self.map_view.path = []
self.map_view.point_list = []
def data_test(self, dataset='expX'):
timestamps, lidars, odometries = load_data_EAI('.', dataset)
for scanno in range(len(lidars)):
self.slam_run(lidars[scanno])
self.slam_view()
def NoLidarInit(self, dataset='expX'):
self.timestamps, self.lidars, self.odometries = load_data_EAI(
'.', dataset)
self.SLAM_continue = False
def NoLidarGetScans(self):
scans_m = []
if len(self.lidars) > 1:
scans = self.lidars.pop(0)
for scan in scans:
scans_m.append(float(scan) / 1000.0)
return scans, scans_m
else:
scans = self.lidars[0]
for scan in scans:
scans_m.append(float(scan) / 1000.0)
return scans, scans_m
def routine_switch(self):
self.map_view.path = self.map_view.routinePath.copy()
self.map_view.routinePath.reverse()
self.map_view.END_flag = False
def slam_control(self):
if self.control == True:
return self.map_view.track_control()
else:
return
def slam_run(self, scans):
self.slam.update(scans) #地图更新
self.pose[0], self.pose[1], self.pose[2] = self.slam.getpos() #位置预测
self.slam.getmap(self.mapbytes) #地图读取
self.map_view.setPose(self.pose[0] / 1000., self.pose[1] / 1000.,
self.pose[2])
def slam_view(self):
self.map_view.display(self.pose[0] / 1000.,
self.pose[1] / 1000.,
self.pose[2],
self.mapbytes,
test=False)
def slam_mjpg(self):
s = self.map_scale_meters_per_pixel
self.map_view.display_mjpg(self.pose[0] / 1000.,
self.pose[1] / 1000.,
self.pose[2],
self.mapbytes,
test=False)
pos = [
self.map_size_pixels - int((self.pose[0] / 1000.) / s),
int((self.pose[1] / 1000.) / s), self.pose[2]
]
return self.map_view.get_jpg(), pos
def setPoint(self, x, y):
return self.map_view.setPoint(self.map_size_pixels - x, y)
def _m2pix(self, x_m, y_m):
s = self.map_scale_meters_per_pixel
#p = self.map_size_pixels
return int(x_m / s), int(y_m / s)
def draw_history(self):
self.map_view.get_history_pose()
def del_history(self):
self.map_view.del_history_pose()
class Map(Thread):
MAP_SIZE_PIXELS = 1000
MAP_SIZE_METERS = 7
LIDAR_DEVICE = '/dev/ttyACM0'
THRESH_DETECTION = 100
RAW_ANGLE = np.pi/4
LIDAR_POS = np.array([140,140])
def __init__(self):
Thread.__init__(self)
#self.lidar = Lidar(LIDAR_DEVICE)
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, map_quality=50, hole_width_mm=200, max_search_iter=10000)
self.N = 0
self.bot_pos = [[1500, 1300]]
self.bot_ori = [0]
self.state = True
def run(self):
self.i = 0
x_th = 1500
y_th = 1300
while(self.state):
if(self.i<10):
y_th +=35
elif(self.i<33):
x_th -= 45
elif(self.i<45):
y_th -= 35
else:
y_th -= 10
x_th += 10
# Update SLAM with current Lidar scan
#self.slam.update(self.lidar.getScan())
data = getHokuyoData([x_th, y_th],
-self.i*5/180.0*np.pi, [np.array([[0, 2000, 2000, 0],[0, 0, 3000, 3000]]),
np.array([[650, 1350, 1350, 650],[1450, 1450, 1550, 1550]]),
np.array([[1500, 1600, 1600, 1500],[1000, 1000, 1100, 1100]]),
np.array([[1000+200*np.cos(k*10/180*np.pi) for k in range(0,36)],
[2500+200*np.sin(k*10/180*np.pi) for k in range(0,36)]])], 0)
list = data[1].tolist()
if(len(list) == 682):
self.slam.update(list, [0,0,0.1])
# Get current robot position
y, x, theta = self.slam.getpos()
x-=3500
y-=3500
x = -x
#y-=198
x+=1500
y+=1300
#angle = self.RAW_ANGLE + theta*3.1415/180
#R = np.array([[np.cos(angle), -np.sin(angle)],[np.sin(angle), np.cos(angle)]])
#pos = R.dot(np.array([x,y]))
pos = np.array([x,y])
self.bot_pos.append(pos)
self.bot_ori.append(theta)
self.i += 1
def stop(self):
self.state = False
LIDAR_DEVICE = '/dev/ttyACM0'
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.components import XVLidar as LaserModel
from xvlidar import XVLidar as Lidar
from cvslamshow 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 an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
slam.update([pair[0] for pair in lidar.getScan()])
LIDAR_DEVICE = '/dev/ttyACM0'
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import URG04LX as LaserModel
from breezylidar import URG04LX as Lidar
from roboviz import MapVisualizer
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
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, '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()
class Robot:
def __init__(self):
# Connect to Lidar unit
self.lidar = lidarReader('/dev/mylidar', 115200)
self.mover = MotorController()
# Create an RMHC SLAM object with a laser model and optional robot model
self.slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display
self.viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize empty map
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.left_distance_table = np.zeros(80)
self.right_distance_table = np.zeros(80)
self.thread = threading.Thread(target=self.navigate, args=())
self.thread.start()
def constructmap(self):
while True:
time.sleep(0.0001)
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
self.slam.update(self.lidar.getdata())
# Get current robot position
x, y, theta = self.slam.getpos()
# Get current map bytes as grayscale
self.slam.getmap(self.mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not self.viz.display(x / 1000., y / 1000., theta,
self.mapbytes):
break
exit(0)
def navigate(self):
while True:
time.sleep(
0.01) # (yield) allowing reading data from the serailport
front_too_close, left_too_close, right_too_close = False, False, False
if (self.lidar.angle_180 < 400):
front_too_close = True
left_angle = np.argmin(self.lidar.left_container)
right_angle = np.argmin(self.lidar.right_container)
if (self.lidar.left_container[left_angle] < 250):
left_too_close = True
if (self.lidar.right_container[right_angle] < 250):
right_too_close = True
if (front_too_close and left_too_close and right_too_close):
self.mover.backward()
elif (front_too_close):
if (self.lidar.angle_270 > self.lidar.angle_90):
self.mover.turn_left()
else:
self.mover.turn_right()
elif (left_too_close or right_too_close):
if (left_too_close):
self.mover.turn_right(left_angle)
else:
self.mover.turn_left(right_angle)
else:
self.mover.forward()
print(front_too_close, left_too_close, right_too_close,
self.lidar.angle_180)
import time
if __name__ == '__main__':
lidar = rplidar.RPLidar(sys.argv[1])
print('RPLidar health:', lidar.get_health()[0])
print('RPLidar info: ', lidar.get_info())
lidar.start_motor()
scans_iterator = lidar.iter_scans()
slam = RMHC_SLAM(RPLidarA1(), 800, 5)
uvon_dev = serial.Serial(sys.argv[2], 115200)
time.sleep(2)
uvon_dev.write(b'2\n')
uvon_dev.write(b'I1\n')
atexit.register(lambda: uvon_dev.write(b'2\n'))
atexit.register(lambda: uvon_dev.write(b'I0\n'))
atexit.register(lidar.stop)
atexit.register(lidar.disconnect)
forward_cmd = b'm1,15,0,15\n'
backward_cmd = b'm0,15,1,15\n'
left_cmd = b'm1,1,0,15\n'
def run_slam(LIDAR_DEVICE, MAP_SIZE_PIXELS, MAP_SIZE_METERS, mapbytes,
posbytes, odometry, command, state, STATES):
MIN_SAMPLES = 200
# Connect to Lidar unit
lidar = start_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,
map_quality=_DEFAULT_MAP_QUALITY,
hole_width_mm=_DEFAULT_HOLE_WIDTH_MM,
random_seed=None,
sigma_xy_mm=_DEFAULT_SIGMA_XY_MM,
sigma_theta_degrees=_DEFAULT_SIGMA_THETA_DEGREES,
max_search_iter=_DEFAULT_MAX_SEARCH_ITER)
# Initialize empty map
mapbytes_ = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Send command to start scan
lidar.start_scan()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
previous_angles = None
pose_change = (0, 0, 0)
pose_change_find = (0, 0, 0)
iter_times = 1
while command[0] != b'qa':
# read a scan
items = lidar.scan()
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if state[0] == STATES['stop']:
if command[0] == b'lm':
mapbytes_find = mapbytes
pose_change_find = find_slam_pos(lidar, MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
mapbytes_find,
pose_change_find,
iter_times / 2.0, slam)
iter_times = iter_times + 1.0
print(pose_change_find)
pose_change = pose_change_find
pose_change_find = (0, 0, 0)
'''
items = lidar.scan()
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
slam.update(distances, pose_change, scan_angles_degrees=angles)
'''
if (iter_times > 10):
command[0] = b'w'
print(slam)
slam.setmap(mapbytes_find)
pose_change = pose_change_find
pose_change_find = (0, 0, 0)
print('ennnnnnnnnnnnnnnnnnd')
print(pose_change)
iter_times = 1
else:
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, pose_change, 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,
pose_change,
scan_angles_degrees=previous_angles)
#pass the value into point_cal function
global point_a, point_b
global current_theta_o
# Get current robot position
x, y, theta = slam.getpos()
current_theta_o = theta
point_a = x - 5000
point_b = y - 5000
# Calculate robot position in the map
xbytes = int(x / 1000. / MAP_SIZE_METERS * MAP_SIZE_PIXELS)
ybytes = int(y / 1000. / MAP_SIZE_METERS * MAP_SIZE_PIXELS)
# Update robot position
posbytes[xbytes + ybytes * MAP_SIZE_PIXELS] = 255
#print('LIDAR::', command[0], x,y,theta)
pose_change = (odometry[0], odometry[1] / np.pi * 180, odometry[2])
# Get current map bytes as grayscale
slam.getmap(mapbytes_)
# Update map
mapbytes[:] = mapbytes_
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
print('LIDAR::thread ends')
import paho.mqtt.client as mqtt
import math
import matplotlib.pyplot as plt
import numpy as np
import socket
import time
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel
from PIL import Image
MAP_SIZE_PIXELS = 500
MAP_SIZE_METERS = 35
MIN_SAMPLES = 200
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=1)
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
Dist = {}
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect(('127.0.0.1', 4013))
def mm2pix(mm):
return int(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS))
def on_connect(self, client, userdata, rc):
print("MQTT Connected.")
self.subscribe("/ldb01/mapdata")
def main():
# Bozo filter for input args
if len(argv) < 3:
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
# Load the data from the file, ignoring 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)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computePoseChange(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()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Pickle the map to a file
pklname = dataset + '.map'
print('Writing map to file ' + pklname)
pickle.dump(mapbytes, open(pklname, 'wb'))
def main():
# Grab input args
dataset = "testLidar"
use_odometry = True
seed = 0
# Load the data from the file, ignoring timestamps
lidars, velocities = load_data('.', dataset)
# Build a robot model if we want odometry
robot = RoboPorter() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(roboPorterLaser(),
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
random_seed=seed) # \
#if seed \
#else Deterministic_SLAM(roboPorterLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
print(len(lidars[scanno]))
# Convert odometry to velocities
velocitity = velocities[
scanno] #dxyMillimeters, dthetaDegrees, dtSeconds
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocitity)
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0
# Save map and trajectory as PGM file
pgm_save('%s.pgm' % dataset, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
class Lidar(BaseLidar):
def __init__(self, on_map_change):
super().__init__(on_map_change=on_map_change)
self.lidar = RPLidar(os.getenv('LIDAR_DEVICE', SLAM["LIDAR_DEVICE"]))
self.slam = RMHC_SLAM(RPLidarA1(), SLAM['MAP_SIZE_PIXELS'],
SLAM['MAP_SIZE_METERS'])
self.map_size_pixels = SLAM['MAP_SIZE_PIXELS']
self.trajectory = []
self.mapbytes = bytearray(SLAM['MAP_SIZE_PIXELS'] *
SLAM['MAP_SIZE_PIXELS'])
self.prev_checksum = 0
def stop(self):
# await super().stop()
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def run(self):
try:
previous_distances = None
previous_angles = None
iterator = self.lidar.iter_scans()
next(iterator)
while True:
items = [(q, 360 - angle, dist)
for q, angle, dist in next(iterator)]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
if len(distances) > SLAM['MIN_SAMPLES']:
self.slam.update(distances, scan_angles_degrees=angles)
previous_distances = distances.copy()
previous_angles = angles.copy()
elif previous_distances is not None:
self.slam.update(previous_distances,
scan_angles_degrees=previous_angles)
x, y, theta = self.slam.getpos()
self.trajectory.append((x, y))
self.slam.getmap(self.mapbytes)
for coords in self.trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
self.mapbytes[y_pix * SLAM['MAP_SIZE_PIXELS'] + x_pix] = 0
checksum = sum(self.mapbytes)
if self.on_map_change and checksum != self.prev_checksum:
# print(checksum)
x = Image.frombuffer(
'L', (self.map_size_pixels, self.map_size_pixels),
self.mapbytes, 'raw', 'L', 0, 1)
bytes_img = io.BytesIO()
x.save(bytes_img, format='PNG')
self.on_map_change(bytearray(bytes_img.getvalue()))
self.prev_checksum = checksum
except Exception as e:
print(e)
finally:
self.stop()
def main():
# Bozo filter for input args
if len(argv) < 3:
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
# Load the data from the file
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)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = 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()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0
# Save map and trajectory as PNG file
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS), mapbytes,
'raw', 'L', 0, 1)
image.save('%s.png' % dataset)
# scan, but on slower computers you'll get an empty map and unchanging position
# at that rate.
MIN_SAMPLES = 200
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel
from rplidar import RPLidar as Lidar
from roboviz import MapVisualizer
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
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
LIDAR_DEVICE = '/dev/ttyACM0'
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.components import XVLidar as LaserModel
from xvlidar import XVLidar 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 an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
slam.update([pair[0] for pair in lidar.getScan()])
from breezyslam.sensors import RPLidarA1 as LaserModel
from rplidar import RPLidar as Lidar
from roboviz import MapVisualizer
from scipy.interpolate import interp1d
import numpy as np
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
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use this to store previous scan in case current scan is inadequate
previous_distances = None
# scan, but on slower computers you'll get an empty map and unchanging position
# at that rate.
MIN_SAMPLES = 200
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel
from rplidar import RPLidar as Lidar
from roboviz import MapVisualizer
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
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, 'SLAM')
# Initialize an empty trajectory
trajectory = []
# Initialize empty map
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Create an iterator to collect scan data from the RPLidar
iterator = lidar.iter_scans()
# We will use these to store previous scan in case current scan is inadequate
previous_distances = None
def main():
# Bozo filter for input args
if len(argv) < 3:
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
# Load the data from the file
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(URG04(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = 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()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' % (nscans, elapsed_sec, nscans/elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
# Save map and trajectory as PGM file
pgm_save('%s.pgm' % dataset, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
def create_slam():
return RMHC_SLAM(SweepLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)