def updateObstacles(obstacleBooleans):
lidar = RPLidar('/dev/ttyUSB0')
time.sleep(0.1)
try:
# for new_scan, quality, angle, distance in lidar.iter_measurments():
# if angle < 0.5 or angle > 359.5:
# print(quality, distance)
# obstacleBooleans.value = distance < 800
for scan in lidar.iter_scans():
distances = [x[2]/10 for x in filter(
lambda x: 170 < x[1] and x[1] < 190, scan)]
N = len(distances)
if N:
mean = sum(distances) / N
print(mean)
obstacleBooleans.value = mean < 80
else:
obstacleBooleans.value = False
except KeyboardInterrupt:
print('Keyboard interrupt, stopping and disconnecting lidar.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def run():
lidar = RPLidar(PORT_NAME)
iterator = lidar.iter_scans(400)
while (1):
update_line(50, iterator)
lidar.stop()
lidar.disconnect()
def runLidar_client(self):
""" Spins the lidar, parses and stores the readings.
If jetson sends a list of bounding boxes that are detected,
the program temporarily stops the lidar from scanning so that
it can access the cache
*** We use a condition_variable to make this main thread sleep
until we find the depth in our |lidar_readings| dictionary
"""
lidar = RPLidar('/dev/ttyUSB0')
startTime = time.time()
stopMessage = [False]
stopReceiever = Thread(target=self.stopLidarThread,
args=(stopMessage, ))
stopReceiever.start()
f = open("scan_reading.txt", "w+")
for i, scan in enumerate(lidar.iter_scans(max_buf_meas=5000)):
f.write("%s\n" % (scan))
while not self._jetsonMsgCv.isSet():
self._jetsonMsgCv.wait()
if stopMessage[0] or i == c.MAX_NUM_ROTATION:
break
# unixTimeStamp made here. Round to the nearest second
currentTime = round(time.time())
self.parseLidarData(scan, currentTime)
lidar.stop()
f.close()
lidar.stop_motor()
lidar.disconnect()
def run():
"""Create a live plot of the data points."""
lidar = RPLidar('/dev/tty.SLAB_USBtoUART')
time.sleep(0.1)
fig, ax = plt.subplots(figsize=(8, 8))
ax.axis('scaled')
ax.axis([-10, 10, -10, 10])
ax.grid(True)
iterator = lidar.iter_scans()
points = ax.scatter([0, 0], [0, 0], s=5, lw=0)
line, = ax.plot([0, 1], [0, 1])
ani = animation.FuncAnimation(fig,
update_line,
fargs=(iterator, points, line),
interval=50)
plt.show()
lidar.stop()
lidar.stop_motor()
lidar._serial_port.reset_input_buffer()
lidar._serial_port.reset_output_buffer()
lidar.disconnect()
def run():
lidar = RPLidar(PORT_NAME)
lidar.reset()
#print statement required to capture value for vba shell
print("reset sent")
time.sleep(.1)
lidar.disconnect()
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 run():
lidar = RPLidar(PORT_NAME)
fig = plt.figure() #
ax = plt.axes(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)
"""
animation.FuncAnimation arguments:
class matplotlib.animation.FuncAnimation(fig, func, frames=None, init_func=None, fargs=None, save_count=None, **kwargs)
fig -> figure object
func -> The function to call at each frame. The first argument will be the next value in frames.
Any additional positional arguments can be supplied via the fargs parameter.
fargs -> Additional arguments to pass to each call to func.
interval -> Delay between frames in milliseconds. Defaults to 200.
"""
plt.show()
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
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')
words = line.split('\t')
word = float(words[2])
data.append(word)
except KeyboardInterrupt:
print('Stoping.')
'''i = 1
for mess in data:
if mess < 50:
print("value number {} is {}".format(i, mess) )
i += 1'''
lidar.stop()
lidar.disconnect()
outfile.close()
np.save(path, np.array(data))
def runLidar():
lidar = RPLidar(PORT_NAME)
iterator = lidar.iter_scans()
animateDataStream(iterator)
lidar.stop()
lidar.disconnect()
def getCoord(self):
'''Main function'''
lidar = RPLidar(self.rpLidar_port)
lidar.stop()
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
lidar.stop()
try:
print('Recording measurments... Press Crl+C to stop.')
for i in range (0,10):
scan = next(lidar.iter_scans())
for measurment in scan:
lidar.stop()
line = '\t'.join(str(v) for v in measurment)
#print(line)
lidar.stop()
time.sleep(0.0625)
#for scan in lidar.iter_scans():
# print(scan)
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
def main():
circle = np.zeros(360)
lidar = RPLidar("/dev/ttyUSB0")
time.sleep(2)
go = 1
#try:
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
iterator = lidar.iter_scans()
obstacle = []
sectors = [[], [], [], [], [], []]
try:
while True:
obstacles, sectors = lidarDistanceAndSector(next(iterator))
if obstacles:
autoSpeed(sectors)
autoSteer(sectors)
print('finished cycle')
except:
print("error occured in main")
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def lidar_logger(timestamp):
global INLOCSYNC, DONE, LIDAR_STATUS
port_name = '/dev/ttyUSB0'
while not INLOCSYNC:
time.sleep(5)
while not DONE:
try:
lidar = RPLidar(port_name)
with open("/root/gps-data/%s_lidar.csv" % timestamp, "w") as f:
f.write("%s\n" % VERSION)
for i, scan in enumerate(lidar.iter_scans(max_buf_meas=1000)):
t = time.time()
lidartime = datetime.datetime.now().strftime(
"%Y-%m-%dT%H:%M:%S.%fZ")
f.write("%s L [" % (lidartime))
for (_, angle, distance) in scan:
f.write("(%0.4f,%0.2f)," % (angle, distance))
f.write("] *\n")
LIDAR_STATUS = True
if DONE:
break
LIDAR_STATUS = False
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
except KeyboardInterrupt:
DONE = True
except Exception as ex:
print("WARNING: %s" % ex)
time.sleep(5)
timestamp = time.strftime("%Y%m%d%H%M", time.gmtime(time.time()))
print("LIDAR Done")
def _measured_distance(self):
"""."""
try:
lidar = RPLidar(self.PORT_NAME)
for measurment in lidar.iter_measurments():
line = '\n'.join(str(v) for v in measurment)
newline = line.split("\n")
if ((float(newline[2]) > 0 and 0.3 > float(newline[2])) or
(float(newline[2]) > 359.7 and 360 > float(newline[2]))):
distance = float(newline[3]) / 10
lidar.stop()
lidar.disconnect()
time.sleep(0.2)
break
self.log.log("_measured_distance() successful",
level=3,
days_to_remain=1)
return (distance)
except:
self.log.log("_measured_distance() exception",
level=3,
days_to_remain=1)
lidar.disconnect()
time.sleep(0.2)
self._measured_distance()
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()
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)
line2 = 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)
ax.set_theta_zero_location('N', offset=0)
ax.set_theta_direction("clockwise")
iterator = lidar.iter_scans()
ani = animation.FuncAnimation(fig,
update_line,
fargs=(iterator, line),
interval=50)
plt.show()
lidar.stop()
lidar.disconnect()
def main(path):
print(__file__ + " start!!")
lidar = Lidar(LIDAR_DEVICE)
time.sleep(3)
try:
print('Recording measurments... Press Crl+C to stop.')
for scan in lidar.iter_scans():
#TO DO - call feature extract
#TO DO - call localizer
#TO DO - publish to network table
print(scan[0][0])
#data from each scan is in tuples: 0 = quality 1 = degrees 2 = distance
except:
print('Stoping.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
return
def tu_lidar(_argv):
lidar = RPLidar('/dev/ttyUSB0')
# check lidar state and health
print(lidar.get_info())
print(lidar.get_health())
for i, scan in enumerate(lidar.iter_scans(max_buf_meas=800, min_len=5)):
print(i, len(scan))
valid_measurements = np.array([])
for measurement in scan:
quality, angle, distance = measurement
if distance != 0:
x = distance * np.cos(convert_degrees_to_rad(angle))
y = distance * np.sin(convert_degrees_to_rad(angle))
cartesian = np.array([x, y])
measurement = np.vstack((measurement, cartesian))
if measurement.shape[0]:
plot_measurement(measurement)
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def run(self, n=320):
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.set_pwm(n)
cnt = 0
try:
print('Recording measurments... Press Crl+C to stop.')
for measurment in lidar.iter_measurments():
line = '\t'.join(str(v) for v in measurment)
list.append(line + '\t' + str(ch.getPosition()) + '\n')
cnt += 1
if lidar.motor:
ln = 'Spinning %d'
else:
ln = 'Stopped %d'
print(ln % cnt)
if cnt > 85000:
break
except KeyboardInterrupt:
print('Stopping.')
except RPLidarException as e:
print(e)
print("complete")
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
outfile.close()
print(lidar.motor)
def run():
lidar = RPLidar(config['lidar']['port'])
pygame.init()
points = [(0, 0) for i in range(361)]
k = 0
try:
print('Initializing')
time.sleep(5)
print('Recording data')
for new, quality, theta, r in lidar.iter_measurments(max_buf_meas=800):
x = (math.cos(math.radians(theta / math.pi) * math.pi) * r) / int(
config['scale'])
y = (math.sin(math.radians(theta / math.pi) * math.pi) * r) / int(
config['scale'])
points[int(theta * 1)] = (x, y)
k += 1
if k > 100:
k = 0
draw(points)
except KeyboardInterrupt:
print('Stopping')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def scan(self):
pointCloud = []
lidar = RPLidar(self.portname)
for i, measurement in enumerate(lidar.iter_measures()):
for j, v in enumerate(measurement):
if j == 0:
newscan = v
if j == 1:
quality = v
if j == 2:
angle = v
if j == 3:
length = v
# change angle to match orientation of vehicle
angle = -1 * (angle - 90)
pointCloud.append([angle, length])
if i > 360:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
return pointCloud
def run(dis):
lidar = RPLidar(PORT_NAME)
try:
c = 0
for scan in lidar.iter_scans():
c = 0
d = 0
for (_, angle, distance) in scan:
if floor(angle) >= 178 and floor(
angle) <= 182 and distance != 0:
#print(floor(distance))
c = c + 1
d = d + floor(distance)
else:
continue
if c != 0:
if d / c <= dis:
return 1
except RPLidarException:
return 0
lidar.stop()
lidar.disconnect()
#run(10)
def main():
lidar = RPLidar(config.LIDAR_PORT_NAME)
time.sleep(5)
measurments_list = []
obstacles = {"left": False, "center": False, "right": False}
for measurment in lidar.iter_measurments(max_buf_meas=config.MAX_BUF_MEAS):
measurments_list.append(measurment)
if len(measurments_list) >= config.NUMBER_MEASURE:
_, frame = cap.read()
frame = cv2.resize(frame, (widthScreen, heightScreen)) #image
obstacles = find_obstacles(measurments_list)
results = trackedObjectDirection(frame, obstacles)
commandsTable[convert(results)][convert(obstacles)]()
measurments_list.clear()
cv2.imshow("camera", frame)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
###release the capture###
#cap.release()
#Close output window
cv2.destroyAllWindows()
#Stop RPLidar
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
#send signal to stop motor to Arduino
### time.sleep() di config.FREQ_COMMANDS in modo da evitare che non sia inviato###
#time.sleep(config.FREQ_COMMANDS)
stops()
def run():
lidar = RPLidar(PORT_NAME)
time.sleep(WAIT)
status, code = lidar.get_health()
time.sleep(WAIT)
lidar.disconnect()
print(status)
def run():
'''Main function'''
max_distance = 0
lidar = RPLidar(PORT_NAME)
try:
print('Recording measurments... Press Crl+C to stop.')
for scan in lidar.iter_scans():
if len(scan) > 100:
lcd.fill((0, 0, 0))
for (_, angle, distance) in scan:
max_distance = max([min([5000, distance]), max_distance])
radians = angle * pi / 180.0
x = distance * cos(radians)
y = distance * sin(radians)
point = (240 + int(x / max_distance * 159),
160 + int(y / max_distance * 159))
lcd.set_at(point, pygame.Color(255, 255, 255))
pygame.display.update()
print(max_distance)
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
outfile.close()
class Lidar(QThread):
# Signal pour envoyer les scans
newScansReceived = pyqtSignal(np.ndarray, np.ndarray, np.ndarray,
np.ndarray)
def __init__(self):
super().__init__()
self.lidar = RPLidar('COM12', baudrate=256000)
#self.lidar = RPLidar('/dev/ttyUSB0', baudrate=256000)
#self.connect()
print('connexion avec le lidar')
self.lidar.logger.setLevel(logging.DEBUG)
consoleHandler = logging.StreamHandler()
#self.lidar.logger.addHandler(consoleHandler)
info = self.lidar.get_info()
print(info)
health = self.lidar.get_health()
print(health)
print("####################")
time.sleep(2)
self.continuer = True
def run(self):
gen = self.lidar.scan2ArrayFromLidar(45, 135, 225, 315)
while self.continuer:
radiusRight, thetaRight, radiusLeft, thetaLeft = next(gen)
self.newScansReceived.emit(radiusRight, thetaRight, radiusLeft,
thetaLeft)
print("fin du thread Lidar")
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def run(path):
'''Main function'''
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
cnt = 0
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')
cnt += 1
if lidar.motor:
ln = 'Spinning %d'
else:
ln = 'Stopped %d'
print(ln % cnt)
if cnt > 360:
break
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
outfile.close()
print(lidar.motor)
def read_lidar_wrapper(angles,previous_readings):
''' Wrapper function for the RPLidar library.
This function takes as inputs:
-> the angles to measure as a list
-> the RPLidar object (instantiated using RPLidar library)
-> the previous_readings dictionary
This function returns:
-> the updated previous_readings dictionary with new measurements
'''
lidar = RPLidar('/dev/ttyUSB0') # Establish connection with Lidar
for i, scan in enumerate(lidar.iter_scans()): # Scan the sensor infinitely
if i>0:
readings = ({int(x[1]): int(x[2]) for x in list(scan) if int(x[1]) in angles})
break # Stop the scan to exit infinite loop
# Sometimes the sensor doesn't read all angles (unfortunately that's how it works)
# so we must add those missing values as the previous sensor readings
# to avoid having a dictionary with missing values
for key in previous_readings.keys():
try:
if readings[key]:
previous_readings[key]=readings[key]
except:
continue
lidar.stop() # Stop the sensor to be able to read it again
lidar.disconnect() # Stop the sensor to be able to read it again
return previous_readings
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 test_rp_lidar():
from rplidar import RPLidar
import serial
import glob
temp_list = glob.glob('/dev/ttyUSB*')
result = []
for a_port in temp_list:
try:
s = serial.Serial(a_port)
s.close()
result.append(a_port)
except serial.SerialException:
pass
print("available ports", result)
lidar = RPLidar(result[0], baudrate=115200)
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print(f'{i}: Got {len(scan)} measurements')
if i > 10:
break
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()
