class BreezySLAM(object):
'''
https://github.com/simondlevy/BreezySLAM
'''
def __init__(self, MAP_SIZE_PIXELS=500, MAP_SIZE_METERS=10):
from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import Laser
laser_model = Laser(scan_size=360,
scan_rate_hz=10.,
detection_angle_degrees=360,
distance_no_detection_mm=12000)
MAP_QUALITY = 5
self.slam = RMHC_SLAM(laser_model, MAP_SIZE_PIXELS, MAP_SIZE_METERS,
MAP_QUALITY)
def run(self, distances, angles, map_bytes):
self.slam.update(distances, scan_angles_degrees=angles)
x, y, theta = self.slam.getpos()
if map_bytes is not None:
self.slam.getmap(map_bytes)
#print('x', x, 'y', y, 'theta', norm_deg(theta))
return x, y, deg2rad(norm_deg(theta))
def shutdown(self):
pass
def 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
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
print(distances)
angles = [item[1] for item in items]
# 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 map and robot pose, exiting gracefully if user closes it
if not viz.display(x / 1000., y / 1000., theta, mapbytes):
exit(0)
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
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():
print "initiating engine..."
seed = 9999
dequeueCount = 0
time_start = time.time()
targetProcess = "node server.js"
targetSignal = signal.SIGUSR1
serverMinTimeDelay = 5
pid = getpid(targetProcess)
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)
trajectory = []
while time.time() - time_start < 600:
time_thisLoop = time.time()
if not q.empty():
while (not q.empty()):
slam.update(q.get())
dequeueCount = dequeueCount + 1
x_mm, y_mm, theta_degrees = slam.getpos()
trajectory.append((x_mm, y_mm))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get map from this batch
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("../../webgui/images/" + "map" + ".png")
print "map created after {} scans".format(dequeueCount)
# Try to tell the server
if pid == False:
print "failure: no server pid"
pid = getpid(targetProcess)
else:
try:
os.kill(int(pid),targetSignal)
print "success: signal sent to server"
except OSError:
print "error: whoops, just lost the pid"
pid = getpid(targetProcess)
# give the server at least serverMinTimeDelay seconds to catch up
while time.time() - time_thisLoop < serverMinTimeDelay:
()
class SLAM:
"""
takes a breezyslam laser object and a flag to determine the
slam algorithms that is used.
"""
def __init__(self, lidar_turret):
self.map_size_pixels = 1600
self.map_size_meters = 50
self.trajectory = []
self.mapbytes = bytearray(self.map_size_pixels * self.map_size_pixels)
self.laser = Laser(lidar_turret.point_cloud_size, 2.4,
lidar_turret.detection_angle_degrees, 0)
self.algorithm = RMHC_SLAM(self.laser, self.map_size_pixels,
self.map_size_meters)
def update(self, timestamp, point_cloud, velocity):
self.algorithm.update(point_cloud, velocity)
x_mm, y_mm, theta_degrees = self.algorithm.getpos()
self.trajectory.append((x_mm, y_mm))
self.algorithm.getmap(self.mapbytes)
def make_image(self, image_name, image_format="pgm"):
self.algorithm.getmap(self.mapbytes)
for coords in self.trajectory:
x_mm, y_mm = coords
x_pix = self.mm2pix(x_mm)
y_pix = self.mm2pix(y_mm)
self.mapbytes[y_pix * self.map_size_pixels + x_pix] = 0
if image_format == "pgm":
pgm_save(image_name + "." + image_format, self.mapbytes,
(self.map_size_pixels, self.map_size_pixels))
else:
image = Image.frombuffer(
'L', (self.map_size_pixels, self.map_size_pixels),
self.mapbytes, 'raw', 'L', 0, 1)
image.save(image_name + "." + image_format)
def get_pos(self):
return self.algorithm.getpos()
def mm2pix(self, mm):
return int(mm / (self.map_size_meters * 1000 / self.map_size_pixels))
def __init__(self):
from breezyslam.algorithms import RMHC_SLAM
lidar = MyLidarModel()
mapbytes = bytearray(800 * 800)
slam = RMHC_SLAM(lidar, 800, 35)
while True:
scan = readLidar()
slam.update(scan)
x, y, theta = slam.getpos(scan)
slam.getmap(mapbytes)
def main():
lidar = Lidar(device='/dev/ttyACM0')
slam = RMHC_SLAM(LaserModel(), map_size_pixels=MAP_SIZE_PIXELS, map_size_meters=MAP_SIZE_METERS)
display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
#image_thread = threading.Thread(target=save_image, args=[display])
#image_thread.start()
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
while True:
slam.update(lidar.getScan())
x, y, theta = slam.getpos()
#print(x, y, theta)
slam.getmap(mapbytes)
display.displayMap(mapbytes)
display.setPose(x, y, theta)
#display.save_image()
display.save_pgm(mapbytes)
if not display.refresh():
exit(0)
def main():
seed = 9999
runCount = 0
dequeueCount = 0
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)
trajectory = []
while dequeueCount < 1000:
time.sleep(10)
if q.empty() == False:
while (q.empty() == False):
slam.update(q.get())
#print "%i" %dequeueCount
dequeueCount = dequeueCount + 1
x_mm, y_mm, theta_degrees = slam.getpos()
trajectory.append((x_mm, y_mm))
# 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('map%i.png' %runCount)
#image.save("/home/card/webgui/images/" + "map" + str(dequeueCount) + ".png")
image.save("/home/card/webgui/images/" + "map" + ".png")
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 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)
# 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()])
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
display.displayMap(mapbytes)
display.displayRobot((x, y, theta))
trajectory.append((x,y))
# Display trajectory
display.displayTrajectory(trajectory)
def slamThread():
global SLAMrot
global SLAMvel
# 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
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
# start time
start_time = time.time()
prevTime = start_time
print("start")
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
slam.update(distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=angles)
prevTime = time.time()
previous_distances = copy.copy(distances)
previous_angles = copy.copy(angles)
print("updated - if")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances, pose_change = ( (SLAMvel, SLAMrot, time.time() - prevTime)),scan_angles_degrees=previous_angles)
prevTime = time.time()
print("updated - else")
print((SLAMvel, SLAMrot, time.time() - prevTime))
# Get current robot position
x, y, theta = slam.getpos()
# Add new position to trajectory
trajectory.append((x, y))
# Get current map bytes as grayscale
slam.getmap(mapbytes)
if(time.time() - start_time > 30):
# 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;
pgm_save('ok.pgm', mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
exit(0)
async def main():
async with websockets.connect(SERVER_URL,
max_size=None,
max_queue=None,
ping_interval=None,
ping_timeout=None) as websocket:
# 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)
# 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
previous_angles = None
# First scan is crap, so ignore it
next(iterator)
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# 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)
while True:
try:
await websocket.send(struct.pack(STRUCT_FORMAT, x, y, theta, mapbytes))
except:
else:
break
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
class LidarGraph(Thread):
def __init__(self, port):
Thread.__init__(self)
self.scan = None
self.running = False
self.port = port
def setup(self):
GPIO.output(23, GPIO.HIGH)
sleep(1)
self.lidar = RPLidar(self.port)
self.lidar.connect()
self.lidar.start_motor()
self.slam = RMHC_SLAM(LaserModel(),
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
map_quality=1,
max_search_iter=50)
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
success = False
while success is not True:
try:
self.iterator = self.lidar.iter_scans()
next(self.iterator) #first scan is shit
success = True
except Exception as e:
print('retrying')
print(e)
sleep(0.5)
def restart(self):
self.stop()
self.go()
def go(self):
self.setup()
self.running = True
def stop(self):
print('Stopping')
self.running = False
self.lidar.stop_motor()
self.lidar.disconnect()
del self.lidar
GPIO.output(23, GPIO.LOW) # turn off lidar relay
sleep(1)
def update(self):
self.scan = next(self.iterator)
self.offsets = np.array([(np.radians(meas[1]), meas[2])
for meas in self.scan])
self.intens = np.array([meas[0] for meas in self.scan])
# BreezySLAM
previous_distances = None
previous_angles = None
items = [item for item in self.scan]
distances = [item[2] for item in items]
angles = [item[1] for item in items]
print(str(len(distances)) + ' distances')
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
print('updating slam')
self.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:
self.slam.update(previous_distances,
scan_angles_degrees=previous_angles)
# Get current robot position
x, y, theta = self.slam.getpos()
print('x ' + str(x) + 'y ' + str(y) + 'theta ' + str(theta))
# Get current map bytes as grayscale
self.slam.getmap(self.mapbytes)
sleep(0.05) #gather more samples?
def data(self):
if self.running != True:
return {'intens': [], 'offsets': []}
return {
'intens': self.intens.tolist(),
'offsets': self.offsets.tolist()
}
def get_map(self):
# slam map
return self.mapbytes
def run(self):
print('run')
iterator = None
while True:
if self.running:
try:
self.update()
print('Updated')
except Exception as e:
print('Exception during update')
print(e)
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
print(len(distances))
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
# First interpolate to get 360 angles from 0 through 359, and corresponding distances
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360))) # slam.update wants a list
# Then update with interpolated distances
slam.update(distances)
# Store interplated distances for next time
previous_distances = distances.copy()
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances)
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Shut down the lidar connection
def main():
# Bozo filter for input args
if len(argv) < 4:
print('Usage: %s <dataset> <output_dir> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 output 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])
output_filename = argv[4]
draw_trajectory = True if int(argv[5]) else False
print("dataset: " + dataset)
print("use_odometry: " + str(use_odometry))
print("seed: " + str(seed))
print("output_filename: " + output_filename)
print("draw_trajectory: " + str(draw_trajectory))
# Load the data from the file, ignoring timestamps
_, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Robot() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(Laser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(Laser(), 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 pose change (dxy_mm, dtheta_degrees, dt_seconds)
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)
if(draw_trajectory):
# 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(output_filename, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
# First scan is crap, so ignore it
next(iterator)
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# 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 map and robot pose, exiting gracefully if user closes it
if not viz.display(x/1000., y/1000., theta, mapbytes):
from Methods import *
from LidarClass import *
from breezyslam.algorithms import RMHC_SLAM
import pandas as pd
from numpy import degrees
import matplotlib.pyplot as plt
Laser = YDlidarx2()
slam2 = RMHC_SLAM(Laser, 1000, 40)
mapbytes = bytearray(1000 * 1000)
class Flora:
pass
Flora.MSP = 1000
P = [(0, 0, 0), (1000, 0, 1)]
for i in range(2):
a = str(i)
ld = pd.read_csv('/home/pi/Flora/FloraMainCode/scans/LD0' + a + '.csv')
tmpd = ld.range * 1000
tmpa = degrees(ld.angle)
slam2.update(tmpd.tolist(), scan_angles_degrees=tmpa.tolist())
slam2.getmap(mapbytes)
Flora.mapbytes = mapbytes
#slam2.position.x_mm = 10000+1000
AA = ByteMapToNP(Flora)
plt.imshow(AA)
plt.show()
# First scan is crap, so ignore it
next(iterator)
while True:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# 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
while True:
# Extrat (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Interpolate to get 360 angles from 0 through 359, and corresponding distances
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360))) # slam.update wants a list
# Update SLAM with current Lidar scan, using third element of (quality, angle, distance) triples
slam.update(distances)
# 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)
# Break on window close
if not display.refresh():
break
class Slam(Node):
"""
SLAM NODE
This node is in charge of receiving data from the LIDAR and creating a map of the environment
while locating the robot in the map.
INPUT
- Bottle detected by the Neuron (to be removed soon)
- Lidar detections
OUTPUT
- Robot position: /robot_pos
- Map as a byte array: /world_map
(this map can be interpred using the map_utils.py package !)
"""
def __init__(self):
super().__init__('slam_node')
# Create second subscription (SLAM)
self.subscription2 = self.create_subscription(
LidarData, 'lidar_data', self.listener_callback_lidar, 100)
self.subscription2 # prevent unused variable warning
# Create third subscription (Motor speeds to compute robot pose change)
self.subscription3 = self.create_subscription(
MotorsSpeed, 'motors_speed', self.listener_callback_motorsspeed,
1000)
self.subscription3 # prevent unused variable warning
# create subscription to control map_quality
self.slam_control_sub = self.create_subscription(
String, 'slam_control', self.slam_control_callback, 1000)
# Create publisher for the position of the robot, and for the map
self.publisher_position = self.create_publisher(
Position, 'robot_pos', 10)
self.publisher_map = self.create_publisher(Map, 'world_map', 10)
# Initialize parameters for slam
laser = LaserModel(detection_margin=DETECTION_MARGIN,
offset_mm=OFFSET_MM)
self.slam = RMHC_SLAM(laser,
MAP_SIZE_PIXELS,
MAP_SIZE_METERS,
map_quality=MAP_QUALITY,
hole_width_mm=OBSTACLE_WIDTH_MM,
x0_mm=X0,
y0_mm=Y0,
theta0=0,
sigma_xy_mm=DEFAULT_SIGMA_XY_MM)
self.trajectory = []
self.mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
self.previous_distances = None
self.previous_angles = None
self.robot_pose_change = np.array([0.0, 0.0, 0.0])
self.is_map_quality_high = True
# DEBUG parameters
self.map_index = 0
self.previous_pos = (0, 0, 0)
self.s = time.time()
print("SLAM is starting")
def slam_control_callback(self, msg):
"""Called to changed the map quality
"""
if msg.data == "save_state":
print("Slam is saving current state")
self.last_valid_map = self.mapbytes.copy()
self.last_valid_pos = self.slam.getpos()
elif msg.data == "recover_state":
# METHOD 1
self.slam.setmap(self.last_valid_map)
## self.slam.setpos(self.last_valid_pos)
elif msg.data == "freeze":
self.last_valid_map = self.mapbytes.copy()
self.last_valid_pos = self.slam.getpos()
elif msg.data == "unfreeze":
self.slam.setmap(self.last_valid_map)
# self.slam.setpos(self.last_valid_pos)
return
##########################
print("changing quality")
if self.is_map_quality_high:
#self.previous_map = self.mapbytes.copy()
self.slam.map_quality = 0
self.slam.sigma_xy_mm = 5 * DEFAULT_SIGMA_XY_MM
else:
#self.slam.setmap(self.previous_map)
self.slam.map_quality = MAP_QUALITY
self.slam.sigma_xy_mm = DEFAULT_SIGMA_XY_MM
self.is_map_quality_high = not self.is_map_quality_high
def listener_callback_motorsspeed(self, msg):
"""
Must compute the robot pose change since the last time the data was collected from the motor
The input are the motors speed and the time change.
Documentation of the SLAM library says :
"pose_change is a tuple (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry"
"""
dx_left = msg.RADIUS * msg.left * (0.1047) * msg.time_delta
dx_right = msg.RADIUS * msg.right * (0.1047) * msg.time_delta
delta_r = 1 / 2 * (dx_left + dx_right) # [mm]
delta_d = -(dx_right - dx_left)
delta_theta = np.arctan(delta_d / msg.LENGTH) * 57.2958 # [deg]
self.robot_pose_change += [delta_r, delta_theta, msg.time_delta]
def listener_callback_lidar(self, msg):
# transform angles to $FORMAT1
angles = np.array(msg.angles)
angles = list(
(angles + 180) % 360) # because LIDAR is facing the robot
distances = list(msg.distances)
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
e = time.time()
self.s = e
self.slam.update(distances,
scan_angles_degrees=angles,
pose_change=tuple(self.robot_pose_change))
# self.analyse_odometry()
self.robot_pose_change = np.array([0.0, 0.0, 0.0])
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
elif self.previous_distances is not None:
# If not adequate, use previous
print("Slam NOT updating")
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
# Get current robot position and current map bytes as grayscale
x, y, theta = self.slam.getpos()
self.slam.getmap(self.mapbytes)
# Send topics
pos = Position()
pos.x = float(x)
pos.y = float(y)
pos.theta = float(theta)
self.publisher_position.publish(pos)
map_message = Map()
map_message.map_data = self.mapbytes
map_message.index = self.map_index
self.publisher_map.publish(map_message)
print("Map sent: ", self.map_index)
self.map_index += 1
# debug functions here (do NOT call them on real runs)
def analyse_odometry(self):
"""Compare the odometry resuls with the change in position according to SLAM.
For this function to work, self.robot_pose_change must be set to something different than zero.
And the variabe self.previous_pos must exist
"""
x, y, theta = self.slam.getpos()
x_old, y_old, theta_old = self.previous_pos
dr = np.sqrt((x - x_old)**2 + (y - y_old)**2)
dtheta = theta - theta_old
print("-------")
print("SLAM change of pos: {:.3f} {:.3f}".format(dr, dtheta))
print("Odometry change of pos: {:.3f} {:.3f} ".format(
self.robot_pose_change[0], self.robot_pose_change[1]))
self.previous_pos = (x, y, theta)
def find_lidar_range(self, distances, angles):
"""Finds the critical angles of the LIDAR and prints them.
Careful, this function works (currently) only with the previous angles format.
"""
(i1, i2) = lidar_utils.get_valid_lidar_range(distances=distances,
angles=angles,
n_points=10)
print('indexes : {} : {} with angles {} - {}'.format(
i1, i2, angles[i1], angles[i2]))
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()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(x/1000., y/1000., theta, mapbytes):
exit(0)
def slam(currentProgram):
trajectory = []
# Connect to Lidar unit
try:
lidar = Lidar(PORT0)
currentProgram.roombaPort = PORT1
iterator = lidar.iter_scans(1000)
lidar.stop()
next(iterator)
print("Lidar 0, Roomba 1")
except:
print("Roomba 0, Lidar 1")
lidar.stop()
lidar.disconnect()
lidar = Lidar(PORT1)
currentProgram.roombaPort = PORT0
iterator = lidar.iter_scans(1000)
lidar.stop()
next(iterator)
# Create an RMHC SLAM object with a laser model and optional robot model
slam = RMHC_SLAM(LaserModel(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
trajectory = []
previous_distances = None
previous_angles = None
# start time
start_time = time.time()
prevTime = start_time
trigger_start = -100
while (not currentProgram.stop):
SLAMvel = currentProgram.SLAMvals[0]
SLAMrot = currentProgram.SLAMvals[1]
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
l = list(zip(angles,distances))
filtered = list(filter(lambda e: (e[0]>=45 and e[0]<=135) and e[1]<300 , l))
# s = sorted(l, key = lambda e: e[0])
if (len(filtered) > constants.POINTS_THRESHOLD) and (time.time()-trigger_start >5):
currentProgram.programStatus = constants.Status.LIDAR_OBSTACLE
topleft = list(filter(lambda e: (e[0]>=105 and e[0]<=135) , filtered))
front = list(filter(lambda e: (e[0]>=75 and e[0]<=105) , filtered))
topright = list(filter(lambda e: (e[0]>=45 and e[0]<=75) , filtered))
if (len(topleft) > 2):
currentProgram.obstacleLocation[0] = 1
if (len(front) > 2):
currentProgram.obstacleLocation[1] = 1
if (len(topright) > 2):
currentProgram.obstacleLocation[2] = 1
trigger_start = time.time()
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
# print("using speeds ", SLAMvel, SLAMrot)
dt = time.time() - prevTime
slam.update(distances, pose_change=(
(SLAMvel*dt, SLAMrot*dt, dt)), scan_angles_degrees=angles)
prevTime = time.time()
previous_distances = copy.copy(distances)
previous_angles = copy.copy(angles)
# print("updated - if")
# If not adequate, use previous
elif previous_distances is not None:
# print("using speeds ", SLAMvel, SLAMrot)
dt = time.time() - prevTime
slam.update(previous_distances, pose_change=(
(SLAMvel*dt, SLAMrot*dt, dt)), scan_angles_degrees=previous_angles)
prevTime = time.time()
# print("updated - else")
# Get current robot position
x, y, theta = slam.getpos()
[x_pix, y_pix] = [mm2pix(x), mm2pix(y)]
currentProgram.robot_pos = [
y_pix // constants.CHUNK_SIZE, x_pix // constants.CHUNK_SIZE]
# print("robot_pos - ",x_pix // constants.CHUNK_SIZE,y_pix // constants.CHUNK_SIZE, theta)
# Get current map bytes as grayscale
slam.getmap(currentProgram.mapbytes)
# Shut down the lidar connection
pgm_save('ok.pgm', currentProgram.mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
lidar.stop()
lidar.disconnect()
for d in mapdata['data'][0]:
ranges = [r * 1000 for r in d[0, 0]['Ranges'].reshape(360)]
angles = [180 * rad / 3.14159 for rad in d[0, 0]['Angles'].reshape(360)]
scans.append([ranges, angles])
for i in np.arange(0, len(scans), 1):
# Extract distances and angles from triples
distances = scans[i][0]
angles = scans[i][1]
# Update SLAM with current Lidar scan and scan angles if adequate
slam.update(distances, scan_angles_degrees=angles)
# Get current robot position
x_mm, y_mm, theta_degrees = slam.getpos()
trajectory.append((x_mm, y_mm))
def mm2pix(mm):
return int(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS))
slam.getmap(mapbytes)
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
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()
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
print(len(distances))
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > MIN_SAMPLES:
# First interpolate to get 360 angles from 0 through 359, and corresponding distances
f = interp1d(angles, distances, fill_value='extrapolate')
distances = list(f(np.arange(360))) # slam.update wants a list
# Then update with interpolated distances
slam.update(distances)
# Store interplated distances for next time
previous_distances = distances.copy()
# If not adequate, use previous
elif previous_distances is not None:
slam.update(previous_distances)
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
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
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 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')
#считаем одометрию для обоих колес
errorCode, x_left = vrep.simxGetJointPosition(clientID, left_motor_handle,
vrep.simx_opmode_streaming)
dx_left = abs(x_left - prev_pos_left)
prev_pos_left = x_left
dx_left = (dx_left + math.pi) % (2 * math.pi) - math.pi
errorCode, x_right = vrep.simxGetJointPosition(clientID,
right_motor_handle,
vrep.simx_opmode_streaming)
dx_right = abs(x_right - prev_pos_right)
prev_pos_right = x_right
dx_right = (dx_right + math.pi) % (2 * math.pi) - math.pi
#обновляем информацию об изменении координат колес, угле между ними и времени
velocities = robot.computePoseChange(time.time(), abs(dx_left),
abs(dx_right))
#обновляем карту
slam.update(scan, velocities)
#находим позицию робота на ней
x, y, theta = slam.getpos()
#получаем карту в виде массива байтов
slam.getmap(mapbytes)
#отображаем карту
if not viz.display(x / 1000., y / 1000., theta, mapbytes):
exit(0)
print('Simulation finished')
exit(0)
while True:
# Receives raw lidar data
raw_data = json.loads(socket1.recv())
raw_data_new = [[i * 1000 for i in raw_data[0:360]]]
for i in range(360, (len(raw_data))):
fixed_data_org = [k * 1000 for k in raw_data[i]]
raw_data_new.append(fixed_data_org)
raw_data_new = list(raw_data_new)[0]
# Sends message back to raw data script
socket1.send_string('Got the raw data: %s ' % raw_data_new)
# Update SLAM with current Lidar scan
slam.update(raw_data_new)
# Get current robot position
map_x, map_y, map_theta = slam.getpos()
#print(x, y)
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Convert byteArray to array containing values 0-255 indicating grayscale colors
mapping = np.reshape(np.frombuffer(mapbytes, dtype=np.uint8),
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
# Convert array containing grayscale colors to 0's for open space and 1's for obstacles
obstacleMap = convertToObstacleMap(mapping)
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 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)
# Connect to Lidar unit
lidar = LidarReader('/dev/ttyACM1', 115200)
# 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)
while True:
# Update SLAM with current Lidar scan, using first element of (scan, quality) pairs
slam.update([pair[0] for pair in lidar.getdata()])
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(x / 1000., y / 1000., theta, mapbytes):
exit(0)
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'))
class Main():
def __init__(self):
self.b = True
self.MIN_SAMPLES = 20
while self.b == True:
try:
self.init_lidar()
self.b = False
except Exception as e:
print(e)
self.init_slam()
def init_lidar(self):
self.lidar = Lidar('/dev/ttyUSB0')
self.iterator = self.lidar.iter_scans(1000)
time.sleep(0.5)
# First scan is crap, so ignore it
next(self.iterator)
def init_slam(self):
self.MAP_SIZE_PIXELS = cfg.MAP_SIZE_PIXELS
self.MAP_SIZE_METERS = cfg.MAP_SIZE_METERS
self.slam = RMHC_SLAM(LaserModel(), self.MAP_SIZE_PIXELS,
self.MAP_SIZE_METERS)
self.mapbytes = bytearray(self.MAP_SIZE_PIXELS * self.MAP_SIZE_PIXELS)
def img_processing(self, mapimg):
t_map = ip.thresholding(mapimg)
d_map = ip.dilation(t_map)
e_map = ip.erosion(d_map)
blur = ip.blur(e_map, cfg.MAP_BLUR, t=0)
return blur
#cv2.imwrite("map2.jpg", blur)
def main_loop(self):
var = 0
time_total = 0
total_matches = 0
max_matches = 0
while var < 50:
# Extract (quality, angle, distance) triples from current scan
items = [item for item in next(self.iterator)]
# Extract distances and angles from triples
distances = [item[2] for item in items]
angles = [item[1] for item in items]
# Update SLAM with current Lidar scan and scan angles if adequate
if len(distances) > self.MIN_SAMPLES:
self.slam.update(distances, scan_angles_degrees=angles)
self.previous_distances = distances.copy()
self.previous_angles = angles.copy()
# If not adequate, use previous
elif self.previous_distances is not None:
self.slam.update(self.previous_distances,
scan_angles_degrees=self.previous_angles)
# Get current robot position
x, y, theta = self.slam.getpos()
# Get current map bytes as grayscale
self.slam.getmap(self.mapbytes)
mapimg = np.reshape(np.frombuffer(self.mapbytes, dtype=np.uint8),
(self.MAP_SIZE_PIXELS, self.MAP_SIZE_PIXELS))
t1 = time.time()
blur = self.img_processing(mapimg)
a, b, matches = ip.find_spot(blur)
t2 = time.time() - t1
if matches > max_matches:
max_matches = matches
time_total += t2
total_matches += matches
var += 1
avg_time = time_total / var
avg_matches = total_matches / var
print("avg_matches: " + str(avg_matches))
print("max_matches: " + str(max_matches))
print("avg_time: " + str(avg_time))
# 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 = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, '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()])
# Get current robot position
x, y, theta = slam.getpos()
# Get current map bytes as grayscale
slam.getmap(mapbytes)
# Display map
display.displayMap(mapbytes)
# Display pose after converting from mm to meters
display.setPose(x / 1000., y / 1000., theta)
# Exit on window close
if not display.refresh():
class Robot:
def __init__(self, messagequeue):
# Connect to Lidar unit
# when initializing the robot object, we first open lidar and start
# reading data from the lidar
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.messages = messagequeue
# All the functions should run in the same process so that the variable can be shared
self.thread = threading.Thread(target=self.navigate, args=())
# the main-thread will exit without checking the sub-thread at the end of the main thread.
# At the same time, all sub-threads with the daemon value of True will end with the main thread, regardless of whether the operation is completed.
self.thread.daemon = True
self.navigating = True
self.thread.start()
# Construct Map should always run in the main process
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())
#print(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
def navigate(self):
while self.navigating:
time.sleep(
0.01) # (yield) allowing reading data from the serailport
if (
self.messages.empty()
): # The camera doesn't detect one traffic sign message.empty()
#if(True): # The camera doesn't detect one traffic sign message.empty()
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(self.lidar.left_container)
#print(front_too_close, left_too_close, right_too_close, self.lidar.angle_180)
else:
sign = self.messages.get() # get the detection id
if (sign == 1):
self.mover.stop()
elif (sign == 2):
self.mover.turn_right()
else:
self.mover.turn_left()
