Added initial autonomy mission class

examples/mission.js

@@ -0,0 +1,27 @@
+var df = require('dateformat')
+  , autonomy = require('../')
+  , mission  = autonomy.createMission()
+  ;
+
+mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt");
+
+mission.takeoff()
+       .hover(1000)
+       .forward(1)
+       .right(1)
+       .backward(1)
+       .left(1)
+       .hover(1000)
+       .land();
+
+mission.run(function (err, result) {
+    if (err) {
+        console.trace("Oops, something bad happened: %s", err.message);
+        mission.client().stop();
+        mission.client().land();
+    } else {
+        console.log("We are done!");
+        process.exit(0);
+    }
+});
+

index.js

@@ -1,12 +1,20 @@
 var autonomy = exports;
+var ardrone = require('ar-drone');
 
-exports.StateEstimator = require('./lib/StateEstimator');
 exports.EKF = require('./lib/EKF');
 exports.Camera = require('./lib/Camera');
 exports.Controller = require('./lib/Controller');
+exports.Mission = require('./lib/Mission');
 
-exports.estimateState = function(client, options) {
+exports.control = function(client, options) {
-    var estimator = new autonomy.StateEstimator(client, options);
+    return new autonomy.Controller(client, options);
-    return estimator;
+}
+
+exports.createMission = function(options) {
+    var client  = ardrone.createClient(options);
+    var control = new autonomy.Controller(client, options);
+    var mission = new autonomy.Mission(client, control, options);
+
+    return mission;
 }
 

lib/Controller.js

@@ -174,6 +174,37 @@ Controller.prototype.ccw = function(angle, callback) {
     return this.cw(-angle, callback);
 }
 
+/*
+ * Climb ups by the given distance (in meters).
+ */
+Controller.prototype.up = function(distance, callback) {
+    var state = this.state();
+    return this.go({x: state.x, y: state.y, z: state.z + distance, yaw: state.yaw}, callback);
+}
+
+/*
+ * Lower itself by the given distance (in meters).
+ */
+Controller.prototype.down = function(distance, callback) {
+    return this.up(-distance, callback);
+}
+
+/*
+ * Go to the target altitude
+ */
+Controller.prototype.altitude = function(altitude, callback) {
+    var state = this.state();
+    return this.go({x: state.x, y: state.y, z: altitude, yaw: state.yaw}, callback);
+}
+
+/*
+ * Go to the target yaw
+ */
+Controller.prototype.yaw = function(yaw, callback) {
+    var state = this.state();
+    return this.go({x: state.x, y: state.y, z: state.z, yaw: yaw}, callback);
+}
+
 /*
  * Sets a new goal and enable the controller. When the goal
  * is reached, the callback is called with the current state.
@@ -193,6 +224,11 @@ Controller.prototype.go = function(goal, callback) {
         goal.yaw = Math.atan2(Math.sin(yaw),Math.cos(yaw));
     }
 
+    // Make sure we don't attempt to go too low
+    if (goal.z != undefined) {
+        goal.z = Math.max(goal.z, 0.5);
+    }
+
     // Update our goal
     this._goal = goal;
     this._goal.reached = false;

lib/Mission.js

@@ -0,0 +1,201 @@
+var async = require('async')
+  , fs    = require('fs')
+  ;
+
+module.exports = Mission;
+function Mission(client, controller, options) {
+
+    options = options || {};
+
+    this._options   = options;
+    this._client    = client;
+    this._control   = controller;
+
+    this._steps     = [];
+}
+
+Mission.prototype.client = function() {
+    return this._client;
+}
+
+Mission.prototype.run = function(callback) {
+    async.waterfall(this._steps, callback);
+}
+
+Mission.prototype.log = function(path) {
+    var dataStream = fs.createWriteStream(path);
+    this._control.on('controlData', function(d) {
+        dataStream.write(d.state.x + "," +
+                        d.state.y + "," +
+                        d.state.z + "," +
+                        d.state.yaw + "," +
+                        d.state.vx + "," +
+                        d.state.vy + "," +
+                        d.goal.x + "," +
+                        d.goal.y + "," +
+                        d.goal.z + "," +
+                        d.goal.yaw + "," +
+                        d.error.ex + "," +
+                        d.error.ey + "," +
+                        d.error.ez + "," +
+                        d.error.eyaw + "," +
+                        d.control.ux + "," +
+                        d.control.uy + "," +
+                        d.control.uz + "," +
+                        d.control.uyaw + "," +
+                        d.last_ok + "," +
+                        d.tag + "\n");
+    });
+}
+
+Mission.prototype.takeoff = function() {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._client.takeoff(cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.land = function() {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._client.land(cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.hover = function(delay) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.hover();
+        setTimeout(cb, delay);
+    });
+
+    return this;
+}
+
+Mission.prototype.wait = function(delay) {
+    this._steps.push(function(cb) {
+        setTimeout(cb, delay);
+    });
+
+    return this;
+}
+
+Mission.prototype.task = function(task) {
+    this._steps.push(function(cb) {
+        task(cb);
+    });
+}
+
+Mission.prototype.taskSync = function(task) {
+    this._steps.push(function(cb) {
+        task();
+        cb();
+    });
+}
+
+Mission.prototype.go = function(goal) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.go(goal, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.forward = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.forward(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.backward = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.backward(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.left = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.left(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.right = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.right(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.up = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.up(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.down = function(distance) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.down(distance, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.cw = function(angle) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.cw(angle, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.ccw = function(angle) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.ccw(angle, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.altitude = function(altitude) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.altitude(altitude, cb);
+    });
+
+    return this;
+}
+
+Mission.prototype.yaw = function(angle) {
+    var self = this;
+    this._steps.push(function(cb) {
+        self._control.yaw(angle,cb);
+    });
+
+    return this;
+}
+
+
+
+

tests/integration/mission.js

@@ -0,0 +1,32 @@
+var df = require('dateformat')
+  , common = require('../common')
+  , autonomy = require(common.root)
+  , mockClient = require('./mock/client')
+  ;
+
+var client     = new mockClient();
+var controller = new autonomy.Controller(client);
+var mission    = new autonomy.Mission(client, controller);
+
+mission.log("mission-" + df(new Date(), "yyyy-mm-dd_hh-MM-ss") + ".txt");
+
+mission.takeoff()
+       .hover(1000)
+       .forward(1)
+       .right(1)
+       .backward(1)
+       .left(1)
+       .hover(1000)
+       .land();
+
+mission.run(function (err, result) {
+    if (err) {
+        console.trace("Oops, something bad happened: %s", err.message);
+        mission.client().stop();
+        mission.client().land();
+    } else {
+        console.log("We are done!");
+        process.exit(0);
+    }
+});
+

tests/integration/mock/client.js

@@ -20,6 +20,14 @@ function Client(options) {
     }, DT);
 }
 
+Client.prototype.takeoff = function(callback) {
+    setTimeout(callback, 1000);
+}
+
+Client.prototype.land = function(callback) {
+    setTimeout(callback, 1000);
+}
+
 Client.prototype.front = function(speed) {
     this._speed.vx = speed;
 }

tests/integration/simple.js

@@ -1,18 +0,0 @@
-// TODO This is broken. Need to write real tests.
-console.log("The test is broken. Will have to write proper tests :-)");
-
-var autonomy    = require('..');
-var client      = require('./mock/client');
-
-var controller  = new autonomy.Controller(new client(), {state: {x: 0, y:0, z:1, yaw: 0}});
-
-console.log("State: %j", controller.state());
-
-controller.on('controlData', function(data) {
-    console.log("%j", data);
-});
-
-controller.go({x: 1, y: 1}, function(state) {
-    console.log("Reached state %j", state);
-    controller.disable();
-});

tests/unit/test-Controller.js

@@ -115,5 +115,57 @@ test('Controller', {
 
         ctrl.zero();
         assert(ctrl._ekf.reset.calledOnce);
+    },
+
+    'up': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        var state = {x: 1, y: 2, z: 3, yaw: 0};
+
+        ctrl._state = state;
+        ctrl.up(1);
+        var goal = ctrl._goal;
+
+        assert.equal(goal.x, state.x);
+        assert.equal(goal.y, state.y);
+        assert.equal(goal.z, state.z + 1);
+        assert.equal(goal.yaw, state.yaw);
+    },
+
+    'down is invserse of up': function() {
+        var ctrl    = new autonomy.Controller(this.mockClient);
+        var cb      = function() {};
+        ctrl._state = {x: 0, y: 0, z: 1, yaw: 0};
+        sinon.spy(ctrl, 'up');
+
+        ctrl.down(1, cb);
+        assert(ctrl.up.calledWith(-1, cb));
+    },
+
+    'cannot go too low': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        var state = {x: 0, y: 0, z: 1, yaw: 0};
+
+        ctrl._state = state;
+        ctrl.down(1);
+        var goal = ctrl._goal;
+
+        assert.equal(goal.x, state.x);
+        assert.equal(goal.y, state.y);
+        assert.equal(goal.z, 0.5);
+        assert.equal(goal.yaw, state.yaw);
+    },
+
+    'altitude': function() {
+        var ctrl  = new autonomy.Controller(this.mockClient);
+        var state = {x: 0, y: 0, z: 1, yaw: 0};
+
+        ctrl._state = state;
+        ctrl.altitude(3);
+        var goal = ctrl._goal;
+
+        assert.equal(goal.x, state.x);
+        assert.equal(goal.y, state.y);
+        assert.equal(goal.z, 3);
+        assert.equal(goal.yaw, state.yaw);
     }
 });