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(): '''Main function''' lidar = RPLidar(PORT_NAME) lidar.start_motor() time.sleep(1) info = lidar.get_info() print(info) try: print('Recording measurments... Press Crl+C to stop.') try: for measurment in lidar.iter_measurments(): # line = '\t'.join(str(v) for v in measurment) # print(line + '\n') if (measurment[2] > 0 and measurment[2] < 90): if (measurment[3] < 1000 and measurment[3] > 100): print("Angle: ", measurment[2] - angleoffset) print("Distance: ", measurment[3]) steer(measurment[2] - angleoffset) except KeyboardInterrupt: print('Stopping.') except KeyboardInterrupt: print('Stopping.') lidar.stop() lidar.stop_motor() c.send("motors", 0, 0, 0) # turn off wheel motors lidar.disconnect()
class Scanner(threading.Thread): def __init__(self, port): threading.Thread.__init__(self) self.lidar = RPLidar(port) self.lidar.stop_motor() sleep(0.5) self.lidar.start_motor() self.setDaemon(True) self.scan = [(0.0, 0.0) for i in range(360)] def check_device(self): info = self.lidar.get_info() print(info) health = self.lidar.get_health() print(health) return (info, health) def get_scan(self): return self.scan def run(self): self.lidar.clear_input() while True: for meas in self.lidar.iter_measurments(): new_scan = meas[0] quality = meas[1] angle = meas[2] distance = meas[3] if angle < 360: self.scan[int(angle)] = (angle, distance / 1000.0)
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 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 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 main(): lidar = RPLidar(PORT) # check RPLidar info = lidar.get_info() print(info) health = lidar.get_health() print(health) # setup value points = np.array([[], []]) previous = np.array([[], []]) rotate = IDENTITY slide = np.array([0, 0]) grid_x = np.linspace(MIN_RANGE, MAX_RANGE, GRID_POINTS) grid_y = np.linspace(MIN_RANGE, MAX_RANGE, GRID_POINTS) grid = cal_positive_grid(points, grid_x, grid_y) fig = plt.figure() # loop while mapping for i, scan in enumerate(lidar.iter_scans(min_len=60)): raw = filter(lambda element: element[1] >= 270 or element[1] <= 90, scan) points = transform_raw(raw) pre, cur = random_filter(previous, points) if len(pre) > 0 and len(cur) > 0 and i % 5 == 0: is_change = False rotation, transform = icp(pre.T, cur.T) rotation_to_degrees(rotation) if rotation_to_degrees(rotation) >= 1: rotate = np.dot(rotation, rotate) is_change = True if math.sqrt(transform[0]**2 + transform[1]**2) >= 5: slide = slide + np.dot(rotate, transform) is_change = True if is_change: previous = points grid = update_grid(grid, points.T, GRID_LENGTH, grid_x, grid_y, origin=slide, rotation=rotate) # make occupancy grid # illustrated if i % 10 == 0: occupancy_grid = cal_occupancy_grid(grid) plt.clf() plt.pcolormesh(grid_x, grid_y, occupancy_grid) plt.plot(slide[1], slide[0], 'ro') tracker = np.dot(rotate, TRACKER) plt.plot(slide[1] + tracker[1], slide[0] + tracker[0], 'bo') plt.pause(0.01) if i == 2: previous = points # stop RPlidar lidar.stop() lidar.stop_motor() lidar.disconnect() # file save save_override_map(occupancy_grid)
def connect(addr): lidar = RPLidar(addr) info = lidar.get_info() #print(info) health = lidar.get_health() #print(health) return lidar
def connect(): lidar = RPLidar('/dev/ttyUSB0') info = lidar.get_info() print(info) health = lidar.get_health() print(health) return lidar
class RpLidar: # Constructor def __init__(self): # Initialize Lidar Lidar self.lidar = RPLidar('com3') info = self.lidar.get_info() print(info) health = self.lidar.get_health() print(health) def runLidar(self): self.iterator = self.lidar.iter_scans(max_buf_meas=10000) def stopLidar(self): self.lidar.stop() self.lidar.disconnect() def GetObstacles(self): scan = next(self.iterator) obstacles = np.array([[80, 70]]) for y in scan: # STEP 1: extract polar coordonates from scan pPolar = [math.radians(y[1]), y[2]] # [angle in radian, distance] # STEP 2: compute cartesian coordonates angle = pPolar[0] passageMatrix = np.array([[np.cos(angle), np.sin(angle)], [(np.sin(angle)) * -1, np.cos(angle)]]) distanceMatrix = np.array([[pPolar[1]], [0]]) pCart = np.dot(passageMatrix, distanceMatrix) # STEP 4: save the obstacles inside an array obstacles = np.append(obstacles, [[pCart[0, 0], pCart[1, 0]]], axis=0) return obstacles def ScanEnvironmemnt(self): cmd = b'\x20' # SCAN_BYTE self.lidar._send_cmd(cmd) dsize, is_single, dtype = self.lidar._read_descriptor() while True: raw = self.lidar._read_response(dsize) print(raw) time.sleep(5)
def main(): flag = False lidar = RPLidar('/dev/ttyUSB0') info = lidar.get_info() health = lidar.get_health() print(info, '\n', health) cap = cv2.VideoCapture(2) if flag: roi, mapx, mapy = Calibration(cap) else: fs = cv2.FileStorage("callibration.xml", cv2.FILE_STORAGE_READ) if not (fs.isOpened()): return 0 roi = fs.getNode('roi').mat() mapx = fs.getNode('mapx').mat() mapy = fs.getNode('mapy').mat() fs.release() x, y, w, h = roi hog = cv2.HOGDescriptor() #svm = ml.SVM_create() #ml.SVM_load('/HOGlegs70x134.xml') #hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector()) hog.load('/home/vladimir/PycharmProjects/PickToGo/HOGlegs.xml') #hog.load('/HOGlegs70x134.xml') while True: #RunLidar(lidar) ret, frame = cap.read() gframe = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) gframe = cv2.remap(gframe, mapx, mapy, cv2.INTER_LINEAR) gframe = gframe[int(y[0]):int(y[0] + h[0]), int(x[0]):int(x[0] + w[0])] orig, frame = DetectUsingHOGdetector(hog, gframe) # upper_bodies = DetectUsingHaar(gframe) # for (x1, y1, w1, h1) in bodies: # cv2.rectangle(frame, (x1, y1), (x1+w, y1+h), (0, 255, 0), 2) # for (x1, y1, w1, h1) in upper_bodies: # cv2.rectangle(frame, (x1, y1), (x1+w, y1+h), (255, 0, 0), 2) # for (x1, y1, w1, h1) in lower_bodies: # cv2.rectangle(frame, (x1, y1), (x1 + w, y1 + h), (0, 0, 255), 2) cv2.imshow('Detection', frame) cv2.imshow('Hog', orig) if cv2.waitKey(1) & 0xFF == ord('q'): lidar.stop() lidar.stop_motor() lidar.disconnect() break
def lidar_logger(output_directory): """ LIDAR Logger """ global LIDAR_STATUS, LIDAR_DATA port_name = '/dev/lidar' lidar = None # Configure config = util.read_config() while not util.DONE: try: lidar = RPLidar(port_name) print(lidar.get_info()) print(lidar.get_health()) # Open the output file timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M") with open(os.path.join(output_directory,timestamp+"_lidar.csv"), "w") as lidar_output: lidar_output.write("%s %s %s *\n" % (config['time'], "VERSION", json.dumps({"class": "VERSION", "version": util.DATA_API}))) lidar_output.write("%s %s %s *\n" % (config['time'], config['class'], json.dumps(config))) for _, scan in enumerate(lidar.iter_scans(max_buf_meas=1500)): lidartime = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%fZ") data = [] for (_, angle, distance) in scan: if distance > 0: data.append((int(angle)%360, int(distance))) lidar_data = { 'class': 'LIDAR', 'device': 'A1M8', 'time': lidartime, 'scan': data, } lidar_output.write("%s %s %s *\n" % (lidar_data['time'], lidar_data['class'], json.dumps(lidar_data))) LIDAR_DATA = lidar_data LIDAR_STATUS = True if util.DONE: break except KeyboardInterrupt: util.DONE = True except Exception as ex: print("LIDAR Logger Exception: %s" % ex) time.sleep(util.ERROR_DELAY) if lidar is not None: lidar.stop() lidar.stop_motor() lidar.disconnect() LIDAR_STATUS = False time.sleep(util.ERROR_DELAY)
def run(): '''Main function''' lidar = RPLidar(PORT_NAME) lidar.start_motor() time.sleep(1) info = lidar.get_info() print(info) try: scan(lidar) except KeyboardInterrupt: stop = True print('Stopping.') lidar.stop() lidar.stop_motor() lidar.disconnect()
def run(): lidar = RPLidar(PORT_NAME) lidar.stop() info = lidar.get_info() print(info) health = lidar.get_health() print(health) print(lidar) lidar.stop() lidar.stop_motor() lidar.disconnect() print(dir(lidar))
def health_check(): lidar = RPLidar(PORT_NAME) 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))) if i > 10: break lidar.stop() lidar.stop_motor() lidar.disconnect() return
def run(): '''Main function''' global stop if stop == False: lidar = RPLidar(PORT_NAME) lidar.start_motor() time.sleep(1) info = lidar.get_info() print(info) ioloop = asyncio.get_event_loop() tasks = [ioloop.create_task(scan(lidar)), ioloop.create_task(drive())] try: ioloop.run_until_complete(asyncio.wait(tasks)) except KeyboardInterrupt: print('Stopping.') stop = True lidar.stop() lidar.stop_motor() c.send("motors", 0, 0, 0) # turn off wheel motors lidar.disconnect() ioloop.close()
class Lidar(Thread): def __init__(self, port, baud, data, limit=500): Thread.__init__(self) self.lidar = RPLidar(port=port, baudrate=baud) try: print(self.lidar.get_info()) except Exception as e: print(e) self.lidar.eteindre() self.lidar.reset() self.lidar.connect() self.lidar.start_motor() self.data = data self.limit = limit def run(self): sleep(1) try: for measurment in self.lidar.iter_measurments( max_buf_meas=self.limit): s = len(self.data[0]) if s >= self.limit: self.data[0].pop(0) self.data[1].pop(0) self.data[0].append(measurment[2] * pi / 180) #agle en rad self.data[1].append(measurment[3] / 1000) #distance en m except Exception as error: print(error) def join(self, timeout=None): self.eteindre() Thread.join(self, timeout) def eteindre(self): self.lidar.stop() self.lidar.stop_motor() self.lidar.disconnect()
def run(): '''Main function''' lidar = RPLidar(PORT_NAME) lidar.start_motor() time.sleep(1) info = lidar.get_info() print(info) drive(0) # start going forward lasttime = int(time.time() * 1000) try: counter = 0 print('Recording measurements... Press Crl+C to stop.') data = 0 try: for measurment in lidar.iter_measurments(): if ((int(time.time() * 1000) - lasttime) < 1000): raise ValueError # do this loop at 10Hz if (measurment[2] > 0 and measurment[2] < 90): # in angular range if (measurment[3] < 1000 and measurment[3] > 100): # in distance range data = data + measurment[2] # angle counter = counter + 1 # increment counter except ValueError: print("this should happen ten times a second") lasttime = int(time.time() * 1000) # reset 10Hz timer if counter > 0: # this means we see something average_angle = data / counter print("Average Angle: ", average_angle - angleoffset) drive(average_angle - angleoffset) except KeyboardInterrupt: print('Stopping.') lidar.stop() lidar.stop_motor() c.send("motors", 0, 0, 0) # turn off wheel motors lidar.disconnect()
class Lidar: def __init__(self, port): self.port = port self.lidar = RPLidar('/dev/tty.SLAB_USBtoUART') info = self.lidar.get_info() print(info) health = self.lidar.get_health() print(health) def start(self): for i, scan in enumerate(self.lidar.iter_scans()): print('%d: Got %d measurments' % (i, len(scan))) if i > 100: break self.stop() def stop(self): self.lidar.stop() #self.lidar.stop_motor() #self.lidar.disconnect()
import socket import time import cv2 from imutils.video import VideoStream import imagezmq from rplidar import RPLidar print('Connecting to webcam...') stream = cv2.VideoCapture(4) #2 print("Connecting to lidar...") lidar = RPLidar('/dev/ttyUSB0') print("") print("-------------------LiDAR-------------------") print(lidar.get_info()) print("-------------------------------------------") print("") print('Connecting to headset - streaming using ZMQ...') min_angle = 39 max_angle = 360 - min_angle # The IP address below is the IP address of the headset computer. sender = imagezmq.ImageSender( connect_to='tcp://localhost:5556') #tcp://192.168.0.101:5556 time.sleep(2.0) jpeg_quality = 35 #0 to 100 resolution_width = 640
class RPLiDAR: def __init__(self, sectors): # self.lidar = RPLidar("\\\\.\\com4") # Check to see if this runs on mac self.lidar = RPLidar("/dev/ttyUSB0") # laptop. If not make change. May be the /dev/ thing self.sectors = sectors self.sector_space = {} self.prev_sector_space = {} time.sleep(1) info = self.lidar.get_info() health = self.lidar.get_health() print(info, health, sep='\n') self.file = open('lidar-data.csv', 'w') self.writer = csv.writer(self.file) def scan_area(self, limit=100): for i, scan in enumerate(self.lidar.iter_scans()): if i > limit: break print('%d: Got %d measurements' % (i, len(scan))) sector_space = np.zeros(6) warning_flag = False for j in scan: k = math.floor(j[1] / 60) if j[2] < 1000: # print("Warning: object at %f degrees is too close" # % (j[1])) sector_space[k] += -math.inf # Set to say space is unsafe warning_flag = True else: sector_space[k] += j[2] # adding distance to sector array if warning_flag: print(sector_space) # outputs six values print("Object(s) are too close...") free_space = max(sector_space) if free_space < 1000: print("There is nowhere safe to venture, immediate landing" " to avoid damage...") break evacuation_direction = \ (sector_space.index(free_space)+1) * 60 - 30 print("Evacuate in the direction of %d degrees" % evacuation_direction) # This is the safest spot def area_report(self, limit=100): for i, scans in enumerate(self.lidar.iter_scans()): if i > limit: # self.stop() break self.sector_space = {} divisor = 360 // self.sectors for scan in scans: section = math.floor(scan[1] / divisor) try: self.sector_space[section] = np.append( self.sector_space[section], scan[2]) except KeyError: self.sector_space[section] = np.array(scan[2]) evaluation_space = self._evaluate_spcae() # print('evaluate space at ', time.time(), evaluation_space, # file=self.file) # self.file.write('evaluate space \n' + str(evaluation_space)) direction = self._get_direction(evaluation_space) if direction == -1: print('There are no safe regions!') print('Go to region %d' % direction) self._write_file(evaluation_space, direction) def _evaluate_spcae(self): evaluation = [] for i in range(self.sectors): try: section = self.sector_space[i] evaluation.append((i, np.min(section), np.max(section), np.average(section), section)) except KeyError: evaluation.append((None, i, None, None, None)) return evaluation def _get_direction(self, evaluation_space): current_section = -1 previous_min = 1 for value in evaluation_space: # print(value) section, min, max, average, sector = value try: if min > previous_min: current_section = section previous_min = min except TypeError: pass return current_section def _write_file(self, evaluation_space, direction): self.writer.writerow('sector number: %d' % direction) for values in evaluation_space: self.writer.writerow((datetime.datetime.time( datetime.datetime.now()).strftime('%H:%M:%S'), values)) def stop(self): self.lidar.stop() self.lidar.stop_motor() self.lidar.disconnect() self.file.close()
import pybullet as p import time import math import time from rplidar import RPLidar PORT_NAME = '/dev/ttyUSB0' lidar = RPLidar(PORT_NAME) time.sleep(1) print("lidar info:", lidar.get_info()) numRays = 5000 iterator = lidar.iter_scans() useGui = True if (useGui): p.connect(p.GUI) else: p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) #p.loadURDF("samurai.urdf") #p.loadURDF("r2d2.urdf",[3,3,1]) rayFrom = [] rayTo = [] rayIds = [] rayLen = 0.1
def connect(self): lidar = RPLidar(self.addr) # demarre le lidar info = lidar.get_info() print(info) health = lidar.get_health() print(health)
from rplidar import RPLidar lidar = RPLidar('/dev/ttyUSB0') 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))) if i > 10: break lidar.stop() lidar.stop_motor() lidar.disconnect()
class RPLidarMQTTBridge(object): health_state = 0 def __init__(self, device=RPLIDAR_DEVICE, topic_prefix=MQTT_TOPIC_PREFIX, host=MQTT_BROKER_HOST, port=MQTT_BROKER_PORT, reset=False): self.mqtt = mqtt.Client(client_id="rplidar_mqtt_bridge", clean_session=True) self.mqtt.on_connect = on_mqtt_connect self.mqtt.connect(host=host, port=port) self.lidar = RPLidar(device) # Generate a unique topic identifier by using the MD5 hash of the device serial number info = self.lidar.get_info() serial = info['serialnumber'] serial_hash = hashlib.md5(serial.encode()).hexdigest() self.topic_prefix = topic_prefix + '/' + serial_hash if reset is True: self.clear_rplidar_readings() self.report_rplidar_source() self.report_rplidar_info(info) def generate_topic(self, subtopic): return self.topic_prefix + subtopic def clear_rplidar_readings(self): self.mqtt.publish(self.generate_topic('/measurement'), None, 1, True) def clear_rplidar_health(self): self.mqtt.publish(self.generate_topic('/health'), None, 1, True) def report_rplidar_measurement(self, measurement): # Only publish valid measurements if self.measurement_valid(measurement): reading = dict(quality=measurement[1], angle=measurement[2], distance=measurement[3], timestamp=iso8601_timestamp()) if measurement[3] == 0: print("logging invalid distance") self.publish_event(reading=reading) def report_rplidar_info(self, info): self.publish_event_raw(topic='/info/model', reading=info['model']) self.publish_event_raw(topic='/info/hardware', reading=info['hardware']) self.publish_event_raw( topic='/info/firmware', reading=("%d.%d" % (info['firmware'][0], info['firmware'][1]))) self.publish_event_raw(topic='/info/serialnumber', reading=info['serialnumber']) def report_rplidar_source(self): self.publish_event_raw(topic='/source', reading="rplidar_mqtt_bridge") def report_rplidar_health(self, health): self.publish_event(reading=health, topic='/health') @staticmethod def measurement_valid(measurement): if len(measurement) != 4: return False # In the case of an invalid measurement, the distance and quality are both set to 0 if measurement[1] == 0: return False if measurement[3] == 0.0: return False return True def publish_event(self, reading, topic='/measurement', raw=False): logging.debug(reading) if raw is True: data = reading else: data = json.dumps(reading, sort_keys=False) self.mqtt.publish(self.generate_topic(topic), data) def publish_event_raw(self, reading, topic='/measurement'): self.publish_event(reading, topic, raw=True) def poll(self): self.mqtt.loop_start() try: health = self.lidar.get_health()[0] if self.health_state != health: self.report_rplidar_health(health) for measurement in self.lidar.iter_measurments(): self.report_rplidar_measurement(measurement) except KeyboardInterrupt: pass finally: self.cleanup() def cleanup(self): self.lidar.stop() self.lidar.disconnect() self.clear_rplidar_health() self.mqtt.disconnect() self.mqtt.loop_stop()
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
def lidar_logger(output_directory): global DONE, LIDAR_STATUS, LIDAR_DATA port_name = '/dev/lidar' lidar = None # Configure try: with open("config.json", "r") as f: config = json.loads(f.read()) except: config = {"class": "CONFIG", "lidar": {"log": True}} config['time'] = datetime.datetime.now().strftime("%Y-%m-%dT%H:%M:%S.%fZ") # Create the output directory if not os.path.isdir(output_directory): os.mkdir(output_directory) while not INLOCSYNC: time.sleep(SYNC_DELAY) while not DONE: try: lidar = RPLidar(port_name) mylog(lidar.get_info()) mylog(lidar.get_health()) # Open the output file timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M") with open(os.path.join(output_directory, timestamp + "_lidar.csv"), "w") as lidar_output: lidar_output.write("#v%d\n" % VERSION) lidar_output.write( "%s %s %s *\n" % (config['time'], config['class'], json.dumps(config))) for i, scan in enumerate(lidar.iter_scans(max_buf_meas=1500)): if INLOCSYNC or ALWAYS_LOG: lidartime = datetime.datetime.now().strftime( "%Y-%m-%dT%H:%M:%S.%fZ") data = [] for (_, angle, distance) in scan: if distance > 0: data.append((int(angle) % 360, int(distance))) lidar_data = { 'class': 'LIDAR', 'device': 'A1M8', 'time': lidartime, 'scan': data, } lidar_output.write( "%s %s %s *\n" % (lidar_data['time'], lidar_data['class'], json.dumps(lidar_data))) LIDAR_DATA = lidar_data LIDAR_STATUS = True if DONE: break except KeyboardInterrupt: DONE = True except Exception as ex: mylog("LIDAR Logger Exception: %s" % ex) time.sleep(ERROR_DELAY) if lidar is not None: lidar.stop() lidar.stop_motor() lidar.disconnect() LIDAR_STATUS = False time.sleep(ERROR_DELAY)
from rplidar import RPLidar lidar = RPLidar('/dev/ttyUSB0') info = lidar.get_info() print(info) health = lidar.get_info() print(health) for i, scan in enumerate(lidar.iter_scans()): print('%d: Got %d measurments' % (i,len(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()