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()