Skip to content
Snippets Groups Projects
Select Git revision
  • v0.1
  • master default
  • v0.2
3 results

beamerwindow.cpp

Blame
  • multiranger_pointcloud.py 12.14 KiB
    #!/usr/bin/env python2
    # -*- coding: utf-8 -*-
    #
    #     ||          ____  _ __
    #  +------+      / __ )(_) /_______________ _____  ___
    #  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
    #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
    #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
    #
    #  Copyright (C) 2018 Bitcraze AB
    #
    #  Crazyflie Python Library
    #
    #  This program is free software; you can redistribute it and/or
    #  modify it under the terms of the GNU General Public License
    #  as published by the Free Software Foundation; either version 2
    #  of the License, or (at your option) any later version.
    #
    #  This program is distributed in the hope that it will be useful,
    #  but WITHOUT ANY WARRANTY; without even the implied warranty of
    #  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    #  GNU General Public License for more details.
    #  You should have received a copy of the GNU General Public License
    #  along with this program; if not, write to the Free Software
    #  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
    #  MA  02110-1301, USA.
    """
    Example script that plots the output ranges from the Multiranger and Flow
    deck in a 3D plot.
    
    When the application is started the Crazyflie will hover at 0.3 m. The
    Crazyflie can then be controlled by using keyboard input:
     * Move by using the arrow keys (left/right/forward/backwards)
     * Adjust the right with w/s (0.1 m for each keypress)
     * Yaw slowly using a/d (CCW/CW)
     * Yaw fast using z/x (CCW/CW)
    
    There's additional setting for (see constants below):
     * Plotting the downwards sensor
     * Plotting the estimated Crazyflie postition
     * Max threashold for sensors
     * Speed factor that set's how fast the Crazyflie moves
    
    The demo is ended by either closing the graph window.
    
    For the example to run the following hardware is needed:
     * Crazyflie 2.0
     * Crazyradio PA
     * Flow deck
     * Multiranger deck
    """
    import logging
    import math
    import sys
    
    import numpy as np
    from vispy import scene
    from vispy.scene import visuals
    from vispy.scene.cameras import TurntableCamera
    
    import cflib.crtp
    from cflib.crazyflie import Crazyflie
    from cflib.crazyflie.log import LogConfig
    
    try:
        from sip import setapi
        setapi('QVariant', 2)
        setapi('QString', 2)
    except ImportError:
        pass
    
    from PyQt5 import QtCore, QtWidgets
    
    logging.basicConfig(level=logging.INFO)
    
    URI = 'radio://0/80/2M'
    
    if len(sys.argv) > 1:
        URI = sys.argv[1]
    
    # Enable plotting of Crazyflie
    PLOT_CF = False
    # Enable plotting of down sensor
    PLOT_SENSOR_DOWN = False
    # Set the sensor threashold (in mm)
    SENSOR_TH = 2000
    # Set the speed factor for moving and rotating
    SPEED_FACTOR = 0.3
    
    
    class MainWindow(QtWidgets.QMainWindow):
    
        def __init__(self, URI):
            QtWidgets.QMainWindow.__init__(self)
    
            self.resize(700, 500)
            self.setWindowTitle('Multi-ranger point cloud')
    
            self.canvas = Canvas(self.updateHover)
            self.canvas.create_native()
            self.canvas.native.setParent(self)
    
            self.setCentralWidget(self.canvas.native)
    
            cflib.crtp.init_drivers(enable_debug_driver=False)
            self.cf = Crazyflie(ro_cache=None, rw_cache='cache')
    
            # Connect callbacks from the Crazyflie API
            self.cf.connected.add_callback(self.connected)
            self.cf.disconnected.add_callback(self.disconnected)
    
            # Connect to the Crazyflie
            self.cf.open_link(URI)
    
            self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3}
    
            self.hoverTimer = QtCore.QTimer()
            self.hoverTimer.timeout.connect(self.sendHoverCommand)
            self.hoverTimer.setInterval(100)
            self.hoverTimer.start()
    
        def sendHoverCommand(self):
            self.cf.commander.send_hover_setpoint(
                self.hover['x'], self.hover['y'], self.hover['yaw'],
                self.hover['height'])
    
        def updateHover(self, k, v):
            if (k != 'height'):
                self.hover[k] = v * SPEED_FACTOR
            else:
                self.hover[k] += v
    
        def disconnected(self, URI):
            print('Disconnected')
    
        def connected(self, URI):
            print('We are now connected to {}'.format(URI))
    
            # The definition of the logconfig can be made before connecting
            lpos = LogConfig(name='Position', period_in_ms=100)
            lpos.add_variable('stateEstimate.x')
            lpos.add_variable('stateEstimate.y')
            lpos.add_variable('stateEstimate.z')
    
            try:
                self.cf.log.add_config(lpos)
                lpos.data_received_cb.add_callback(self.pos_data)
                lpos.start()
            except KeyError as e:
                print('Could not start log configuration,'
                      '{} not found in TOC'.format(str(e)))
            except AttributeError:
                print('Could not add Position log config, bad configuration.')
    
            lmeas = LogConfig(name='Meas', period_in_ms=100)
            lmeas.add_variable('range.front')
            lmeas.add_variable('range.back')
            lmeas.add_variable('range.up')
            lmeas.add_variable('range.left')
            lmeas.add_variable('range.right')
            lmeas.add_variable('range.zrange')
            lmeas.add_variable('stabilizer.roll')
            lmeas.add_variable('stabilizer.pitch')
            lmeas.add_variable('stabilizer.yaw')
    
            try:
                self.cf.log.add_config(lmeas)
                lmeas.data_received_cb.add_callback(self.meas_data)
                lmeas.start()
            except KeyError as e:
                print('Could not start log configuration,'
                      '{} not found in TOC'.format(str(e)))
            except AttributeError:
                print('Could not add Measurement log config, bad configuration.')
    
        def pos_data(self, timestamp, data, logconf):
            position = [
                data['stateEstimate.x'],
                data['stateEstimate.y'],
                data['stateEstimate.z']
            ]
            self.canvas.set_position(position)
    
        def meas_data(self, timestamp, data, logconf):
            measurement = {
                'roll': data['stabilizer.roll'],
                'pitch': data['stabilizer.pitch'],
                'yaw': data['stabilizer.yaw'],
                'front': data['range.front'],
                'back': data['range.back'],
                'up': data['range.up'],
                'down': data['range.zrange'],
                'left': data['range.left'],
                'right': data['range.right']
            }
            self.canvas.set_measurement(measurement)
    
        def closeEvent(self, event):
            if (self.cf is not None):
                self.cf.close_link()
    
    
    class Canvas(scene.SceneCanvas):
        def __init__(self, keyupdateCB):
            scene.SceneCanvas.__init__(self, keys=None)
            self.size = 800, 600
            self.unfreeze()
            self.view = self.central_widget.add_view()
            self.view.bgcolor = '#ffffff'
            self.view.camera = TurntableCamera(
                fov=10.0, distance=30.0, up='+z', center=(0.0, 0.0, 0.0))
            self.last_pos = [0, 0, 0]
            self.pos_markers = visuals.Markers()
            self.meas_markers = visuals.Markers()
            self.pos_data = np.array([0, 0, 0], ndmin=2)
            self.meas_data = np.array([0, 0, 0], ndmin=2)
            self.lines = []
    
            self.view.add(self.pos_markers)
            self.view.add(self.meas_markers)
            for i in range(6):
                line = visuals.Line()
                self.lines.append(line)
                self.view.add(line)
    
            self.keyCB = keyupdateCB
    
            self.freeze()
    
            scene.visuals.XYZAxis(parent=self.view.scene)
    
        def on_key_press(self, event):
            if (not event.native.isAutoRepeat()):
                if (event.native.key() == QtCore.Qt.Key_Left):
                    self.keyCB('y', 1)
                if (event.native.key() == QtCore.Qt.Key_Right):
                    self.keyCB('y', -1)
                if (event.native.key() == QtCore.Qt.Key_Up):
                    self.keyCB('x', 1)
                if (event.native.key() == QtCore.Qt.Key_Down):
                    self.keyCB('x', -1)
                if (event.native.key() == QtCore.Qt.Key_A):
                    self.keyCB('yaw', -70)
                if (event.native.key() == QtCore.Qt.Key_D):
                    self.keyCB('yaw', 70)
                if (event.native.key() == QtCore.Qt.Key_Z):
                    self.keyCB('yaw', -200)
                if (event.native.key() == QtCore.Qt.Key_X):
                    self.keyCB('yaw', 200)
                if (event.native.key() == QtCore.Qt.Key_W):
                    self.keyCB('height', 0.1)
                if (event.native.key() == QtCore.Qt.Key_S):
                    self.keyCB('height', -0.1)
    
        def on_key_release(self, event):
            if (not event.native.isAutoRepeat()):
                if (event.native.key() == QtCore.Qt.Key_Left):
                    self.keyCB('y', 0)
                if (event.native.key() == QtCore.Qt.Key_Right):
                    self.keyCB('y', 0)
                if (event.native.key() == QtCore.Qt.Key_Up):
                    self.keyCB('x', 0)
                if (event.native.key() == QtCore.Qt.Key_Down):
                    self.keyCB('x', 0)
                if (event.native.key() == QtCore.Qt.Key_A):
                    self.keyCB('yaw', 0)
                if (event.native.key() == QtCore.Qt.Key_D):
                    self.keyCB('yaw', 0)
                if (event.native.key() == QtCore.Qt.Key_W):
                    self.keyCB('height', 0)
                if (event.native.key() == QtCore.Qt.Key_S):
                    self.keyCB('height', 0)
                if (event.native.key() == QtCore.Qt.Key_Z):
                    self.keyCB('yaw', 0)
                if (event.native.key() == QtCore.Qt.Key_X):
                    self.keyCB('yaw', 0)
    
        def set_position(self, pos):
            self.last_pos = pos
            if (PLOT_CF):
                self.pos_data = np.append(self.pos_data, [pos], axis=0)
                self.pos_markers.set_data(self.pos_data, face_color='red', size=5)
    
        def rot(self, roll, pitch, yaw, origin, point):
            cosr = math.cos(math.radians(roll))
            cosp = math.cos(math.radians(pitch))
            cosy = math.cos(math.radians(yaw))
    
            sinr = math.sin(math.radians(roll))
            sinp = math.sin(math.radians(pitch))
            siny = math.sin(math.radians(yaw))
    
            roty = np.array([[cosy, -siny, 0],
                             [siny, cosy, 0],
                             [0, 0,    1]])
    
            rotp = np.array([[cosp, 0, sinp],
                             [0, 1, 0],
                             [-sinp, 0, cosp]])
    
            rotr = np.array([[1, 0,   0],
                             [0, cosr, -sinr],
                             [0, sinr,  cosr]])
    
            rotFirst = np.dot(rotr, rotp)
    
            rot = np.array(np.dot(rotFirst, roty))
    
            tmp = np.subtract(point, origin)
            tmp2 = np.dot(rot, tmp)
            return np.add(tmp2, origin)
    
        def rotate_and_create_points(self, m):
            data = []
            o = self.last_pos
            roll = m['roll']
            pitch = -m['pitch']
            yaw = m['yaw']
    
            if (m['up'] < SENSOR_TH):
                up = [o[0], o[1], o[2] + m['up'] / 1000.0]
                data.append(self.rot(roll, pitch, yaw, o, up))
    
            if (m['down'] < SENSOR_TH and PLOT_SENSOR_DOWN):
                down = [o[0], o[1], o[2] - m['down'] / 1000.0]
                data.append(self.rot(roll, pitch, yaw, o, down))
    
            if (m['left'] < SENSOR_TH):
                left = [o[0], o[1] + m['left'] / 1000.0, o[2]]
                data.append(self.rot(roll, pitch, yaw, o, left))
    
            if (m['right'] < SENSOR_TH):
                right = [o[0], o[1] - m['right'] / 1000.0, o[2]]
                data.append(self.rot(roll, pitch, yaw, o, right))
    
            if (m['front'] < SENSOR_TH):
                front = [o[0] + m['front'] / 1000.0, o[1], o[2]]
                data.append(self.rot(roll, pitch, yaw, o, front))
    
            if (m['back'] < SENSOR_TH):
                back = [o[0] - m['back'] / 1000.0, o[1], o[2]]
                data.append(self.rot(roll, pitch, yaw, o, back))
    
            return data
    
        def set_measurement(self, measurements):
            data = self.rotate_and_create_points(measurements)
            o = self.last_pos
            for i in range(6):
                if (i < len(data)):
                    o = self.last_pos
                    self.lines[i].set_data(np.array([o, data[i]]))
                else:
                    self.lines[i].set_data(np.array([o, o]))
    
            if (len(data) > 0):
                self.meas_data = np.append(self.meas_data, data, axis=0)
            self.meas_markers.set_data(self.meas_data, face_color='blue', size=5)
    
    
    if __name__ == '__main__':
        appQt = QtWidgets.QApplication(sys.argv)
        win = MainWindow(URI)
        win.show()
        appQt.exec_()