04 Autonomous drone programming in Python

Introduction

Please check our online course (slides or videos) to understand:

• how to connect to the flight controller
• what's the general software architecture
• how to use yocto or ubuntu or the linux OS of choice in a container

Installation

On this page, I suppose you're using Ubuntu installed as described on this page. Install the packages:

sudo apt-get install python-pip python-opencv python-opencv-apps python-zbar zbar-tools vim-python-jedi vim-python-jedi python-editor eric vim-python
pip install Cython numpy
pip install pyrealsense

Flight control

Hello world in PyMAVLINK

Here's a simple python script using the basic pymavlink wrapper to arm the motors for 3 seconds. Arming the motors is the simplest action we can test to show everything is connected. Note: we're using tcp:127.0.0.1:5760 to connect to the flight controller, as we'll do for all the following examples.

UNPLUG THE PROPELLERS BEFORE RUNNING THIS CODE. WE INSIST.

#!/usr/bin/python
from __future__ import print_function

import pymavlink.mavutil as mavutil
import sys
import time

mav = mavutil.mavlink_connection('tcp:127.0.0.1:5760')
mav.wait_heartbeat()
mav.mav.command_long_send(mav.target_system, mav.target_component,
                          mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1,
                          0, 0, 0, 0, 0, 0)
time.sleep(3)
mav.mav.command_long_send(mav.target_system, mav.target_component,
                          mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 0,
                          0, 0, 0, 0, 0, 0)

The motors should spin for 3 seconds. If they don't, it may be because:

• you have not calibrated your flight controller with QGroundControl
• the radio remote is not connected
• your wall power supply does not provide enough power

Hello world in DroneKit

It’s important to know the basics of MAVLINK, as it the base of all communications with the Flight Controllers. But coding frames with python-mavlink is not developer friendly. DroneKit, developed by 3D Robotics, is one of the friendly python abstractions available under Apache v2 Licence : python.dronekit.io To install on Intel Aero, first connect your drone to a WiFi network (in client mode) with internet access and install Dronekit:

pip install dronekit

UNPLUG THE PROPELLERS BEFORE RUNNING THIS CODE. WE INSIST.

Here's the code, still arming the motors for 5 seconds:

#!/usr/bin/python
from dronekit import connect, VehicleMode, LocationGlobalRelative
import time

vehicle = connect('tcp:127.0.0.1:5760', wait_ready=False)
print "Arming motors:"
vehicle.mode    = VehicleMode("GUIDED")
vehicle.armed   = True
while not vehicle.armed:
        print "  Waiting for arming to be finished"
        time.sleep(1)
print "Keeping motors armed for 5s"
time.sleep(5)
print "Disarming"
vehicle.armed   = False

Hello world in PyMAVProxy

On top of being a developer friendly layer on top of MAVLINK, MAVProxy was designed to bridge the gap between programming-only libraries like DroneKit and graphical-only tools like QGroundControl. Check: ardupilot.github.io/MAVProxy Some people use it on a remote computer to control the drone and have optional user interfaces to complex functions, but you can use on the drone itself for autonomous drone development.

MAVProxy is using the MAVLINK protocol, but is focused on the Ardupilot variant, not the PX4 variant. IF you're interested in MAVProxy, you'll have to flash the Flight Controller with the Ardupilot firmware. To install on Intel Aero, first connect your drone to a WiFi network (in client mode) with internet access and install MAVProxy:

pip install MAVProxy

UNPLUG THE PROPELLERS BEFORE RUNNING THIS CODE. WE INSIST.

Launch the shell

mavproxy.py --master=tcp:127.0.0.1:5760 --quadcopter

And type a few commands to arm/disarm the motors:

arm throttle
disarm
bat

Cameras

Intel RealSense

We're covering Python programming with Intel RealSense SDK (pyrealsense) with model R200, included in the Intel Aero Ready To Fly Drone. If you have a newer camera like the D430, you'll need another SDK.

import pyrealsense as pyrs
serv = pyrs.Service()
import logging
logging.basicConfig(level = logging.INFO)

## create a device from device id and streams of interest
cam = serv.Device(device_id = 0, streams = [pyrs.stream.ColorStream(fps = 10)])
for _ in range(60):
    cam.wait_for_frames()
    print(cam.color)
cam.stop()
serv.stop()

OpenCV and RealSense

You can consider the depth data coming from the RealSense camera as an image, and manipulate it with OpenCV. Here's an example.

Barcodes

Let's use zbar to detect barcodes from the camera:

from sys import argv
import zbar
proc = zbar.Processor()
proc.parse_config('enable')

# set the correct device number for your system
device = '/dev/video13'
if len(argv) > 1:
    device = argv[1]
proc.init(device)
proc.process_one()
for symbol in proc.results:
    print 'barcode type=', symbol.type, ' data=', '"%s"' % symbol.data

And here's a test sheet with various sizes of barcodes.

Networking

Networked drone in WebSockets

Please refer to the module D2 - Software - Networked Drone of the course for more information about the following codes.

UNPLUG THE PROPELLERS BEFORE RUNNING THIS CODE. WE INSIST.

The Python websocket API on Intel Aero:

pip install websocket-server

A WebSocket client in your browser, to simulate a web call:

Optionally, a Library on your development station to send the request from a script instead of your browser:

pip install websocket-client

And here's the server code running on Intel Aero:

#!/usr/bin/python
from websocket_server import WebsocketServer
import re
from dronekit import connect, VehicleMode, LocationGlobalRelative
import time
vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
vehicle.mode    = VehicleMode("GUIDED")
print("Flight Controller Connected")
def new_client(client, server):
        print("Client connected")
        server.send_message_to_all("Client connected")
def message_received(client, server, message):
	if len(message) > 200:
		message = message[:200]+'..'
	print("Arming motors")
	vehicle.armed   = True
	while not vehicle.armed:
		time.sleep(1)
	time.sleep(5)
	print("Disarming")
	vehicle.armed   = False
server = WebsocketServer(8080, '0.0.0.0')
server.set_fn_new_client(new_client)
server.set_fn_message_received(message_received)
server.run_forever()

And the optional client code, running on a remote computer (change the IP 192.168.0.100 with Aero's IP on YOUR NETWORK):

#!/usr/bin/python
from websocket import create_connection
ws = create_connection("ws://192.168.0.100:8080")
ws.send("Alert, send drone")
result = ws.recv()
print("Received '%s'" % result)
ws.close()

Networked drone in MQTT

Please refer to the module D2 - Software - Networked Drone of the course for more information about the following codes.

To install the Python MQTT API on Intel Aero (should be there already):

pip install paho-mqtt 

A MQTT client in your browser, to generate a message posting.

UNPLUG THE PROPELLERS BEFORE RUNNING THIS CODE. WE INSIST.

Here's the client code, running on Intel Aero:

#!/usr/bin/python
import paho.mqtt.client as mqtt
from dronekit import connect, VehicleMode, LocationGlobalRelative
import time
vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
vehicle.mode    = VehicleMode("GUIDED")
print("Flight Controller Connected")

def on_connect(client, userdata, rc):
	print("Client connected ")
	client.subscribe("aero-paul")
def on_message(client, userdata, msg):
	print("Arming motors ("+msg.topic+"/"+str(msg.payload)+")")
	vehicle.armed   = True                                   
	while not vehicle.armed:                                 
		time.sleep(1)                           
	time.sleep(5)                                   
	print("Disarming")                            
	vehicle.armed   = False                       

client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
client.connect("test.mosquitto.org", 1883, 60)
client.loop_forever()

Peripherals

Intel Aero board LEDs

There's a multicolor LED on top of the board (if the board is in the enclosure, you can see the light from the white cable hole), and an orange LED under the board. As the LEDs are enclosed in the Ready-To-Fly design, it is not very useful. But if you build your own drone design or enclosing you may want to let the LEDs visible and use them. To install the IO module:

pip install python-periphery

And here is a sample code to test all the LED colors:

#!/usr/bin/python
import time
from periphery import GPIO

print "Top LED Blue"
gpio = GPIO(403, "out")
gpio.write(bool(1))
time.sleep(1)
gpio.write(bool(0))
gpio.close()

print "Top LED Green"
gpio = GPIO(397, "out")
gpio.write(bool(1))
time.sleep(1)
gpio.write(bool(0))
gpio.close()

print "Top LED Red"
gpio = GPIO(437, "out")
gpio.write(bool(1))
time.sleep(1)
gpio.write(bool(0))
gpio.close()

print "Bottom LED Orange"
gpio = GPIO(507, "out")
gpio.write(bool(1))
time.sleep(1)
gpio.write(bool(0))
gpio.close()

CAN

The Intel Aero Compute Board includes a MCP2515 CAN controller and MCP2562 CAN transceiver. The controller is connected to the Atom processor via the SPI interface on bus 1 (SPI1) chip select 0 (CS0). It can be accessed via spidev as /dev/spidev1.0. Python spidev libraries