move offboard commands to a separate thread so that they can be inter…

…rupted by rpc client and so that

rpc client can call them repeatedly at high speed (like moveByVelocity) efficiently.

AirLib/AirLib.vcxproj

@@ -41,6 +41,7 @@
     <ClInclude Include="include\common\common_utils\Timer.hpp" />
     <ClInclude Include="include\common\common_utils\type_utils.hpp" />
     <ClInclude Include="include\common\common_utils\Utils.hpp" />
+    <ClInclude Include="include\common\common_utils\WorkerThread.hpp" />
     <ClInclude Include="include\common\DelayLine.hpp" />
     <ClInclude Include="include\common\EarthUtils.hpp" />
     <ClInclude Include="include\common\FirstOrderFilter.hpp" />

AirLib/AirLib.vcxproj.filters

@@ -330,6 +330,9 @@
     <ClInclude Include="include\common\common_utils\Timer.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\common\common_utils\WorkerThread.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\controllers\DroneControllerBase.cpp">

AirLib/include/common/common_utils/WorkerThread.hpp

@@ -0,0 +1,233 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef commn_utils_WorkerThread_hpp
+#define commn_utils_WorkerThread_hpp
+
+#include <functional>
+#include <thread>
+#include <atomic>
+#include <vector>
+#include <memory>
+#include <exception>
+#include <future>
+#include <mutex>
+#include "Utils.hpp"
+#include "Timer.hpp"
+
+namespace msr {
+    namespace airlib {
+
+        class CancelableBase {
+        protected:
+            std::atomic<bool> is_cancelled_;
+            std::atomic<bool> is_complete_;
+        public:
+            CancelableBase() : is_cancelled_(false), is_complete_(false) {
+            }
+            bool isCancelled() {
+                return is_cancelled_;
+            }
+
+            void cancel() {
+                is_cancelled_ = true;
+            }
+
+            virtual void execute() = 0;
+
+            bool sleep(double secs)
+            {
+                //We can pass duration directly to sleep_for however it is known that on 
+                //some systems, sleep_for makes system call anyway even if passed duration 
+                //is <= 0. This can cause 50us of delay due to context switch.
+                if (isCancelled()) {
+                    return false;
+                }
+
+                common_utils::Timer timer;
+                timer.start();
+
+                while(secs > 0 && !isCancelled() && !is_complete_ && timer.seconds() < secs)
+                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+                return !isCancelled();
+            }
+
+            void complete() {
+                is_complete_ = true;
+            }
+
+            bool isComplete() {
+                return is_complete_;
+            }
+
+        };
+
+        // This wraps a condition_variable so we can handle the case where we may signal before wait
+        // and implement the semantics that say wait should be a noop in that case.
+        class WorkerThreadSignal
+        {
+            std::condition_variable cv_;
+            std::mutex mutex_;
+            std::atomic<bool> signaled_;
+        public:
+            WorkerThreadSignal() : signaled_(false) {
+            }
+            void signal() {
+                {
+                    std::unique_lock<std::mutex> lock(mutex_);
+                    signaled_ = true;
+                }
+                cv_.notify_one();
+            }
+
+            template<class _Predicate>
+            void wait(_Predicate cancel)
+            {
+                // wait for signal or cancel predicate
+                std::unique_lock<std::mutex> lock(mutex_);
+                while (!signaled_) {
+                    cv_.wait_for(lock, std::chrono::milliseconds(1), [this, cancel] {
+                        return cancel();
+                    });
+                }
+                signaled_ = false;
+            }
+
+        };
+
+        // This class provides a synchronized worker thread that guarantees to execute
+        // cancelable tasks on a background thread one at a time.
+        // It guarantees that previous task is cancelled before new task is started.
+        // The queue size is 1, which means it does NOT guarantee all queued tasks are executed.
+        // If enqueue is called very quickly the thread will not have a chance to execute each
+        // task before they get cancelled, worst case in a tight loop all tasks are starved and
+        // nothing executes. 
+        class WorkerThread {
+        public:
+            WorkerThread()
+            : thread_running_(false), thread_stopped_(false) {
+            }
+
+            ~WorkerThread() {
+                thread_stopped_ = true;
+                cancel();
+            }
+            void enqueue(std::shared_ptr<CancelableBase> item) {
+                {
+                    std::unique_lock<std::mutex> lock(mutex_);
+                    std::shared_ptr<CancelableBase> pending = pending_;
+                    if (pending != nullptr) {
+                        pending->cancel();
+                    }
+                }
+
+                bool running = false;
+                {
+                    std::unique_lock<std::mutex> lock(mutex_);
+                    pending_ = item;
+                    running = thread_running_;
+                }
+                if (running) {
+                    thread_waiting_.signal();
+                }
+                else {
+                    start();
+                }
+            }
+
+            bool enqueueAndWait(std::shared_ptr<CancelableBase> item, float max_wait_seconds) {
+                {
+                    std::unique_lock<std::mutex> lock(mutex_);
+                    std::shared_ptr<CancelableBase> pending = pending_;
+                    if (pending != nullptr) {
+                        pending->cancel();
+                    }
+                }
+
+                bool running = false;
+                {
+                    std::unique_lock<std::mutex> lock(mutex_);
+                    pending_ = item;
+                    running = thread_running_;
+                }
+                if (running) {
+                    thread_waiting_.signal();
+                }
+                else {
+                    start();
+                }
+
+                item->sleep(max_wait_seconds);
+                return !item->isCancelled();
+            }
+            void cancel() {
+                std::unique_lock<std::mutex> lock(mutex_);
+                std::shared_ptr<CancelableBase> pending = pending_;
+                pending_ = nullptr;
+                if (pending != nullptr) {
+                    pending->cancel();
+                }
+                if (thread_.joinable()) {
+                    thread_waiting_.signal();
+                    thread_.join();
+                }
+            }
+        private:
+            void start() {
+                if (!thread_running_) {
+                    {
+                        std::unique_lock<std::mutex> lock(mutex_);
+                        if (thread_.joinable()) {
+                            thread_.join();
+                        }
+                    }
+                    thread_stopped_ = false;
+                    thread_ = std::thread(&WorkerThread::run, this);
+                    thread_started_.wait([this] { return static_cast<bool>(thread_stopped_); });
+                }
+            }
+            void run() {
+                thread_running_ = true;
+                {
+                    std::unique_lock<std::mutex> lock(mutex_);
+                    thread_started_.signal();
+                }
+                while (!thread_stopped_ && pending_ != nullptr) {
+                    std::shared_ptr<CancelableBase> pending;
+                    {
+                        std::unique_lock<std::mutex> lock(mutex_);
+                        pending = pending_;
+                    }
+                    if (pending != nullptr && !pending->isCancelled()) {
+                        try {
+                            pending->execute();
+                        }
+                        catch (std::exception& e) {
+                            common_utils::Utils::logMessage("WorkerThread caught unhandled exception: %s", e.what());
+                        }
+                        // we use cancel here to communicate to enqueueAndWait that the task is complete.
+                        pending->complete();
+                    }
+
+                    if (!thread_stopped_) {
+                        thread_waiting_.wait([this] { return static_cast<bool>(thread_stopped_); });
+                    }
+                }
+                thread_running_ = false;
+            }
+        private:
+            WorkerThreadSignal thread_started_;
+            WorkerThreadSignal thread_waiting_;
+
+            // thread state
+            std::shared_ptr<CancelableBase> pending_;
+            std::mutex mutex_;
+            std::thread thread_;
+            std::atomic<bool> thread_running_;
+            std::atomic<bool> thread_stopped_;
+        };
+
+    }
+}
+#endif

AirLib/include/controllers/DroneControllerBase.hpp

@@ -11,6 +11,7 @@
 #include "common/CommonStructs.hpp"
 #include "VehicleControllerBase.hpp"
 #include "DroneCommon.hpp"
+#include "common/common_utils/WorkerThread.hpp"
 
 namespace msr { namespace airlib {
 
@@ -69,6 +70,11 @@ class DroneControllerBase : public VehicleControllerBase {
     };
     typedef common_utils::EnumFlags<ImageType>  ImageTypeFlags;
 
+    enum class LandedState : uint {
+        Landed = 0,
+        Flying = 1
+    };
+
 public: //interface for outside world
     /// The drone must be armed before it will fly.  Set arm to true to arm the drone.  
     /// On some drones arming may cause the motors to spin on low throttle, this is normal.
@@ -80,15 +86,19 @@ class DroneControllerBase : public VehicleControllerBase {
 
     /// When armed you can tell the drone to takeoff.  This will fly to a preset altitude (like 2.5 meters)
     /// above the home position.  Once the drone is safely in the air you can use other commands to fly from there.
-    /// If the drone is already flying takeoff will be ignored.
+    /// If the drone is already flying takeoff will be ignored.  Pass non-zer max_wait_seconds if you want the
+    /// method to also wait until the takeoff altitude is achieved.
     virtual bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) = 0;
 
     /// At any point this command will disable offboard control and land the drone at the current GPS location.
     /// How quickly the drone descends is up to the drone.  Some models will descend slowly if they have no 
     /// lidar telling them how far it is to the ground, while others that can see the ground will descend more
     /// quickly until they get near the ground.  None of that behavior is defined in this API because it is 
-    /// depends on what kind of hardware the drone has onboard.
-    virtual bool land(CancelableBase& cancelable_action) = 0;
+    /// depends on what kind of hardware the drone has onboard.  Pass non-zer max_wait_seconds if you want the
+    /// method to also wait until the drone reports it has landed, the timeout here is a bit tricky, depends
+    /// on how high you are and what the drone's configured descent velocity is.  If you don't want to wait
+    /// pass zero.  You can also periodically check getLandedState to see if it has landed.
+    virtual bool land(float max_wait_seconds, CancelableBase& cancelable_action) = 0;
 
     /// This command is a safety measure, at any point this command will cancel offboard control and send the
     /// drone back to the launch point (or home position).  Most drones are also configured to climb to a safe
@@ -154,7 +164,7 @@ class DroneControllerBase : public VehicleControllerBase {
     virtual bool moveToZ(float z, float velocity, const YawMode& yaw_mode,
         float lookahead, float adaptive_lookahead, CancelableBase& cancelable_action);
 
-    virtual bool moveByManual(float vx_max, float vy_max, float z_min, DrivetrainType drivetrain, const YawMode& yaw_mode, float duration, CancelableBase& cancelable_action);
+    virtual bool moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode, CancelableBase& cancelable_action);
 
     /// Rotate the drone to the specified fixed heading (yaw) while remaining stationery at the current x, y, and z.
     virtual bool rotateToYaw(float yaw, float margin, CancelableBase& cancelable_action);
@@ -186,6 +196,9 @@ class DroneControllerBase : public VehicleControllerBase {
     /// Get the current orientation (or attitude) of the drone as a Quaternion.
     virtual Quaternionr getOrientation() = 0;
 
+    /// Get current state of the drone, is it landed or in the air
+    virtual LandedState getLandedState() = 0;
+
     /// Assigned remote control to use for this controller, 
     /// -1 = onboard RC, 0+ = joystick ID available on OS
     virtual int getRemoteControlID() { return -1; }