A Clojure Autopilot for Parrot A.R. Drone
Another round of code dump, I've been playing with an A.R. Drone, following is a simple autopilot implementation which uses an overhead camera for tracking and guidance.
The way this whole setup works is, I've glued two circles on top of the drone, an overhead camera tracks these two circles. Big blue circle which sits at the center of the drone gives its location, combined with the pink/reddish circle that sits towards the front of the drone, taking the angle between the centers of the circles gives its orientation.
Once you know the position and orientation of the drone, all it takes is simple vector math to get it follow a pre-defined path.
(ns parrot.core
(:refer-clojure :exclude [+ - * = sequence])
(:use [clojure.contrib.generic [arithmetic :only [+ - *]] [comparison :only [=]]]
[pid.core]
[ardrone.core]
[alter-ego.core]
[vector-2d.core]
[vision.core]))
(defn cam-to-world [x y]
(vector-2d
(scale x 0 640 -320 320)
(scale y 0 480 240 -240)))
(defn world-to-cam [{x :x y :y }]
[(scale x -320 320 0 640 )
(scale y -240 240 480 0 )])
Calculations assume origin is at the center of the frame, +x going towards the right of the frame and +y going up the frame. OpenCV assumes origin is at the top left corner. Above functions converts cam coordinates to world coordinates and vice versa.
(defn locate-circle [orig frame c1 c2]
(let [masked (--> (in-range-s frame c1 c2)
(erode 1)
(dilate 5))
loc (with-contours [c [masked :external :chain-approx-none [0 0]]]
(-> c bounding-rects first))]
(if-let [[x y width height] loc]
(let [center-x (+ x (/ width 2))
center-y (+ y (/ height 2))
loc (cam-to-world center-x center-y)]
(rectangle orig [x y] [(+ width x) (+ height y)] java.awt.Color/red 2)
(release masked)
loc)
(do (release masked)
nil))))
locate-circle takes the frame from the camera, a copy of the frame converted to HSV color space and lower/upper limits (Hue Saturation Value) for the color we are trying to isolate. We isolate the color range using in-range-s, it returns a binary image in which pixels that fall with in the range are white rest is black. eroding and then dilating the image removes noise (specks of white from the white tape on the floor.) Finally we find the contours of the match using bounding-rects calculate its center and return it or nil if there is no match.
(defn locate-marker [frame]
(let [hsv (convert-color frame :bgr-hsv)
blue-circle (locate-circle frame hsv [80 150 150 0] [90 255 255 0])
green-circle (locate-circle frame hsv [15 0 150 0] [30 255 255 0])]
(circle frame (world-to-cam (vector-2d 250 200)) 2 java.awt.Color/gray 5)
(let [[x1 y1] (world-to-cam (vector-2d -320 0))
[x2 y2] (world-to-cam (vector-2d 320 0))]
(line frame [x1 y1] [x2 y2] java.awt.Color/gray 1))
(let [[x1 y1] (world-to-cam (vector-2d 0 -240))
[x2 y2] (world-to-cam (vector-2d 0 240))]
(line frame [x1 y1] [x2 y2] java.awt.Color/gray 1))
(view :cam frame)
(release hsv)
(when (and (not (nil? blue-circle))
(not (nil? green-circle)))
[blue-circle green-circle])))
locate-marker tries to match both markers, draws some debug information, shows the frame and returns a vector of vector-2d points when there is match.
(defonce drone-loc (atom [(vector-2d 0 0) 0]))
(defonce cam-running (atom true))
(defn start-cam []
(let [capture (capture-from-cam 0)]
(swap! cam-running (fn [_] true))
(future
(while @cam-running
(try
(let [frame (query-frame capture)]
(if-let [[blue green] (locate-marker frame)]
(let [angle (angle-between-points blue green)]
(swap! drone-loc (fn [_] [blue angle])))))
(catch Exception e
(println e))))
(release capture)
(close-window :cam))))
(defn stop-cam []
(swap! cam-running (fn [_] false)))
start-cam is where we continuously locate the marker, calculate the angle between the markers and update the drone-loc atom until stop-cam is called.
(defpid alt-hold :kp 0.50 :ki 1/400 :kd 1/10 :set-point 1 :bounds [0 3 -1 1]) (defpid yaw-hold :kp 2 :ki 1/10 :kd 1/20 :set-point 90 :bounds [-180 180 1 -1])
These define two PID controllers, one to keep the altitude and another one to keep the orientation at their set points (1 meter above ground facing 90 degrees).
(defpid dist-hold
:kp 1
:ki 0
:kd 4
:set-point 0
:bounds [-1000 1000 0.8 -0.8])
(defn arrive [target]
(let [[loc angle] @drone-loc
to-target (normalize (- target loc))
local-target (rotate to-target (- angle))
{:keys [x y]} (* local-target (dist-hold (dist loc target)) -1)]
(attitude x y (yaw-hold angle) (alt-hold (:alt (nav-data))))))
To fly to a certain point on the map, we begin by drawing a unit vector to the target (to-target) as long as drone is pointing towards 0 degrees (direction of +x) we can use the vectors' x component as pitch and y component as roll but since this is a quadrotor which can move in any direction without turning towards the target, we rotate to-target using the drones current orientation that allows us to map x , y to picth , roll whatever the drones orientation. Finally we scale local-target using a PID controller so it slows down as it approaches the target.
If you are familiar with Craig Reynolds steering behaviors this is basically the arrive behavior with a caveat, what I don't track is the drones' speed, even though local-target gets smaller as it approaches the target it does not apply any breaking so if it is travelling from one end of the map to the other it will fly past the target and come back to it.
(defn within-distance? [target distance]
(<= (int (dist (first @drone-loc) target)) distance))
(defn move-to [p accuracy]
(parallel :selector
(until-success
(sequence
(action (Thread/sleep 20) true)
(action (arrive p))
(action (within-distance? p accuracy))))
(forever
(action
(let [{:keys [x y]} p
{:keys [alt battery]} (nav-data)]
(println :wp [x y]
:battery battery
:alt alt
:yaw (second @drone-loc)
:error (try (dist (first @drone-loc) p)
(catch Exception e :na))))
(Thread/sleep 100)
true))))
To fly to a point on the map, we send an arrive command every 20 milliseconds until we are within a desired distance from the target.
(defn take-off-seq [] (action (reset-comm-watchdog) (nav-data-start) (trim) (takeoff)))
To take off, we reset the communication watch dog, tell drone to start sending telemetry (battery level, altitude, yaw etc.) back, send a trim command to let it calibrate itself then a takeoff command to take off.
(defn land-seq [] (action (hover) (dotimes [_ 10] (land)) (nav-data-stop)))
To land, we send a hover command which levels the drone and send land command bunch of times (since communication is done over UDP sending it once works 90% of the time) and stop the thread that listens for telemetry data.
(defn navigate [& wps] (sequence (take-off-seq) (until-success (action (Thread/sleep 100) (> (:alt (nav-data)) 0.5))) (dynamic (->> wps (map (fn [[wp acc]] (move-to wp acc))) (apply sequence))) (land-seq)))
navigation take a list of waypoint accuracy pairs and puts all of the above together. It will return a sequence that will take off, wait until the drone is half a meter in the air, start flying towards a waypoint until it is within given accuracy/distance and move on to the next one. Finally when it goes through all the waypoints it will land.
(comment (exec-repl (navigate [(vector-2d 0 0) 50] [(vector-2d 200 50) 150] [(vector-2d -200 50) 200] [(vector-2d 0 0) 50]) (land-seq)) )
project.clj
(defproject parrot "1.0.0-SNAPSHOT" :dependencies [[org.clojure/clojure "1.3.0"] [pid "0.1.0-SNAPSHOT"] [ardrone "0.1.0-SNAPSHOT"] [org.clojure/algo.generic "0.1.0"] [alter-ego "0.0.5-SNAPSHOT"] [vector-2d "1.0.0-SNAPSHOT"] [vision "1.0.0-SNAPSHOT"]] :jvm-opts ["-Djna.library.path=/home/nakkaya/Dropbox/code/vision/resources/lib" "-server"])