def scanner():
scan = []
lidar = RPLidar("COM3")
lidar.clear_input()
iterator = lidar.iter_scans()
for i in range(0,3):
scan += next(iterator)
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
return scan
class Rplidar(Component):
def __init__(self, cfg):
super().__init__(inputs=[], outputs=['lidar', ], threaded=True)
self.port = cfg['lidar']
self.distance = [] # List distance, will be placed in datastorage
self.angles = [] # List angles, will be placed in datastorage
self.lidar = RPLidar(self.port)
self.on = True
self.lidar.clear_input()
def onStart(self):
"""Called right before the main loop begins"""
self.lidar.connect()
self.lidar.start_motor()
def step(self, *args):
return self.distance, self.angles,
def thread_step(self):
"""The component's behavior in its own thread"""
scans = self.lidar.iter_scans()
while self.on:
try:
for distance, angle in scans:
for item in distance: # Just pick either distance or angle since they have the same amount in list
self.distance = [item[2]] # Want to get the 3rd item which gives the distance from scans
self.angles = [item[1]] # Want to get the 2nd item which gives the angle from scans
except serial.SerialException:
print('Common exception when trying to thread and shutdown')
def onShutdown(self):
"""Shutdown"""
self.on = False
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def getName(self):
return 'Rplidar'
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)
class RPLidar():
def __init__(self, port='/dev/ttyUSB0'):
from rplidar import RPLidar
self.port = port
self.frame = np.zeros(shape=365)
self.lidar = RPLidar(self.port)
self.lidar.clear_input()
time.sleep(1)
self.on = True
def update(self):
self.measurements = self.lidar.iter_measurments(500)
for new_scan, quality, angle, distance in self.measurements:
angle = int(angle)
self.frame[angle] = 2 * distance / 3 + self.frame[angle] / 3
if not self.on:
break
def run_threaded(self):
return self.frame
class RPLidar():
def __init__(self, port='/dev/ttyUSB0'):
from rplidar import RPLidar
self.port = port
self.frame = np.zeros(shape=365)
self.lidar = RPLidar(self.port)
self.lidar.clear_input()
time.sleep(1)
self.on = True
# self.lidar.stop()
# self.lidar.disconnect()
def update(self):
self.measurements = self.lidar.iter_measurments(500)
for new_scan, quality, angle, distance in self.measurements:
#angle = int(angle)
distance = int(distance)
#self.frame[angle] = 2*distance/3 + self.frame[angle]/3
#self.frame[10]=self.frame[angle]
#self.frame = self.frame/10
#self.frame[distance]=np.mean(distance)
self.frame = distance
#self.frame=np.mean(self.frame)
if not self.on:
break
def run_threaded(self):
return self.frame
def shutdown(self):
# indicate that the thread should be stopped
self.on = False
time.sleep(.5)
print('stopping Lidar')
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
class RPLidar(object):
'''
https://github.com/SkoltechRobotics/rplidar
'''
def __init__(self, port='/dev/ttyUSB0'):
from rplidar import RPLidar
self.port = port
self.distances = [] #a list of distance measurements
self.angles = [] # a list of angles corresponding to dist meas above
self.lidar = RPLidar(self.port)
self.lidar.clear_input()
time.sleep(1)
self.on = True
#print(self.lidar.get_info())
#print(self.lidar.get_health())
def update(self):
scans = self.lidar.iter_scans(550)
while self.on:
try:
for scan in scans:
self.distances = [item[2] for item in scan]
self.angles = [item[1] for item in scan]
except serial.serialutil.SerialException:
print(
'serial.serialutil.SerialException from Lidar. common when shutting down.'
)
def run_threaded(self):
return self.distances, self.angles
def shutdown(self):
self.on = False
time.sleep(2)
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
class LidarReader(threading.Thread):
def __init__(self, config, mbus, event, lidar_data):
threading.Thread.__init__(self)
self.config = config
self.message_bus = mbus
self.event = event
self.lidar_data = lidar_data
self._running = True
self.lidar_controller = RPLidar(config['lidar_port_name'])
self.lidar_controller.stop()
self.lidar_updates_topic_name = TopicNames.lidar
def terminate(self):
self._running = False
def publish_data(self):
self.message_bus.send(
self.lidar_updates_topic_name,
Message(Message.lidar_data, self.lidar_data.tolist()))
#self.config.log.info('lidar publish to (%s)' % self.lidar_updates_topic_name)
def run(self):
self.config.log.info('The lidar processor is creating topic: %s' %
self.lidar_updates_topic_name)
self.message_bus.create_topic(self.lidar_updates_topic_name)
try:
self.config.log.info("waiting for lidar to start")
time.sleep(5)
self.lidar_controller.clear_input()
info = self.lidar_controller.get_info()
self.config.log.info(
"lidar info: Model:{} Firmware:{} Hardware:{} SN:{}".format(
info['model'], info['firmware'], info['hardware'],
info['serialnumber']))
health = self.lidar_controller.get_health()
self.config.log.info(
"lidar health: Status:{} Error Code:{}".format(
health[0], health[1]))
self.config.log.info("started reading loop...")
# average N measurments per angle
num_scans = 0
data = np.zeros((360, 2), dtype=int)
for measurment in self.lidar_controller.iter_measurments():
if not self._running:
self.lidar_controller.stop_motor()
print("*****************************************")
break
new_scan, quality, angle, distance = measurment
if (distance > 0 and quality > 5):
theta = min(int(np.floor(angle)), 359)
data[theta, 0] += distance
data[theta, 1] += 1
if new_scan: num_scans += 1
if num_scans > 10:
with np.errstate(divide='ignore', invalid='ignore'):
mean_distance = data[:, 0] / data[:, 1]
# interpolate nan's
mask = np.isnan(mean_distance)
mean_distance[mask] = np.interp(np.flatnonzero(mask),
np.flatnonzero(~mask),
mean_distance[~mask])
np.copyto(self.lidar_data, mean_distance)
self.publish_data()
self.event.set()
# reset accumulators
data = np.zeros((360, 2), dtype=int)
num_scans = 0
except (KeyboardInterrupt, SystemExit):
# this is not working... neeed to move inside rplidar code?
self.lidar_controller.stop()
self.lidar_controller.stop_motor()
self.lidar_controller.disconnect()
raise
finally:
self.lidar_controller.stop_motor()
self.lidar_controller.stop()
self.lidar_controller.disconnect()
return
class RPLidar(object):
'''
https://github.com/SkoltechRobotics/rplidar
'''
def __init__(self, lower_limit=0, upper_limit=360, debug=False):
from rplidar import RPLidar
import glob
port_found = False
self.lower_limit = lower_limit
self.upper_limit = upper_limit
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)
port_found = True
except serial.SerialException:
pass
if port_found:
self.port = result[0]
self.distances = [] #a list of distance measurements
self.angles = [
] # a list of angles corresponding to dist meas above
self.lidar = RPLidar(self.port, baudrate=115200)
self.lidar.clear_input()
time.sleep(1)
self.on = True
#print(self.lidar.get_info())
#print(self.lidar.get_health())
else:
print("No Lidar found")
def update(self):
scans = self.lidar.iter_scans(550)
while self.on:
try:
for scan in scans:
self.distances = [item[2] for item in scan]
self.angles = [item[1] for item in scan]
except serial.serialutil.SerialException:
print(
'serial.serialutil.SerialException from Lidar. common when shutting down.'
)
def run_threaded(self):
sorted_distances = []
if (self.angles != []) and (self.distances != []):
angs = np.copy(self.angles)
dists = np.copy(self.distances)
filter_angs = angs[(angs > self.lower_limit)
& (angs < self.upper_limit)]
filter_dist = dists[(angs > self.lower_limit) &
(angs < self.upper_limit
)] #sorts distances based on angle values
angles_ind = np.argsort(
filter_angs) # returns the indexes that sorts filter_angs
if angles_ind != []:
sorted_distances = np.argsort(
filter_dist) # sorts distances based on angle indexes
return sorted_distances
def shutdown(self):
self.on = False
time.sleep(2)
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
class RPLidar(object):
'''
https://github.com/SkoltechRobotics/rplidar
'''
def __init__(self, port='/dev/ttyUSB0'):
from rplidar import RPLidar
self.port = port
self.distances1 = 0
self.angles1 = 0
self.distances2 = 0
self.angles2 = 0
self.distances3 = 0
self.angles3 = 0
self.lidar = RPLidar(self.port)
self.lidar.clear_input()
time.sleep(1)
self.on = True
#print(self.lidar.get_info())
#print(self.lidar.get_health())
def update(self):
scans = self.lidar.iter_scans(550)
while self.on:
try:
d1 = 0
d1_num = 0
a1 = 0
a1_num = 0
d2 = 0
d2_num = 0
a2 = 0
a2_num = 0
d3 = 0
d3_num = 0
a3 = 0
a3_num = 0
for scan in scans:
for index in range(len(scan)):
if scan[index][1] >= 90 and scan[index][1] < 150:
d1 += scan[index][2]
d1_num = d1_num + 1
a1 += scan[index][1]
a1_num = d1_num + 1
elif scan[index][1] >= 150 and scan[index][1] < 210:
d2 += scan[index][2]
d2_num = d1_num + 1
a2 += scan[index][1]
a2_num = d1_num + 1
elif scan[index][1] >= 210 and scan[index][1] < 270:
d3 += scan[index][2]
d3_num = d1_num + 1
a3 += scan[index][1]
a3_num = d1_num + 1
elif (int(scan[index][1])) == 359 and d1_num != 0:
self.distances1 = d1 / d1_num
self.angles1 = a1 / a1_num
self.distances2 = d2 / d2_num
self.angles2 = a2 / a2_num
self.distances3 = d3 / d3_num
self.angles3 = a3 / a3_num
d1 = 0
d1_num = 0
a1 = 0
a1_num = 0
d2 = 0
d2_num = 0
a2 = 0
a2_num = 0
d3 = 0
d3_num = 0
a3 = 0
a3_num = 0
'''
print ("angles: ",self.angles2)
print ("distances: ",self.distances2)
'''
'''
self.distances = [item[2] for item in scan]
self.angles = [item[1] for item in scan]
'''
except serial.serialutil.SerialException:
print(
'serial.serialutil.SerialException from Lidar. common when shutting down.'
)
def run_threaded(self):
return self.distances1, self.angles1, self.distances2, self.angles2, self.distances3, self.angles3
def run(self):
return self.distances1, self.angles1
def shutdown(self):
self.on = False
time.sleep(2)
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
""" import argparse import random import time import sys from rplidar import RPLidar from pythonosc import osc_message_builder from pythonosc import udp_client PORT_NAME = '/dev/ttyUSB0' lidar = RPLidar(PORT_NAME) lidar.stop() lidar.clear_input() #Added this delay avoid Lidar throwing Incorrect descriptor me ssage and failing time.sleep(2) ################################################################# info = lidar.get_info() print(info) health = lidar.get_health() print(health) ANGLE = 2 DISTANCE = 3 #2000 prob ok for exhibition, 500 for garage IGNORE_REDINGS_BEYOND = 4500 PORT_FOR_RECEIVING_MACHINE = 5005 try:
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()
