software/js-drone: use same logic as the simulator

Use an update loop to decide the instructions to give to the autopilot.

software/js-drone/README.md

@@ -2,7 +2,7 @@
 
 ## Presentation ##
 
-* Deploy `main.js` script on a drone to fly it
+* Deploy `user.js` script on a drone to fly it
 
 * Compile all required libraries to run flight scripts
 
@@ -26,4 +26,4 @@
 
 ## How it works ##
 
-Run `quickjs binary location` `scripts location`/main.js
+Run `quickjs binary location` `scripts location`/main.js `scripts location`/user.js

software/js-drone/buildout.hash.cfg

@@ -14,12 +14,16 @@
 # not need these here).
 [instance-profile]
 filename = instance.cfg
-md5sum = ae1ccd9272303ee0102cdcec8ddef562
+md5sum = 6529287d0d5438aa6be0fc58602e996f
 
 [main]
 filename = main.js
-md5sum = 195c4ba934fb05f46dec18a5a132c450
+md5sum = b5814f9c0985d7b06c77637baf30a124
 
 [pubsub]
 filename = pubsub.js
 md5sum = d8798c3206f129e8715afd3ca23afa1a
+
+[worker]
+filename = worker.js
+md5sum = 4d0ec5e300385bcd3078724ddd8993b4

software/js-drone/instance-input-schema.json

@@ -22,12 +22,6 @@
       "type": "boolean",
       "default": false
     },
-    "leader-id": {
-      "title": "Leader ID",
-      "description": "Unique identifier of the drone chosen to be the leader.",
-      "type": "integer",
-      "default": 1
-    },
     "multicast-ipv6": {
       "title": "IP of the multicast group",
       "description": "IP address used to communicate with the other drones.",

software/js-drone/instance.cfg

 [buildout]
 parts =
   main
+  user
 
 eggs-directory = ${buildout:eggs-directory}
 develop-eggs-directory = ${buildout:develop-eggs-directory}
@@ -37,15 +38,20 @@ init =
 
   subscription_script = '''
   me.onStart = function() {
-    const f = std.fdopen(std.in, "r");
+    me.f = me.fdopen(me.in, "r");
     console.log("Use q to quit");
   };
 
-  me.onUpdate= function() {
-    while(f.getline() != "q") {
+  me.onUpdate= function(timestamp) {
+    while(me.f.getline() != "q") {
       continue;
     }
-    f.close();
+    try {
+      me.f.close();;
+    } catch (error) {
+      console.error(error);
+    }
+    me.exit(0);
   };
   '''
 
@@ -60,22 +66,34 @@ context =
   raw qjs_wrapper ${qjs-wrapper:location}/lib/libqjswrapper.so
   $${:extra-context}
 
+[main]
+<= js-dynamic-template
+extra-context =
+  key autopilot_ip drone:autopilot-ip
+  key id drone:id
+  key leader_id drone:leader-id
+  key is_leader drone:is-leader
+  key is_a_simulation drone:is-a-simulation
+  key is_publisher drone:is-publisher
+  key log_dir directory:log
+  key pubsub_script pubsub:rendered
+  key worker_script worker:rendered
+
 [pubsub]
 <= js-dynamic-template
 extra-context =
   key ipv6 drone:multicast-ipv6
   key net_if drone:net-if
 
-[main]
+[user]
+recipe = slapos.recipe.template:jinja2
+output = $${directory:etc}/user.js
+context =
+  key script drone:flight-script
+inline = {{ script }}
+
+[worker]
 <= js-dynamic-template
 extra-context =
-  key autopilot_ip drone:autopilot-ip
   key drone_id_list drone:drone-id-list
-  key flight_script drone:flight-script
   key id drone:id
-  key is_a_simulation drone:is-a-simulation
-  key is_leader drone:is-leader
-  key is_publisher drone:is-publisher
-  key leader_id drone:leader-id
-  key log_dir directory:log
-  key pubsub_script pubsub:rendered

software/js-drone/main.js

+/*global console*/
 import {
   arm,
-  doParachute,
-  getAltitude,
-  getAltitudeRel,
-  getInitialAltitude,
-  getLatitude,
-  getLongitude,
-  getYaw,
-  initPubsub,
-  landed,
-  loiter,
-  reboot,
-  setAirspeed,
-  setAltitude,
-  setCheckpoint,
-  setTargetCoordinates,
   start,
   stop,
   stopPubsub,
-  takeOffAndWait,
-  Drone
+  takeOffAndWait
 } from "{{ qjs_wrapper }}";
-import {sleep, Worker} from "os";
-import * as std from "std";
-
-const IP = "{{ autopilot_ip }}";
-const URL = "udp://" + IP + ":7909";
-const LOG_FILE = "{{ log_dir }}/mavsdk-log";
-
-const IS_LEADER = {{ 'true' if is_leader else 'false' }};
-const LEADER_ID = {{ leader_id }};
-const IS_PUBLISHER = {{ 'true' if is_publisher else 'false' }}
-const SIMULATION = {{ 'true' if is_a_simulation else 'false' }};
-
-const droneIdList = {{ drone_id_list }};
-const droneDict = {};
-
-const pubsubScript = "{{ pubsub_script }}";
-var pubsubWorker;
-var pubsubRunning = false;
-
-const me = {
-  'id': "{{ id }}",
-  'getCurrentPosition': function() {
-    return {
-      'x': getLatitude(),
-      'y': getLongitude(),
-      'z': getAltitudeRel()
-    };
-  },
-  'onStart': function() {},
-  'onUpdate': function() {},
-  'setAirspeed': setAirspeed,
-  'setTargetCoordinates': setTargetCoordinates
-}
-
-function connect() {
+import { setTimeout, Worker } from "os";
+import { exit } from "std";
+
+(function (console, setTimeout, Worker) {
+  "use strict";
+  const IP = "{{ autopilot_ip }}",
+    URL = "udp://" + IP + ":7909",
+    LOG_FILE = "{{ log_dir }}/mavsdk-log",
+    IS_PUBLISHER = {{ 'true' if is_publisher else 'false' }},
+    SIMULATION = {{ 'true' if is_a_simulation else 'false' }};
+
+  // Use a Worker to ensure the user script
+  // does not block the main script
+  // (preventing it to be stopped for example)
+
+  // Create the update loop in the main script
+  // to prevent it to finish (and so, exit the quickjs process)
+  var pubsubWorker,
+    pubsubRunning = false,
+    worker = new Worker("{{ worker_script }}"),
+    user_script = scriptArgs[1],
+    // Use the same FPS than browser's requestAnimationFrame
+    FPS = 1000 / 60,
+    previous_timestamp,
+    can_update = false,
+    i = 0;
+
+  function connect() {
     console.log("Will connect to", URL);
     exit_on_fail(start(URL, LOG_FILE, 60), "Failed to connect to " + URL);
-}
+  }
 
-function exit_on_fail(ret, msg) {
-  if(ret) {
+  function exit_on_fail(ret, msg) {
+    if (ret) {
       console.log(msg);
-    quit();
-    std.exit(-1);
+      quit(1);
+    }
   }
-}
 
-function quit() {
+  function quit(exit_code) {
     stop();
     if(pubsubRunning) {
       stopPubsub();
     }
-}
-
-function takeOff() {
-  exit_on_fail(arm(), "Failed to arm");
-  takeOffAndWait();
-}
-
-function waitForLanding() {
-  while(!landed()) {
-    sleep(1000);
+    exit(exit_code);
   }
-}
 
-if(IS_PUBLISHER) {
+  if (IS_PUBLISHER) {
     console.log("Connecting to aupilot\n");
     connect();
-}
+  }
 
-pubsubWorker = new Worker(pubsubScript);
-pubsubWorker.onmessage = function(e) {
+  pubsubWorker = new Worker("{{ pubsub_script }}");
+  pubsubWorker.onmessage = function(e) {
     if (!e.data.publishing)
       pubsubWorker.onmessage = null;
-}
-
-initPubsub(droneIdList.length);
-for (let i = 0; i < droneIdList.length; i++) {
-  let id = droneIdList[i]
-  droneDict[id] = new Drone(id);
-  droneDict[id].init(i);
-}
+  }
 
-pubsubWorker.postMessage({ action: "run", id: me.id, publish: IS_PUBLISHER });
-pubsubRunning = true;
+  worker.postMessage({type: "initPubsub"});
 
-{{ flight_script }}
+  function takeOff() {
+    exit_on_fail(arm(), "Failed to arm");
+    takeOffAndWait();
+  }
 
-if(IS_PUBLISHER && SIMULATION) {
+  function load() {
+    if(IS_PUBLISHER && SIMULATION) {
       takeOff();
-}
+    }
+
+    // First argument must provide the user script path
+    if (user_script === undefined) {
+      console.log('Please provide the user_script path.');
+      quit(1);
+    }
 
-me.onStart()
-me.onUpdate();
+    worker.postMessage({
+      type: "load",
+      path: user_script
+    });
+  }
 
-if(IS_PUBLISHER) {
-  waitForLanding();
-  quit();
-} else {
+  function loop() {
+    var timestamp = Date.now(),
+      timeout;
+    if (can_update) {
+      if (FPS <= (timestamp - previous_timestamp)) {
+        // Expected timeout between every update
+        can_update = false;
+        worker.postMessage({
+          type: "update",
+          timestamp: timestamp
+        });
+        // Try to stick to the expected FPS
+        timeout = Math.min(FPS, FPS - (timestamp - previous_timestamp - FPS));
+        previous_timestamp = timestamp;
+      } else {
+        timeout = FPS - (timestamp - previous_timestamp);
+      }
+    } else {
+      // If timeout occurs, but update is not yet finished
+      // wait a bit
+      timeout = FPS / 4;
+    }
+    // Ensure loop is not done with timeout < 1
+    // Otherwise, it will goes crazy for 1 second
+    setTimeout(loop, Math.max(1, timeout));
+  }
+
+  worker.onmessage = function (e) {
+    var type = e.data.type;
+    if (type === 'initialized') {
+      pubsubWorker.postMessage({ action: "run", id: {{ id }}, publish: IS_PUBLISHER });
+      pubsubRunning = true;
+      load();
+    } else if (type === 'loaded') {
+      previous_timestamp = -FPS;
+      can_update = true;
+      // Start the update loop
+      loop();
+    } else if (type === 'updated') {
+      can_update = true;
+    } else if (type === 'exited') {
+      worker.onmessage = null;
+      if(IS_PUBLISHER) {
+        quit(e.data.exit);
+      } else {
         stopPubsub();
-};
+        exit(e.data.exit);
+      };
+    } else {
+      console.log('Unsupported message type', type);
+      quit(1);
+    }
+  };
+}(console, setTimeout, Worker));

software/js-drone/software.cfg

@@ -8,6 +8,7 @@ parts =
     instance-profile
     main
     pubsub
+    worker
     slapos-cookbook
 
 [download-file-base]
@@ -25,3 +26,6 @@ output = ${buildout:directory}/template.cfg
 
 [pubsub]
 <= download-file-base
+
+[worker]
+<= download-file-base

software/js-drone/worker.js

+/*global console*/
+import {
+  Drone,
+  doParachute,
+  getAltitude,
+  getAltitudeRel,
+  getInitialAltitude,
+  getLatitude,
+  getLongitude,
+  getYaw,
+  initPubsub,
+  landed,
+  loiter,
+  setAirspeed,
+  setAltitude,
+  setCheckpoint,
+  setTargetCoordinates
+} from "{{ qjs_wrapper }}";
+import { Worker } from "os"
+import * as std from "std";
+
+(function (console, Worker) {
+  // Every script is evaluated per drone
+  "use strict";
+  const drone_dict = {},
+    drone_id_list = {{ drone_id_list }};
+
+  var parent = Worker.parent,
+    user_me = {
+      //for debugging purpose
+      fdopen: std.fdopen,
+      in: std.in,
+      //to move into user script
+      setCheckpoint: setCheckpoint,
+      //required to fly
+      doParachute: doParachute,
+      drone_dict: {},
+      exit: function(exit_code) {
+        parent.postMessage({type: "exited", exit: exit_code});
+        parent.onmessage = null;
+      },
+      getAltitudeAbs: getAltitude,
+      getCurrentPosition: function() {
+        return {
+          x: getLatitude(),
+          y: getLongitude(),
+          z: getAltitudeRel()
+        };
+      },
+      getInitialAltitude: getInitialAltitude,
+      getYaw: getYaw,
+      id: {{ id }},
+      landed: landed,
+      loiter: loiter,
+      setAirspeed: setAirspeed,
+      setAltitude: setAltitude,
+      setTargetCoordinates: setTargetCoordinates
+    };
+
+  function loadUserScript(path) {
+    var script_content = std.loadFile(path);
+    if (script_content === null) {
+      console.log('Failed to load user script ' + path);
+      std.exit(1);
+    }
+    try {
+      std.evalScript(
+        'function execUserScript(from, me) {' +
+          script_content +
+        '};'
+      );
+    } catch (e) {
+      console.log('Failed to evaluate user script', e);
+      std.exit(1);
+    }
+    execUserScript(null, user_me);
+
+    // Call the drone onStart function
+    if (user_me.hasOwnProperty('onStart')) {
+      user_me.onStart();
+    }
+  }
+
+  function handleMainMessage(evt) {
+    var type = evt.data.type;
+
+    if (type === 'initPubsub') {
+      initPubsub(drone_id_list.length);
+      for (let i = 0; i < drone_id_list.length; i++) {
+        let id = drone_id_list[i];
+        user_me.drone_dict[id] = new Drone(id);
+        user_me.drone_dict[id].init(i);
+      }
+      parent.postMessage({type: "initialized"});
+    }
+    else if (type === 'load') {
+      loadUserScript(evt.data.path);
+      parent.postMessage({type: "loaded"});
+    } else if (type === 'update') {
+      // Call the drone onStart function
+      if (user_me.hasOwnProperty('onUpdate')) {
+        user_me.onUpdate(evt.data.timestamp);
+      }
+      parent.postMessage({type: "updated"});
+    } else {
+      throw new Error('Unsupported message type', type);
+    }
+  }
+
+  parent.onmessage = function (evt) {
+    try {
+      handleMainMessage(evt);
+    } catch (error) {
+      // Catch all potential bug to exit the main process
+      // if it occurs
+      console.log(error);
+      std.exit(1);
+    }
+  };
+
+}(console, Worker));