Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
335 lines (273 sloc) 10.9 KB
"""
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.
"""
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
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander
from cflib.utils.multiranger import Multiranger
try:
from sip import setapi
setapi('QVariant', 2)
setapi('QString', 2)
except ImportError:
pass
from PyQt4 import QtGui, QtCore
logging.basicConfig(level=logging.INFO)
URI = 'radio://0/65/1M'
if len(sys.argv) > 1:
URI = sys.argv[1]
# Enable plotting of Crazyflie
PLOT_CF = False
# Enable plotting of down sensor
PLOT_SENSOR_UP = False
PLOT_SENSOR_DOWN = False
# Set the sensor threashold (in mm)
SENSOR_TH = 3000
# Set the speed factor for moving and rotating
SPEED_FACTOR = 0.3
def is_close(range):
MIN_DISTANCE = 0.3 # m
if range is None:
return False
else:
return range < MIN_DISTANCE
class MainWindow(QtGui.QMainWindow):
def __init__(self, URI):
QtGui.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': 1.0}
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 and PLOT_SENSOR_UP):
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 = QtGui.QApplication(sys.argv)
win = MainWindow(URI)
win.show()
appQt.exec_()
You can’t perform that action at this time.