Provide check and landing points through operator

multicopter_swarm.js

@@ -14,52 +14,8 @@
 
   var ALTITUDE_DIFF = 5,
     BASE_ALTITUDE = 25,
-    CHECKPOINT_LIST = [
-      {
-        latitude: 45.310770,
-        longitude: 5.950148
-      },
-      {
-        latitude: 45.311229,
-        longitude: 5.949820
-      },
-      {
-        latitude: 45.311246,
-        longitude: 5.950584
-      },
-      {
-        latitude: 45.310785,
-        longitude: 5.950877
-      }
-    ],
     EPSILON = 4,
-    PARACHUTE_POINT_ARRAY = [
-      {
-        latitude: 45.310479,
-        longitude: 5.950495
-      },
-      {
-        latitude: 45.310393,
-        longitude: 5.950677
-      },
-      {
-        latitude: 45.310376,
-        longitude: 5.950295
-      },
-      {
-        latitude: 45.310304,
-        longitude: 5.950459
-      },
-      {
-        latitude: 45.310235,
-        longitude: 5.950608
-      }
-    ],
     TARGETED_DISTANCE = 10,
-    TAKE_OFF_POINT = {
-      latitude: 45.310479,
-      longitude: 5.950495
-    },
     MIN_CMD_INTERVAL = 500,
     MIN_SPEED = 4,
     DEFAULT_SPEED = 5,
@@ -119,6 +75,14 @@
     }
   }
 
+  function communicate(drone, okState, state) {
+    var infos = {id: drone.id, okState: okState, state: state};
+    if (okState) {
+      infos.timestamp = Date.now();
+    }
+    return drone.sendMsg(JSON.stringify(infos));
+  }
+
   me.onStart = function () {
     me.direction_set = false;
     me.flightAltitude = BASE_ALTITUDE + me.id * ALTITUDE_DIFF;
@@ -126,9 +90,9 @@
     me.going_to_parachute = false;
     me.id_list = [me.id];
     me.timestamp_list = [0];
-    me.id_list.sort();
     me.landing = false;
     me.last_cmd_timestamp = 0;
+    me.lost_a_drone = false;
     me.next_checkpoint = 0;
     me.started = false;
     me.stopped = false;
@@ -151,58 +115,55 @@
       neighbor_position = me.getDroneDict()[neighbor_id];
     }
 
-    if (!me.path_planning){
+    if (!me.checkpoint_list) {
       if (leader_id !== me.id) {
         console.log(
           "timestamp difference",
           position.timestamp - neighbor_position.timestamp
         );
       }
-      return me.sendMsg(JSON.stringify(
-        {id: me.id, okState: true, state: "Ready"}
-      ));
+      return communicate(me, true, "Ready");
     }
 
     if (me.landing && me.isLanding()) {
-      return me.sendMsg(JSON.stringify(
-        {id: drone.id, okState: false, state: "Landing"}
-      ));
+      return communicate(me, false, "Landing");
     }
 
     if (!me.started) {
-      if(me.isReadyToFly()) {
+      if (me.isReadyToFly()) {
         me.started = true;
       } else {
-        return me.sendMsg(JSON.stringify(
-          {id: me.id, okState: true, state: "Taking Off", timestamp: Date.now()}
-        ));
+        return communicate(me, true, "Taking Off");
       }
     }
 
     if (!me.landing) {
-      if(!me.isReadyToFly()) {
-        return me.sendMsg(JSON.stringify(
-          {id: me.id, okState: false, state: "Emergency"}
-        ));
+      if (!me.isReadyToFly()) {
+        return communicate(me, false, "Emergency");
       }
 
       JSON.parse(JSON.stringify(me.timestamp_list)).forEach(
-        function (last_timestamp, id) {
-          if (me.id_list[id] !== me.id
+        function (last_timestamp, index) {
+          if (me.id_list[index] !== me.id
               && Math.abs(timestamp - last_timestamp) >= TIMEOUT) {
             console.log(
               "Lost drone",
-              me.id_list[id],
+              me.id_list[index],
               "timestamp difference",
               timestamp - last_timestamp
             );
-            me.id_list.splice(me.id_list.indexOf(id), 1);
-            me.timestamp_list.splice(me.id_list.indexOf(id), 1);
+            me.id_list.splice(index, 1);
+            me.timestamp_list.splice(index, 1);
+            me.lost_a_drone = true;
           }
         }
       );
 
-      console.log("leader id", leader_id, "me id", me.id);
+      if (me.lost_a_drone) {
+        me.lost_a_drone = false;
+        return communicate(me, true, "Flying");
+      }
+
       if (leader_id !== me.id) {
         distance2d = distance(
           position.latitude,
@@ -215,17 +176,15 @@
           distanceToTakeOffPoint = distance(
             neighbor_position.latitude,
             neighbor_position.longitude,
-            TAKE_OFF_POINT.latitude,
-            TAKE_OFF_POINT.longitude
+            me.landing_point_list[0].latitude,
+            me.landing_point_list[0].longitude
           );
           console.log(
             "Distance from neighbor to takeoff point",
             distanceToTakeOffPoint
           );
           if (distanceToTakeOffPoint < 2 * TARGETED_DISTANCE) {
-            return me.sendMsg(JSON.stringify(
-              {id: me.id, okState: true, state: "Waiting", timestamp: Date.now()}
-            ));
+            return communicate(me, true, "Waiting");
           }
           me.following = true;
         }
@@ -245,28 +204,26 @@
         );
 
         console.log("distance to leader", distance2d);
-        return me.sendMsg(JSON.stringify(
-          {id: me.id, okState: true, state: "Flying", timestamp: Date.now()}
-        ));
+        return communicate(me, true, "Flying");
       }
 
       checkpointIndex = (!me.reverse) ? me.next_checkpoint
-        : CHECKPOINT_LIST.length - me.next_checkpoint - 1;
+        : me.checkpoint_list.length - me.next_checkpoint - 1;
       if (!me.direction_set) {
         console.log("Going to Checkpoint", checkpointIndex);
         me.next_point = {
-          "latitude": CHECKPOINT_LIST[checkpointIndex].latitude,
-          "longitude": CHECKPOINT_LIST[checkpointIndex].longitude
+          "latitude": me.checkpoint_list[checkpointIndex].latitude,
+          "longitude": me.checkpoint_list[checkpointIndex].longitude
         };
         me.direction_set = true;
       }
 
       if (
-        pointReached(me, CHECKPOINT_LIST[checkpointIndex], EPSILON)
+        pointReached(me, me.checkpoint_list[checkpointIndex], EPSILON)
       ) {
         console.log("Reached Checkpoint", checkpointIndex);
         me.next_checkpoint += 1;
-        me.next_checkpoint %= CHECKPOINT_LIST.length;
+        me.next_checkpoint %= me.checkpoint_list.length;
         me.direction_set = false;
         return me.sendMsg(JSON.stringify({
           id: me.id,
@@ -279,9 +236,9 @@
     }
 
     if (me.going_to_parachute &&
-        pointReached(me, PARACHUTE_POINT_ARRAY[me.id], EPSILON)) {
+        pointReached(me, me.landing_point_list[me.id], EPSILON)) {
       me.going_to_parachute = false;
-      return exitOnFail(drone.land(), "Failed to land");
+      return exitOnFail(me.land(), "Failed to land");
     }
 
     if (!me.landing || me.going_to_parachute) {
@@ -293,16 +250,19 @@
         Date.now()
       );
     }
-    return me.sendMsg(JSON.stringify(
-      {id: me.id, okState: true, state: "Flying", timestamp: Date.now()}
-    ));
+
+    if (me.landing) {
+      return communicate(me, false, "Landed");
+    } else {
+      return communicate(me, true, "Flying");
+    }
   };
 
   me.onGetMsg = function (msg) {
-    if (msg.hasOwnProperty("path_planning")) {
-      me.path_planning = true;
+    if (msg.hasOwnProperty("checkpoint_list")) {
+      me.checkpoint_list = msg.checkpoint_list;
+      me.landing_point_list = msg.landing_point_list;
       console.log("running");
-      me.timestamp_list = Array.apply(null, Array(me.id_list.length)).map(function () { return Date.now() });
       return me.takeOff();
     }
 
@@ -313,12 +273,13 @@
 
       case "stopped":
         me.stopped = true;
-        if (me.id === (!me.reverse? me.id_list[me.id_list.length - 1] : me.id_list[0])) {
+        if (me.id ===
+            (!me.reverse ? me.id_list[me.id_list.length - 1] : me.id_list[0])) {
           me.landing = true;
           me.going_to_parachute = true;
           me.next_point = {
-            "latitude": PARACHUTE_POINT_ARRAY[me.id].latitude,
-            "longitude": PARACHUTE_POINT_ARRAY[me.id].longitude
+            "latitude": me.landing_point_list[me.id].latitude,
+            "longitude": me.landing_point_list[me.id].longitude
           };
           console.log("Going to rally");
         }
@@ -340,21 +301,23 @@
 
       case "Ready":
         if (!me.id_list.includes(msgDict.id)) {
-          me.id_list[me.id_list.length - 1] = msgDict.id;
+          me.id_list[me.id_list.length] = msgDict.id;
           me.id_list.sort();
         }
         break;
 
       case "Landing":
-        if (me.id_list.includes(msgDict.id)) {
+        if (msgDict.id !== me.id && me.id_list.includes(msgDict.id)) {
           me.id_list.splice(me.id_list.indexOf(msgDict.id), 1);
-          me.timestamp_list.splice(me.id_list.indexOf(id), 1);
-          if (me.stopped && me.id === (!me.reverse? me.id_list[me.id_list.length - 1] : me.id_list[0])) {
+          me.timestamp_list.splice(me.id_list.indexOf(msgDict.id), 1);
+          if (me.stopped && me.id ===
+              (!me.reverse ? me.id_list[me.id_list.length - 1] : me.id_list[0])
+              ) {
             me.landing = true;
             me.going_to_parachute = true;
             me.next_point = {
-              "latitude": PARACHUTE_POINT_ARRAY[me.id].latitude,
-              "longitude": PARACHUTE_POINT_ARRAY[me.id].longitude
+              "latitude": me.landing_point_list[me.id].latitude,
+              "longitude": me.landing_point_list[me.id].longitude
             };
           }
         }
@@ -365,8 +328,9 @@
       }
     }
 
-    if(msgDict.hasOwnProperty("timestamp")) {
-      me.timestamp_list[msgDict.id] = msgDict.timestamp;
+    if (me.id_list.includes(msgDict.id)
+        && msgDict.hasOwnProperty("timestamp")) {
+      me.timestamp_list[me.id_list.indexOf(msgDict.id)] = msgDict.timestamp;
     }
 
     if (msgDict.hasOwnProperty("next_checkpoint")) {

operator.js

@@ -3,5 +3,53 @@
  * start button of its GUI is pressed.
 */
 
-var map = operator.getMapJSON();
-operator.sendMsg({flag_positions: map.flag_list, path_planning: ""});
+/*jslint nomen: true, indent: 2, maxerr: 3, maxlen: 80 */
+/*global operator*/
+(function (operator) {
+  "use strict";
+
+  var map = operator.getMapJSON();
+  operator.sendMsg({
+    checkpoint_list: [
+      {
+        latitude: 45.310770,
+        longitude: 5.950148
+      },
+      {
+        latitude: 45.311229,
+        longitude: 5.949820
+      },
+      {
+        latitude: 45.311246,
+        longitude: 5.950584
+      },
+      {
+        latitude: 45.310785,
+        longitude: 5.950877
+      }
+    ],
+    flag_positions: map.flag_list,
+    landing_point_list: [
+      {
+        latitude: 45.310479,
+        longitude: 5.950495
+      },
+      {
+        latitude: 45.310393,
+        longitude: 5.950677
+      },
+      {
+        latitude: 45.310376,
+        longitude: 5.950295
+      },
+      {
+        latitude: 45.310304,
+        longitude: 5.950459
+      },
+      {
+        latitude: 45.310235,
+        longitude: 5.950608
+      }
+    ]
+  });
+}(operator));