Autonomus flight with pointcloud (vispy)
Autonomus flight with pointcloud (vispy)
Hello everyone!
I'm working on combining custom autonomous flight script with pointcloud to make autonomous 3D plot of obstacles detected by multiranger. Autonomous algorithm can be found here: (https://github.com/neolant-com/Quadcopt ... _test_2.py), it works fine, does its job, so I want to merge this one with pointcloud_multiranger from here: (https://github.com/bitcraze/crazyflie-l ... ntcloud.py), which works with keyboard inputs, but doesn't work without them for some reason. The problem is that autonomous flight cycle just doesn't start: drone takes off, gets to 1 meter height as have been set up and hovers there. It can be controlled by keypresses, but it doesn't go anywhere on its own. I tried to make these two combinations, one at a time (https://github.com/neolant-com/Quadcopt ... ntcloud.py, one block in the middle starting at line 201 - static class method, and one block in the very end). Both of them treat my autonomous flight script as it's not there. One more problem is that there are no errors when I run the script, so I don't know what's wrong with it.
1) Is it possible that there is some kind of module or logging conflict?
2) Can I combine anything with vispy/Qt at all?
3) Is it possible to make one script with two simultaneous functions, one of which is going to be autonomous flight and the other one being the pointcloud (Asyncio maybe)?
Best regards,
Marsel.
I'm working on combining custom autonomous flight script with pointcloud to make autonomous 3D plot of obstacles detected by multiranger. Autonomous algorithm can be found here: (https://github.com/neolant-com/Quadcopt ... _test_2.py), it works fine, does its job, so I want to merge this one with pointcloud_multiranger from here: (https://github.com/bitcraze/crazyflie-l ... ntcloud.py), which works with keyboard inputs, but doesn't work without them for some reason. The problem is that autonomous flight cycle just doesn't start: drone takes off, gets to 1 meter height as have been set up and hovers there. It can be controlled by keypresses, but it doesn't go anywhere on its own. I tried to make these two combinations, one at a time (https://github.com/neolant-com/Quadcopt ... ntcloud.py, one block in the middle starting at line 201 - static class method, and one block in the very end). Both of them treat my autonomous flight script as it's not there. One more problem is that there are no errors when I run the script, so I don't know what's wrong with it.
1) Is it possible that there is some kind of module or logging conflict?
2) Can I combine anything with vispy/Qt at all?
3) Is it possible to make one script with two simultaneous functions, one of which is going to be autonomous flight and the other one being the pointcloud (Asyncio maybe)?
Best regards,
Marsel.
Last edited by Vegacheew on Wed Jun 19, 2019 10:10 am, edited 1 time in total.
Re: Autonomus flight with pointcloud (vispy)
Hello again!
It looks like the board gets no reaction from motion commander for some reason and keeps hovering at default height, or it may get the command but it is overwritten immediately by hover dictionary (default position). Can I set up starting position in other way, maybe by using motion commander, or update hover dictionary with motion commander?
It looks like the board gets no reaction from motion commander for some reason and keeps hovering at default height, or it may get the command but it is overwritten immediately by hover dictionary (default position). Can I set up starting position in other way, maybe by using motion commander, or update hover dictionary with motion commander?
Re: Autonomus flight with pointcloud (vispy)
Hi Marsel,
I hope you were able to solve your problem in the main time.
You noticed that the multiranger_pointcloud.py was not working. Did you report this issue on the github repository?
Are scripts like multiranger_push.py or motion_commander_demo.py working for you?
About your question of simultaneous actions, you could make seperate threads in python but I would not recommend using them. You could use the 'start_forward', 'start_left' etc. functions of the motion commander, since those are non-blocking and you can do other things in the mean time. Look at motion_commander_demo.py (line 90-95) to see an simple example.
I hope you were able to solve your problem in the main time.
You noticed that the multiranger_pointcloud.py was not working. Did you report this issue on the github repository?
Are scripts like multiranger_push.py or motion_commander_demo.py working for you?
About your question of simultaneous actions, you could make seperate threads in python but I would not recommend using them. You could use the 'start_forward', 'start_left' etc. functions of the motion commander, since those are non-blocking and you can do other things in the mean time. Look at motion_commander_demo.py (line 90-95) to see an simple example.
Re: Autonomus flight with pointcloud (vispy)
Hello Kimberly!
I'm still struggling with this problem. Both scripts multiranger_push.py and motion_commander_demo.py work properly for me. Multiranger_pointcloud.py works as well, but it works with keyboard input only. The problem is that I can't make it work using multiranger. When I trying to launch my automove function inside, it takes off and starts moving forward, but for some unknown reason, when I try to obtain data from Multiranger, it always returns None and it doesn't matter where I put this piece of code, it always prints <None type> when I try to debug it in with PyCharm console. In the same time, Qt window plotting pointcloud pops up and works well.
So basically, as I understood, autonomous flight doesn't work because CF doesn't see any obstacles because Multiranger isn't working on high-level. But pointcloud in Qt window works, so CF can see these obstacles via raw logging.
So, it seems logging works for process of building pointcloud, but can't work for Multiranger at the same time? If so, how can I split the same data in order to build the pointcloud and to maneuver autonomosly at the same time? How can I use the same log to send motion commands?
Thanks,
Marsel.
I'm still struggling with this problem. Both scripts multiranger_push.py and motion_commander_demo.py work properly for me. Multiranger_pointcloud.py works as well, but it works with keyboard input only. The problem is that I can't make it work using multiranger. When I trying to launch my automove function inside, it takes off and starts moving forward, but for some unknown reason, when I try to obtain data from Multiranger, it always returns None and it doesn't matter where I put this piece of code, it always prints <None type> when I try to debug it in with PyCharm console. In the same time, Qt window plotting pointcloud pops up and works well.
So basically, as I understood, autonomous flight doesn't work because CF doesn't see any obstacles because Multiranger isn't working on high-level. But pointcloud in Qt window works, so CF can see these obstacles via raw logging.
So, it seems logging works for process of building pointcloud, but can't work for Multiranger at the same time? If so, how can I split the same data in order to build the pointcloud and to maneuver autonomosly at the same time? How can I use the same log to send motion commands?
Thanks,
Marsel.
Re: Autonomus flight with pointcloud (vispy)
the multiranger_push is probably sending commands that are blocking keyboard commands.
If you start using threads , you can probably organize the code better.
If you start using threads , you can probably organize the code better.
Re: Autonomus flight with pointcloud (vispy)
Hmm... the logging is supposed to work together with the multiranger... Could you send the python code you are flying with?
Are you logging the data as in logbasicsync.py or logbasic.py ? It might be better to do it with callbacks like in logbasic.py
Are you logging the data as in logbasicsync.py or logbasic.py ? It might be better to do it with callbacks like in logbasic.py
Re: Autonomus flight with pointcloud (vispy)
Hello Kimberly!
I changed my code a little bit, added threads, and I'm looking for measurements logging in order to give commands avoid collisions, but it still doesn't fully work. It can fly in one direction and it's building pointcloud, but it still doesn't avoid obstacles.
The old version of the code where logging conflict seems to happen is here:
And the data is being logged as in logbasic.py.
I changed my code a little bit, added threads, and I'm looking for measurements logging in order to give commands avoid collisions, but it still doesn't fully work. It can fly in one direction and it's building pointcloud, but it still doesn't avoid obstacles.
Code: Select all
"""
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 time
from threading import Thread
from multiprocessing import Process
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 = 4000
# Set the speed factor for moving and rotating
SPEED_FACTOR = 0.1
def is_close(range):
MIN_DISTANCE = 300 # mm
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(1400, 1000)
self.setWindowTitle('Multi-ranger point cloud')
self.canvas = Canvas()
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.motion_commander = MotionCommander(self.cf)
self.multiranger = Multiranger(self.cf)
self.KEEP_FLYING = True
time.sleep(2)
# self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.2}
# self.hoverTimer = QtCore.QTimer()
# self.hoverTimer.timeout.connect(self.sendHoverCommand)
# self.hoverTimer.singleShot(self.sendHoverCommand())
# self.hoverTimer.start()
def sendHoverCommand(self):
self.motion_commander.take_off(0.2, 0.2)
time.sleep(1)
self.motion_commander.start_forward(0.05)
while self.KEEP_FLYING:
print(f"range.up = {self.measurement['up']}")
print(f"range.front = {self.measurement['front']}")
print(f"range.left = {self.measurement['left']}")
print(f"range.right = {self.measurement['right']}")
print(f"is_close(up) = {is_close(self.measurement['up'])}")
if is_close(self.measurement['up']):
self.motion_commander.land(0.1)
self.KEEP_FLYING = False
if is_close(self.measurement['front']):
self.motion_commander.turn_right(90, 90)
time.sleep(1)
self.motion_commander.start_forward(0.05)
if is_close(self.measurement['front']) and self.measurement['left'] > self.measurement['right']:
self.motion_commander.turn_left(90, 90)
time.sleep(1)
self.motion_commander.start_forward(0.05)
if is_close(self.measurement['front']) and self.measurement['left'] < self.measurement['right']:
self.motion_commander.turn_right(90, 90)
time.sleep(1)
self.motion_commander.start_forward(0.05)
if is_close(self.measurement['left']):
self.motion_commander.turn_right(45, 90)
time.sleep(1)
self.motion_commander.start_forward(0.05)
if is_close(self.measurement['right']):
self.motion_commander.turn_left(45, 90)
time.sleep(1)
self.motion_commander.start_forward(0.05)
# 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.position = position
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.measurement = measurement
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):
scene.SceneCanvas.__init__(self, keys=None)
self.size = 1300, 900
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.freeze()
scene.visuals.XYZAxis(parent=self.view.scene)
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()
th_1, th_2, th_3 = Thread(target=win.sendHoverCommand), Thread(target=appQt.exec_), Thread(target=win.canvas.set_measurement)
th_1.start(), th_2.start(), th_3.start()
th_1.join(), th_2.join(), th_3.join()
Code: Select all
"""
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.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 = True
# Enable plotting of down sensor
PLOT_SENSOR_UP = False
PLOT_SENSOR_DOWN = False
# Set the sensor threashold (in mm)
SENSOR_TH = 4000
# Set the speed factor for moving and rotating
SPEED_FACTOR = 0.1
keep_flying = True
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)
# self.scf = SyncCrazyflie(URI, cf=self.cf)
# Connect to the Crazyflie
self.cf.open_link(URI)
self.motion_commander = MotionCommander(self.cf)
self.multiranger = Multiranger(self.cf)
# self.motion_commander.take_off(0.3, 0.1)
# 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.automotion)
self.hoverTimer.setInterval(100)
self.hoverTimer.start()
def automotion(self):
while 1:
print(f"Multiranger.up = {self.multiranger.up}, {type(self.multiranger.up)}")
if is_close(self.multiranger.up):
self.motion_commander.land(0.1)
break
if is_close(self.multiranger.front) and is_close(self.multiranger.right) and is_close(self.multiranger.left):
self.motion_commander.turn_right(180, 90)
if is_close(self.multiranger.front) and self.multiranger.right < self.multiranger.left:
self.motion_commander.turn_left(90, 90)
if is_close(self.multiranger.front) and self.multiranger.right > self.multiranger.left:
self.motion_commander.turn_right(90, 90)
if is_close(self.multiranger.back):
self.motion_commander.start_forward(0.2)
if is_close(self.multiranger.left):
self.motion_commander.turn_right(45)
if is_close(self.multiranger.right):
self.motion_commander.turn_left(45)
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=20.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_()
Re: Autonomus flight with pointcloud (vispy)
Hmm it is a bit difficult to see what is going on. If it keeps going forward it means that the script is being blocked somewhere. So it sees the obstacles but it won't stop and turn right?
Could you maybe make a more minimal example, because it is a bit difficult to see what is going on. Could you make something that goes forward, logs at the same time and stops once it detects something in its front laser range sensor?
This week we are not present at our office due to holidays, but next week or after I could try out your code here and see how we can fix it.
Could you maybe make a more minimal example, because it is a bit difficult to see what is going on. Could you make something that goes forward, logs at the same time and stops once it detects something in its front laser range sensor?
This week we are not present at our office due to holidays, but next week or after I could try out your code here and see how we can fix it.
Re: Autonomus flight with pointcloud (vispy)
I managed to do it. Threads didn't help, everything had to be done as signals via PyQt. Now it works as intended, the only thing that is needed is quick speed fix.
Re: Autonomus flight with pointcloud (vispy)
Ah okay, good you were able to figure it out! Keep us up to date with the progress