Autonomus flight with pointcloud (vispy)

Discussions about autonomous flight in general, regardless of positioning method
Post Reply
Vegacheew
Beginner
Posts: 7
Joined: Mon Jun 17, 2019 5:17 pm

Autonomus flight with pointcloud (vispy)

Post by Vegacheew »

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.
Last edited by Vegacheew on Wed Jun 19, 2019 10:10 am, edited 1 time in total.
Vegacheew
Beginner
Posts: 7
Joined: Mon Jun 17, 2019 5:17 pm

Re: Autonomus flight with pointcloud (vispy)

Post by Vegacheew »

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?
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Autonomus flight with pointcloud (vispy)

Post by kimberly »

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.
Vegacheew
Beginner
Posts: 7
Joined: Mon Jun 17, 2019 5:17 pm

Re: Autonomus flight with pointcloud (vispy)

Post by Vegacheew »

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.
wydmynd
Member
Posts: 87
Joined: Thu Dec 06, 2018 9:25 am

Re: Autonomus flight with pointcloud (vispy)

Post by wydmynd »

the multiranger_push is probably sending commands that are blocking keyboard commands.

If you start using threads , you can probably organize the code better.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Autonomus flight with pointcloud (vispy)

Post by kimberly »

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
Vegacheew
Beginner
Posts: 7
Joined: Mon Jun 17, 2019 5:17 pm

Re: Autonomus flight with pointcloud (vispy)

Post by Vegacheew »

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.

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()

The old version of the code where logging conflict seems to happen is here:

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_()
And the data is being logged as in logbasic.py.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Autonomus flight with pointcloud (vispy)

Post by kimberly »

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.
Vegacheew
Beginner
Posts: 7
Joined: Mon Jun 17, 2019 5:17 pm

Re: Autonomus flight with pointcloud (vispy)

Post by Vegacheew »

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.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Autonomus flight with pointcloud (vispy)

Post by kimberly »

Ah okay, good you were able to figure it out! Keep us up to date with the progress
Post Reply