Beispiel #1
0
def run():
    lidar = RPLidar(PORT_NAME)
    time.sleep(WAIT)
    status, code = lidar.get_health()
    time.sleep(WAIT)
    lidar.disconnect()
    print(status)
Beispiel #2
0
 def __init__(self, wheel_radius, wheel_dist, ARDUINO_HCR, LIDAR_DEVICE):
     # Connect to Arduino unit
     self.arduino = Serial(ARDUINO_HCR, 57600)
     # Connect to Lidar unit
     self.lidar = RPLidar(LIDAR_DEVICE)
     # Create an iterator to collect scan data from the RPLidar
     time.sleep(1)
     self.iterator = self.lidar.iter_scans()
     # First scan is crap, so ignore it
     #next(self.iterator)
     self.WHEELS_DIST = wheel_dist
     self.WHEELS_RAD = wheel_radius
     self.x = 0
     self.y = 0
     self.yaw = 0
     self.linear_velocity = 0
     self.angular_velocity = 0
     self.omegaRight = 0
     self.omegaLeft = 0
     self.vr = 0
     self.vl = 0
     self.scan = []
     self.prev_time = time.time()
     self.current_time = time.time()
     self.dt = 0
Beispiel #3
0
def stop(port):
    lidar = RPLidar(port)
    lidar.start_motor()
    sleep(0.01)
    lidar.stop()
    lidar.stop_motor()
    lidar.disconnect()
Beispiel #4
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 connect():
    global lidar
    PORT_NAME = '/dev/ttyUSB0'
    lidar = RPLidar(PORT_NAME)
    lidar.clear_input()
    health = lidar.get_health()
    print(health)
Beispiel #6
0
 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")
Beispiel #7
0
 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
Beispiel #8
0
 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()
Beispiel #9
0
    def __init__(self):

        # Initialize Lidar Lidar
        self.lidar = RPLidar('com3')
        info = self.lidar.get_info()
        print(info)
        health = self.lidar.get_health()
        print(health)
Beispiel #10
0
 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
Beispiel #11
0
def connect(addr):
    lidar = RPLidar(addr)

    info = lidar.get_info()
    #print(info)

    health = lidar.get_health()
    #print(health)
    return lidar
Beispiel #12
0
 def lidar_stop(self):
     if self.process.poll() is None:
         self.process.kill()
     while self.process.poll() is None:
         pass
     self.lidar = RPLidar(self.lidar_port)
     self.lidar.connect()
     self.lidar.stop_motor()
     self.lidar.stop()
Beispiel #13
0
def connect():
    lidar = RPLidar('/dev/ttyUSB0')

    info = lidar.get_info()
    print(info)

    health = lidar.get_health()
    print(health)
    return lidar
Beispiel #14
0
 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
Beispiel #15
0
    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)]
Beispiel #16
0
    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)
Beispiel #17
0
 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
Beispiel #18
0
    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
Beispiel #19
0
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()
Beispiel #20
0
    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()
Beispiel #21
0
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 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()
Beispiel #23
0
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 update_line(num, iterator, ax, Aport, lidar):
    print(num)
    try:
        scan = next(iterator)
    except:
        print("Second attempt for a Lidar Scan")
        lidar = RPLidar(PORT_NAME)  # Connect to the RPLidar Port
        iterator = lidar.iter_scans()  # Object to pull scans from the RPLidar
        scan = next(iterator)
    """
    Iterator returns 3 arguments:
    meas(0) -> quality : int
            Reflected laser pulse strength
    Meas(1) -> angle : float
            The measurement heading angle in degree unit [0, 360)
    meas(2) -> distance : float
            Measured object distance related to the sensor's rotation center.
            In millimeter unit. Set to 0 when measurement is invalid. 
    """
    # Pass measurements to related variables
    try:
        # First Attempt to read an angle value from the MCU Serial Port
        line = Aport.readline()
        #Aport.write(b'value received')
        line = str(line, "utf-8")
        angle = int(re.sub("[^0-9]", "", line))
    # If a misread occurs, try again
    except:
        # Second attempt to read an angle value from the MCU Serial Port
        # print("Angle misread, second attempt")
        line = Aport.readline()
        # Aport.write(b'value received')
        line = str(line, "utf-8")
        angle = int(re.sub("[^0-9]", "", line))
    # Affine Transform:
    # angle = (Input-Inputstart)/(inputend - inputstart)*(outputend- outputstart)
    angle = (angle) * (360 - 0) / (1024 - 0) + 0
    theta = np.array([(np.radians(meas[1])) for meas in scan])
    R = np.array([meas[2] for meas in scan])
    phi = np.deg2rad(
        angle
    )  # Test value in the phi angle, to be switched with Arduino object value

    # Perform Spherical to Cartesian Conversion
    X = R * np.sin(phi) * np.cos(theta)
    Y = R * np.sin(phi) * np.sin(theta)
    Z = R * np.cos(phi)

    offsets = np.array([X, Y, Z])
    ax.clear()
    ax.set_zlim(-2500, 2500)
    ax.set_xlim(-2500, 2500)
    ax.set_ylim(-2500, 2500)
    ax.scatter3D(X, Y, Z)
    return X, Y, Z
Beispiel #25
0
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()
Beispiel #26
0
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 _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()
Beispiel #28
0
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()
Beispiel #29
0
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)
Beispiel #30
0
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()
Beispiel #31
0
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()
Beispiel #33
0
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()
Beispiel #34
0
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()
Beispiel #35
0
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))
Beispiel #36
0
            ax.plot(x_y[1], x_y[0], "-" if args.plot_lines else "o")
        elif args.front == 2:
            ax.plot([-1 * (x) for x in x_y[0]], x_y[1], "-" if args.plot_lines else "o")
        elif args.front == 3:
            ax.plot([-1 * (y) for y in x_y[1]], [-1 * (x) for x in x_y[0]], "-" if args.plot_lines else "o")
        elif args.front == 4:
            ax.plot(x_y[0], [-1 * (y) for y in x_y[1]], "-" if args.plot_lines else "o")


#         else:
#             ax.plot(x_y[0], x_y[1], 'o') # This is just for debugging what is wrong with the above TODO

if __name__ == "__main__":
    parse_args()
    print args.front, type(args.front)
    lidar = RPLidar(args.port_name)
    lidar.connect()
    print lidar.get_health()
    print lidar.get_device_info()

    lidar.start_scan()
    time.sleep(2)  # Let that baby warm up

    set_plot()
    plt.ion()
    plt.show()

    try:
        while True:
            point_list = [p for p in lidar._raw_points]
            rp_point_list = [RPLidarPoint(raw_point) for raw_point in point_list]
Beispiel #37
0
#GLOBALS
done = 0

def signal_handler(signal, frame):
        global done 
        done = 1


if version[0] == '3':
    import _thread as thread
else:
    import thread

if __name__ == '__main__':
    signal.signal(signal.SIGINT, signal_handler)
    lidar = RPLidar(COM_PORT)
    while done == 0 :
        time.sleep(1)
        distances = [pair[0] for pair in lidar.getScan()]
        
        print " New Scan :    angle |   distance"
        i = 0
        while i < 360:
            print "           {angle} | {distance}".format(angle = i, distance = distances[i])
            i = i+1

    lidar.set_exitflag()
    print""
    print "DONE"
        
Beispiel #38
0
class RPLidarPlotter(tk.Frame):
    '''
    RPLidarPlotter extends tk.Frame to plot Lidar scans.
    '''
    
    def __init__(self):
        '''
        Takes no args.  Maybe we could specify colors, lidar params, etc.
        '''
        
        # Create the frame        
        tk.Frame.__init__(self, borderwidth = 4, relief = 'sunken')
        self.master.geometry(str(DISPLAY_CANVAS_SIZE_PIXELS)+ "x" + str(DISPLAY_CANVAS_SIZE_PIXELS))
        self.master.title('XV Lidar [ESC to quit]')
        self.grid()
        self.master.rowconfigure(0, weight = 1)
        self.master.columnconfigure(0, weight = 1)
        self.grid(sticky = tk.W+tk.E+tk.N+tk.S)
        self.background = DISPLAY_CANVAS_COLOR
        
        # Add a canvas for drawing
        self.canvas =  tk.Canvas(self, \
            width = DISPLAY_CANVAS_SIZE_PIXELS, \
            height = DISPLAY_CANVAS_SIZE_PIXELS,\
            background = DISPLAY_CANVAS_COLOR)
        self.canvas.grid(row = 0, column = 0,\
                    rowspan = 1, columnspan = 1,\
                    sticky = tk.W+tk.E+tk.N+tk.S)

        # Set up a key event for exit on ESC
        self.bind('<Key>', self._key)

        # This call gives the frame focus so that it receives input
        self.focus_set()

        # No scanlines initially                             
        self.lines = []
        
        # Connect to the XVLidar
        self.lidar = RPLidar(COM_PORT)
        
        # No scan data to start
        self.scandata = []
        
        # Pre-compute some values useful for plotting
        
        scan_angle_rad = [radians(-RPLIDAR_DETECTION_DEG/2 + (float(k)/RPLIDAR_SCAN_SIZE) * \
                                   RPLIDAR_DETECTION_DEG) for k in range(RPLIDAR_SCAN_SIZE)]
                                   
        self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2
        scale = self.half_canvas_pix / float(RPLIDAR_MAX_SCAN_DIST_MM)

        self.cos = [-cos(angle) * scale for angle in scan_angle_rad]
        self.sin = [ sin(angle) * scale for angle in scan_angle_rad]
        
        # Add scan lines to canvas, to be modified later        
        self.lines = [self.canvas.create_line(\
                         self.half_canvas_pix, \
                         self.half_canvas_pix, \
                         self.half_canvas_pix + self.sin[k] * RPLIDAR_MAX_SCAN_DIST_MM,\
                         self.half_canvas_pix + self.cos[k] * RPLIDAR_MAX_SCAN_DIST_MM)
                         for k in range(RPLIDAR_SCAN_SIZE)]
                         
        [self.canvas.itemconfig(line, fill=DISPLAY_SCAN_LINE_COLOR) for line in self.lines]
        
        # Start a new thread and set a flag to let it know when we stop running
        thread.start_new_thread( self.grab_scan, () )       
        self.running = True      

    # Runs on its own thread
    def grab_scan(self):

        while True:
            
            # Lidar sends 360 (distance, quality) pairs, which may be empty on start
            self.scandata = [pair[0] for pair in self.lidar.getScan()]

            self.count += 1

            if not self.running:
                break

            # yield to plotting thread
            sleep(.0001)

    def run(self):
        '''
        Call this when you're ready to run.
        '''

        # Record start time and initiate a count of scans for testing
        self.count = 0
        self.start_sec = time()
        self.showcount = 0

        # Start the recursive timer-task
        plotter._task() 

        # Start the GUI
        plotter.mainloop()


    def destroy(self):
        '''
        Called automagically when user clicks X to close window.
        '''  

        self._quit()

    def _quit(self):

        self.running = False
        elapsed_sec = time() - self.start_sec

        del self.lidar

        exit(0)

    def _key(self, event):

        # Make sure the frame is receiving input!
        self.focus_force()
        if event.keysym == 'Escape':
            self._quit()

    def _task(self):

        # Modify the displayed lines according to the current scan
        [self.canvas.coords(self.lines[k], 
            self.half_canvas_pix, \
                    self.half_canvas_pix, \
                    self.half_canvas_pix + self.sin[k] * self.scandata[k],\
                    self.half_canvas_pix + self.cos[k] * self.scandata[k]) \
                    for k in range(len(self.scandata))]    

        # Reschedule this task immediately
        self.after(1, self._task)

        # Record another display for reporting performance
        self.showcount += 1
Beispiel #39
0
    def __init__(self):
        '''
        Takes no args.  Maybe we could specify colors, lidar params, etc.
        '''
        
        # Create the frame        
        tk.Frame.__init__(self, borderwidth = 4, relief = 'sunken')
        self.master.geometry(str(DISPLAY_CANVAS_SIZE_PIXELS)+ "x" + str(DISPLAY_CANVAS_SIZE_PIXELS))
        self.master.title('XV Lidar [ESC to quit]')
        self.grid()
        self.master.rowconfigure(0, weight = 1)
        self.master.columnconfigure(0, weight = 1)
        self.grid(sticky = tk.W+tk.E+tk.N+tk.S)
        self.background = DISPLAY_CANVAS_COLOR
        
        # Add a canvas for drawing
        self.canvas =  tk.Canvas(self, \
            width = DISPLAY_CANVAS_SIZE_PIXELS, \
            height = DISPLAY_CANVAS_SIZE_PIXELS,\
            background = DISPLAY_CANVAS_COLOR)
        self.canvas.grid(row = 0, column = 0,\
                    rowspan = 1, columnspan = 1,\
                    sticky = tk.W+tk.E+tk.N+tk.S)

        # Set up a key event for exit on ESC
        self.bind('<Key>', self._key)

        # This call gives the frame focus so that it receives input
        self.focus_set()

        # No scanlines initially                             
        self.lines = []
        
        # Connect to the XVLidar
        self.lidar = RPLidar(COM_PORT)
        
        # No scan data to start
        self.scandata = []
        
        # Pre-compute some values useful for plotting
        
        scan_angle_rad = [radians(-RPLIDAR_DETECTION_DEG/2 + (float(k)/RPLIDAR_SCAN_SIZE) * \
                                   RPLIDAR_DETECTION_DEG) for k in range(RPLIDAR_SCAN_SIZE)]
                                   
        self.half_canvas_pix = DISPLAY_CANVAS_SIZE_PIXELS / 2
        scale = self.half_canvas_pix / float(RPLIDAR_MAX_SCAN_DIST_MM)

        self.cos = [-cos(angle) * scale for angle in scan_angle_rad]
        self.sin = [ sin(angle) * scale for angle in scan_angle_rad]
        
        # Add scan lines to canvas, to be modified later        
        self.lines = [self.canvas.create_line(\
                         self.half_canvas_pix, \
                         self.half_canvas_pix, \
                         self.half_canvas_pix + self.sin[k] * RPLIDAR_MAX_SCAN_DIST_MM,\
                         self.half_canvas_pix + self.cos[k] * RPLIDAR_MAX_SCAN_DIST_MM)
                         for k in range(RPLIDAR_SCAN_SIZE)]
                         
        [self.canvas.itemconfig(line, fill=DISPLAY_SCAN_LINE_COLOR) for line in self.lines]
        
        # Start a new thread and set a flag to let it know when we stop running
        thread.start_new_thread( self.grab_scan, () )       
        self.running = True      
Beispiel #40
0
MIN_SAMPLES = 180

from breezyslam.algorithms import RMHC_SLAM
from breezyslam.sensors import RPLidarA1 as LaserModel

from rplidar import RPLidar as Lidar

from roboviz import MapVisualizer

from scipy.interpolate import interp1d
import numpy as np

if __name__ == '__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()