def run(pwm):
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.set_pwm(int(pwm))
outfile = open(pwm + '.txt', '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 > 10000:
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(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 GetForwardDistance(nMaxWaitForward):
nDistance = -1
global nForwardTolerance
TheLidar = RPLidar('/dev/ttyUSB0')
try:
TheLidar.set_pwm(nLidarSpeed)
tStart = time.perf_counter()
# search or zero bearing (straight ahead)
for measurment in TheLidar.iter_measurments():
# apply quality checks
# if (( measurment[1] > 10) and (measurment[3] < nMaxRange)):
if (measurment[1] >= nMinimumQuality):
#print(measurment[2])
if (measurment[2] > (360 - nForwardTolerance)):
nDistance = measurment[3]
break
else:
if (measurment[2] < (0 + nForwardTolerance)):
nDistance = measurment[3]
break
# if ((measurment[2] > (360 - nForwardTolerance)) or ((measurment[2] < 0 + nForwardTolerance))):
# nDistance = measurment[3]
# #
# if (time.perf_counter()-tStart) < .05 and nForwardTolerance > .5 :
# # take opportunity to narrow the range
# nForwardTolerance = nForwardTolerance -.01
# print("lowering tolerance to:"+str(nForwardTolerance) + " time "+str(time.perf_counter()-tStart))
# tStart = time.perf_counter()
#
#print(time.perf_counter()-tStart)
# widen range if taking too long
if (time.perf_counter() - tStart) > nMaxWaitForward:
nForwardTolerance = nForwardTolerance + 1 # add a degree, wider field of view beyond zero
print("raising tolerance to:" + str(nForwardTolerance) +
" time " + str(time.perf_counter() - tStart))
tStart = time.perf_counter()
#
except:
print("exception GetForwardDistance ")
raise
finally:
TheLidar.stop()
TheLidar.disconnect()
return nDistance
def get_data():
lidar = RPLidar('COM7', baudrate=115200)
lidar.set_pwm(pwm=800)
for scan in lidar.iter_scans(max_buf_meas=500):
print(scan)
print(len(scan))
for thing in scan:
print(thing)
break
lidar.stop()
return scan
def start_lidar(device):
rospy.init_node('lidar')
pub = rospy.Publisher(topics.LIDAR, String, queue_size=1)
lidar = RPLidar(device)
lidar.stop_motor()
time.sleep(1)
lidar.set_pwm(MAX_MOTOR_PWM)
lidar._serial_port.setDTR(False)
lidar.set_pwm(MAX_MOTOR_PWM)
# lidar.set_pwm(MAX_MOTOR_PWM) # set to full speed
while not rospy.is_shutdown():
for i, scan in enumerate(lidar.iter_scans()):
scan = convert_scan_to_vectors(scan)
pub.publish(pickle.dumps(scan))
def Find360Distance(nTimeToSearch):
nDistance = -1
global nForwardTolerance
TheLidar = RPLidar('/dev/ttyUSB0')
try:
TheLidar.set_pwm(nLidarSpeed)
tStart = time.perf_counter()
nLongest = 0
nAngleForLongest = 0
global Distances
global Headings
for measurment in TheLidar.iter_measurments():
# apply quality checks
if ((measurment[1] >= nMinimumQuality)):
if measurment[3] > nLongest:
nLongest = measurment[3]
nAngleForLongest = measurment[2]
#print(nLongest,nAngleForLongest)
if (time.perf_counter() - tStart) > nTimeToSearch:
Distances = [nLongest]
Headings = [nAngleForLongest]
print("New longest 360 distance ", nLongest)
break
except:
print("exception GetForwardDistance ")
raise
finally:
TheLidar.stop()
TheLidar.disconnect()
def run(number = '0'):
'''Main function'''
lidar = RPLidar(PORT_NAME % number)
#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'''
outfile = None
for pwm in range(300, 1001, 25):
lidar.clear_input()
lidar.set_pwm(pwm)
print('pwm: %d' % pwm)
ofn = ('pwm%d.txt' % pwm)
if outfile is not None:
outfile.close()
outfile = open(ofn, 'w')
cnt = 0
for measurment in lidar.iter_measurments():
line = '\t'.join(str(v) for v in measurment)
outfile.write(line + '\n')
cnt += 1
if cnt >= 360:
break
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
outfile.close()
print(lidar.motor)
def run():
lidar = RPLidar(PORT_NAME)
fig = plt.figure()
ax = plt.subplot(111, projection='polar')
lidar.set_pwm(1023)
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 GetDistances():
lidar = RPLidar(PORT_NAME)
lidar.set_pwm(500)
print("move unit now!")
time.sleep(10)
print("measuring new location!")
for measurment in lidar.iter_measurments():
# convert angle and distance to x and y
if measurment[1] == 15 and measurment[3] < nMaxRange and measurment[
3] > nMinRange:
# finding the 2 missing legs given a hypotenuse(distance) and angle in degrees from the lidar
if round(measurment[2], 0) in [359, 0, 1]:
# print("north ", measurment[2], measurment[3] )
nCurNorth = measurment[3]
if round(measurment[2], 0) in [89, 90, 91]:
# print("East ", measurment[2], measurment[3])
nCurEast = measurment[3]
if round(measurment[2], 0) in [179, 180, 181]:
# print("south ", measurment[2], measurment[3])
nCurSouth = measurment[3]
if round(measurment[2], 0) in [269, 270, 271]:
# print("West ", measurment[2], measurment[3])
nCurWest = measurment[3]
if nCurNorth != 0 and nCurEast != 0 and nCurSouth != 0 and nCurWest != 0:
break
print("4 directions NESW ", nCurNorth, nCurEast, nCurSouth, nCurWest)
from rplidar import RPLidar
import matplotlib
from matplotlib import pyplot
import time
import matplotlib.pyplot as plt
lidar = RPLidar('COM7', baudrate=115200)
lidar.stop_motor()
lidar.set_pwm(pwm=1)
print('timing finishing')
lidar.start_motor()
health = lidar.get_health()
print(health)
i = 0
theta = []
r = []
def plotprog():
plt.plot(r, theta)
plt.ylabel('some numbers')
plt.show()
while (i < 100):
for scan in lidar.iter_scans(max_buf_meas=100):
print(i)
for data in scan:
i = i + 1
if i % 10 == 0:
theta.append(data[1] % 360)
# # !/usr/bin/env python3 # '''Records measurments to a given file. Usage example: # $ ./record_measurments.py out.txt''' # #mport sys import time import math import random from rplidar import RPLidar PORT_NAME = '/dev/ttyUSB0' lidar = RPLidar(PORT_NAME) lidar.set_pwm(500) # Import and initialize the pygame library import pygame pygame.init() # Set up the drawing window screen = pygame.display.set_mode([1000, 1000]) font = pygame.font.Font(pygame.font.get_default_font(), 24) # Fill the background with white screen.fill((255, 255, 255)) # Based on LIDAR specs nMinRange = 200 nMaxRange = 6000 # needed for placing things on the grid, what is the origin in the screen variable above, using center of screen nXAdjust = 500
from rplidar import RPLidar
from time import sleep
lidar = RPLidar('/dev/ttyUSB0')
lidar.set_pwm(0)
sleep(1)
info = lidar.get_info()
print(info)
health = lidar.get_health()
print(health)
for i, scan in enumerate(lidar.iter_scans()):
print('%d: Got %d measurments' % (i, len(scan)))
print(scan)
if i > 10:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
class LIDAR_Interface(Thread):
# These are all in Hz
_MAX_SCAN_RATE = 10
_MIN_SCAN_RATE = 0
_MAX_SCAN_PWM = 1023
_MIN_SCAN_PWM = 0
_DEFAULT_SCAN_RATE = 5.5
_GENERATOR_BUFF_SIZE = 500
_ANGULAR_TOLERANCE = 2
def __init__(self, loc: str = "/dev/ttyUSB1", sample_rate: float = 4000, scan_rate: float = 5.5, stack_depth:int = 10):
self.__lidar = RPLidar(port=loc)
super(LIDAR_Interface, self).__init__()
self.__min_parsable = 5
self.__sample_rate = sample_rate
self.__scan_rate = scan_rate
self.__samples_per_rev = LIDAR_Interface._GENERATOR_BUFF_SIZE # this may change after testing
self.__iter_scan = self.__lidar.iter_scans(self.__samples_per_rev)
self.__stack = Stack(stack_depth)
self.__current_scan = []
self.__prev_scan = []
self._max_distance = 5000
self._min_distance = 0
self.__running = False
atexit.register(self.exit_func)
# control functions
def stop_thread(self):
self.__running = False
def exit_func(self):
self.stop_thread()
self.stop_motor()
self.stop_sensor()
self.__lidar.disconnect()
def stop_sensor(self):
self.__lidar.stop()
def stop_motor(self):
self.__lidar.stop_motor()
def reset_sensor(self):
self.__lidar.reset()
self.__lidar.clear_input()
def start_motor(self):
self.__lidar.start_motor()
@property
def running(self):
return self.__running
# properties
@property
def max_distance(self):
return self._max_distance
@max_distance.setter
def max_distance(self, distance: int):
if distance > self._min_distance:
if distance < 5000:
self._max_distance = distance
@property
def min_distance(self):
return self._min_distance
@min_distance.setter
def min_distance(self, distance: int):
if distance >= 0:
if distance < self._max_distance:
self._min_distance = distance
@property
def sensor_health(self):
return self.__lidar.get_health()
@property
def sensor_info(self):
return self.__lidar.get_info()
@property
def sample_rate(self):
return self.__sample_rate
@property
def scan_rate(self):
return self.__scan_rate
@scan_rate.setter
def scan_rate(self, value: float):
if LIDAR_Interface._MAX_SCAN_RATE >= value >= LIDAR_Interface._MIN_SCAN_RATE:
self.__scan_rate = value
self.__sample_rate = LIDAR_Interface._GENERATOR_BUFF_SIZE * self.__scan_rate
self._set_scan_rate()
@property
def pop_recent_scan(self) -> list:
if self.__stack.length > 0:
return self.__stack.pop()
return list()
@property
def recent_scan(self) -> list:
if self.__stack.length > 0:
return self.__stack.top
return list()
@property
def stack(self) -> Stack:
return self.__stack
# conversion function
@staticmethod
def _map(x: float, from_min: float, from_max: float, to_min: float, to_max: float) -> float:
return (x - from_min) * (to_max - to_min) / ((from_max - from_min) + to_min)
# Motor control functions
def _set_scan_rate(self):
self.__lidar.set_pwm(self._map(self.__scan_rate, LIDAR_Interface._MIN_SCAN_RATE, LIDAR_Interface._MAX_SCAN_RATE,
LIDAR_Interface._MIN_SCAN_PWM, LIDAR_Interface._MAX_SCAN_PWM))
# thread function
def start(self) -> None:
super(LIDAR_Interface, self).start()
self.__running = True
def run(self) -> None:
while self.__running:
# iter must produce a full rotation (360 points) before we can use it
#_scan = self.__lidar.iter_scans(min_len=self.__samples_per_rev)
if self.__iter_scan.__sizeof__() > 0:
_scan = next(self.__iter_scan)
for scan in _scan:
current = Ray(scan[2], scan[1], scan[0])
if current.radius > self._min_distance:
if current.radius < self._max_distance:
self.__current_scan.append(current)
# if the current scan has the total points for a rotation we can consume it and reset the current scan
if self.__current_scan.__len__() >= 360:
self.__stack.push(self.__current_scan.copy())
self.__current_scan.clear()
self.__lidar.stop()
self.__lidar.stop_motor()
self.__lidar.disconnect()
