Fixed null pointer in tag detection

examples/repl.js

@@ -1,13 +1,32 @@
 var fs = require('fs');
 var path = require('path');
-var df = require('dateformat');
-var arDrone = require('ar-drone');
-var autonomy = require('..');
+var df = require('dateformat')
+  , arDrone = require('ar-drone')
+  , arDroneConstants = require('ar-drone/lib/constants')
+  , autonomy = require('..');
 
 var client  = arDrone.createClient();
 var ctrl    = new autonomy.Controller(client, {debug: false});
 var repl    = client.createRepl();
 
+function navdata_option_mask(c) {
+  return 1 << c;
+}
+
+// From the SDK.
+var navdata_options = (
+    navdata_option_mask(arDroneConstants.options.DEMO)
+  | navdata_option_mask(arDroneConstants.options.VISION_DETECT)
+  | navdata_option_mask(arDroneConstants.options.MAGNETO)
+  | navdata_option_mask(arDroneConstants.options.WIFI)
+);
+
+// Connect and configure the drone
+client.config('general:navdata_demo', true);
+client.config('general:navdata_options', navdata_options);
+client.config('video:video_channel', 1);
+client.config('detect:detect_type', 12);
+
 // Add a ctrl object to the repl. You can use the controller
 // from there. E.g.
 // ctrl.go({x:1, y:1});
@@ -15,16 +34,31 @@ var repl    = client.createRepl();
 repl._repl.context['ctrl'] = ctrl;
 
 // Log navdata and estimated state - for debugging
-//
-// var folder = df(new Date(), "yyyy-mm-dd_hh-MM-ss");
-// fs.mkdir(path.join('/tmp', folder), function() {
-//    navStream = fs.createWriteStream(path.join('/tmp', folder, 'navdata.txt'));
-//    stateStream = fs.createWriteStream(path.join('/tmp', folder, 'state.txt'));
-// });
+var folder = df(new Date(), "yyyy-mm-dd_hh-MM-ss");
+fs.mkdir(path.join('/tmp', folder), function() {
+  dataStream = fs.createWriteStream(path.join('/tmp', folder, 'data.txt'));
+});
 
-//client.on('navdata', function(data) {
-//    navStream.write(JSON.stringify(data) + "\n");
-//    stateStream.write(JSON.stringify(ctrl.state()) + "\n");
-//});
+client.on('navdata', function(data) {
+    var state = ctrl.state()
+      , cmds  = ctrl.commands()
+      , vx    = data.demo.velocity.x / 1000
+      , vy    = data.demo.velocity.y / 1000
+      , vz    = data.demo.velocity.z / 1000
+      , x     = state.x
+      , y     = state.y
+      , z     = state.z
+      , yaw   = state.yaw
+      , cx    = cmds.cx
+      , cy    = cmds.cy
+      , cz    = cmds.cz
+      , cyaw  = cmds.cyaw
+      , tag   = (data.visionDetect && data.visionDetect.nbDetected > 0)
+      ;
+
+  dataStream.write(x + "," + y + "," + z + "," + yaw + ","
+                   + vx + "," + vy + "," + vz +"," + cx + ","
+                   + cy + "," + cz + "," + cyaw + "," + tag + "\n");
+});
 
 

lib/Controller.js

@@ -23,6 +23,7 @@ function Controller(client, options) {
     this._enabled = false;
     this._client  = client;
     this._goal    = null;
+    this._cmds    = {cx: 0, cy: 0, cz: 0, cyaw: 0};
 
     var self = this;
     client.on('navdata', function(d) {
@@ -56,6 +57,10 @@ Controller.prototype.state = function() {
     return this._state;
 }
 
+Controller.prototype.commands = function() {
+    return this._cmds;
+}
+
 /*
  * Sets a new goal and enable the controller. When the goal
  * is reached, the callback is called with the current state.
@@ -105,7 +110,7 @@ Controller.prototype._processNavdata = function(d) {
         // back-projecting the pixel position p(x,y) to the drone
         // coordinate system P(X,Y).
         // TODO: Should we use dist or the measure altitude ?
-        var measured = camera.p2m(xc + wc/2, yc + hc/2, dist);
+        var measured = this._camera.p2m(xc + wc/2, yc + hc/2, dist);
 
         // Rotation is provided by the drone, we convert to radians
         measured.yaw = yaw.toRad();
@@ -192,6 +197,8 @@ Controller.prototype._control = function() {
         }
     }
 
+    this._cmds = {cx: cx, cy: cy, cz: cz, cyaw: cyaw};
+
     if (this._debug) {
         console.log("--------------------- Control step ----------------------------------------------");
         console.log("Goal: \t %d,%d,%d,%d", this._goal.x, this._goal.y, this._goal.z, this._goal.yaw);