def LiDarProcess():
lidar = RPLidar(PORT_NAME)
try:
print('Recording measurments... Press Crl+C to stop.')
for scan in lidar.iter_scans():
CONTROL = [1.0, 1.0, 1.0, 1.0]
for point in scan:
ang = degreeFixer(-point[1]+270.0)
pra = distanceFilter(point[2], ang)
if ang < 90 or ang > 270:
if pra[0] < CONTROL[0]:
CONTROL[0] = pra[0]
if ang > 0 and ang < 180:
if pra[1] < CONTROL[1]:
CONTROL[1] = pra[1]
if ang > 90 and ang < 270:
if pra[0] < CONTROL[2]:
CONTROL[2] = pra[0]
if ang > 180 and ang < 360:
if pra[1] < CONTROL[3]:
CONTROL[0] = pra[1]
print("Process result: | X_P - %f | Y_P - %f | X_N - %f | Y_N - %f |" % (CONTROL[0], CONTROL[1], CONTROL[2], CONTROL[3]))
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
def run(dis):
lidar = RPLidar(PORT_NAME)
try:
for scan in lidar.iter_scans():
c = 0
d = 0
for (_, angle, distance) in scan:
if floor(angle) >= 170 and floor(
angle) <= 190 and distance != 0 and distance <= dis:
return 1
elif floor(angle) >= 210 and floor(
angle) <= 225 and distance != 0:
#print(floor(distance))
c = c + 1
d = d + floor(distance)
else:
continue
if c != 0:
Smart2(d / c)
except RPLidarException:
return 0
lidar.stop()
lidar.disconnect()
#Smart2(20)
def stop(port):
lidar = RPLidar(port)
lidar.start_motor()
sleep(0.01)
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def run(path):
'''Main function'''
lidar = RPLidar(PORT_NAME)
data = []
try:
print('Recording measurments... Press Crl+C to stop.')
for scan in lidar.iter_scans():
data.append(np.array(scan))
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
np.save(path, np.array(data))
def run(path):
'''Main function'''
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
try:
print('Recording measurments... Press Crl+C to stop.')
for measurment in lidar.iter_measurments():
line = '\t'.join(str(v) for v in measurment)
outfile.write(line + '\n')
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
outfile.close()
def run():
lidar = RPLidar(PORT_NAME)
fig = plt.figure()
ax = plt.subplot(111, projection='polar')
line = ax.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],
cmap=plt.cm.Greys_r, lw=0)
ax.set_rmax(DMAX)
ax.grid(True)
iterator = lidar.iter_scans()
ani = animation.FuncAnimation(fig, update_line,
fargs=(iterator, line), interval=50)
plt.show()
lidar.stop()
lidar.disconnect()
def run():
'''Main function'''
plt.ion()
lidar = RPLidar(PORT_NAME)
subplot = plt.subplot(111, projection='polar')
plot = subplot.scatter([0, 0], [0, 0], s=5, c=[IMIN, IMAX],
cmap=plt.cm.Greys_r, lw=0)
subplot.set_rmax(DMAX)
subplot.grid(True)
plt.show()
for scan in lidar.iter_scans():
if not plt.get_fignums():
break
update(plot, scan)
lidar.stop()
lidar.disconnect()
def run():
'''Main function'''
lidar = RPLidar(PORT_NAME)
old_t = None
data = []
try:
print('Press Ctrl+C to stop')
for _ in lidar.iter_scans():
now = time.time()
if old_t is None:
old_t = now
continue
delta = now - old_t
print('%.2f Hz, %.2f RPM' % (1/delta, 60/delta))
data.append(delta)
old_t = now
except KeyboardInterrupt:
print('Stoping. Computing mean...')
lidar.stop()
lidar.disconnect()
delta = sum(data)/len(data)
print('Mean: %.2f Hz, %.2f RPM' % (1/delta, 60/delta))
# 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
if not viz.display(x/1000., y/1000., theta, mapbytes):
exit(0)
# Shut down the lidar connection
lidar.stop()
lidar.disconnect()
