use simclock for thread sleep, avoid OS clocks

AirLib/AirLib.vcxproj

@@ -22,6 +22,7 @@
     <ClInclude Include="include\common\ClockBase.hpp" />
     <ClInclude Include="include\common\Common.hpp" />
     <ClInclude Include="include\common\CommonStructs.hpp" />
+    <ClInclude Include="include\common\ClockFactory.hpp" />
     <ClInclude Include="include\common\common_utils\ctpl_stl.h" />
     <ClInclude Include="include\common\common_utils\EnumFlags.hpp" />
     <ClInclude Include="include\common\common_utils\ExceptionUtils.hpp" />

AirLib/AirLib.vcxproj.filters

@@ -324,6 +324,9 @@
     <ClInclude Include="include\common\SimClock.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\common\ClockFactory.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\controllers\DroneControllerBase.cpp">

AirLib/include/common/ClockBase.hpp

@@ -4,21 +4,31 @@
 #ifndef airsim_core_ClockBase_hpp
 #define airsim_core_ClockBase_hpp
 
+#include <thread>
+#include <chrono>
 #include "Common.hpp"
 
 namespace msr { namespace airlib {
 
 class ClockBase {
 public:
+    //returns value indicating nanoseconds elapsed since some reference timepoint in history
+    //typically nanoseconds from Unix epoch
     virtual TTimePoint nowNanos() = 0;
+    //converts time interval for wall clock to current clock
+    //For example, if implementation is scaled clock simulating 5X spped then below
+    //will retun dt*5
+    virtual TTimeDelta fromWallDelta(TTimeDelta dt) = 0;
+    virtual TTimeDelta toWallDelta(TTimeDelta dt) = 0;
+
 
     TTimeDelta elapsedSince(TTimePoint since)
     {
         return elapsedBetween(nowNanos(), since);
     }
     static TTimeDelta elapsedBetween(TTimePoint second, TTimePoint first)
     {
-        return (second - first) / 1E9;
+        return (second - first) / 1.0E9;
     }
     TTimeDelta updateSince(TTimePoint& since)
     {
@@ -27,6 +37,31 @@ class ClockBase {
         since = cur;
         return elapsed;
     }
+
+    void sleep_for(TTimeDelta wall_clock_dt)
+    {
+        TTimeDelta dt = fromWallDelta(wall_clock_dt);
+
+        if (dt <= 0)
+            return;
+
+        //for intervals > 2ms just sleep otherwise do spilling otherwise delay won't be accurate
+        if (dt > 2.0/1000)  
+            std::this_thread::sleep_for(duration<double>(dt));
+        else {
+            static constexpr std::chrono::duration<double> MinSleepDuration(0);
+            clock::time_point start = clock::now();
+            //spin wait
+            while (duration<double>(clock::now() - start).count() < dt) {
+                std::this_thread::sleep_for(MinSleepDuration);
+            }
+        }
+    }
+
+private:
+    typedef std::chrono::high_resolution_clock clock;
+    template <typename T>
+    using duration = std::chrono::duration<T>;
 };
 
 }} //namespace

AirLib/include/common/ClockFactory.hpp

@@ -0,0 +1,30 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef airsim_core_ClockFactory_hpp
+#define airsim_core_ClockFactory_hpp
+
+#include "SimClock.hpp"
+
+namespace msr { namespace airlib {
+
+class ClockFactory {
+public:
+    static ClockBase* get()
+    {
+        static SimClock clock;
+
+        return &clock;
+    }
+
+    //don't allow multiple instances of this class
+    ClockFactory(ClockFactory const&) = delete;
+    void operator=(ClockFactory const&) = delete;
+
+private:
+    //disallow instance creation
+    ClockFactory(){}
+};
+
+}} //namespace
+#endif

AirLib/include/common/SimClock.hpp

@@ -11,10 +11,42 @@ namespace msr { namespace airlib {
 
 class SimClock : public ClockBase {
 public:
+    SimClock(double scale = 1, TTimeDelta latency = 0)
+        : scale_(scale), latency_(latency)
+    {
+        offset_ = latency * (scale_ - 1);
+    }
+
     virtual TTimePoint nowNanos() override
     {
-        return Utils::getTimeSinceEpochNanos();
+        if (offset_ == 0 && scale_ == 1) //optimized normal route
+            return Utils::getTimeSinceEpochNanos();
+        else {
+            /*
+                Apply scaling and latency.
+
+                Time point is nanoseconds from some reference r. If latency = 0 then r = 0 .
+                scaled time point is then given by (r + ((now - r) / scale)).
+                This becomes (r*(s-1) + now)/scale or (offset + now / scale).
+            */
+            return static_cast<TTimePoint>((Utils::getTimeSinceEpochNanos() + offset_) / scale_);
+        }
     }
+
+    virtual TTimeDelta fromWallDelta(TTimeDelta dt) override
+    {
+        return dt * scale_;
+    }
+    virtual TTimeDelta toWallDelta(TTimeDelta dt)  override
+    {
+        return dt / scale_;
+    }
+
+
+private:
+    double scale_;
+    TTimeDelta latency_;
+    double offset_;
 };
 
 }} //namespace

AirLib/include/common/UpdatableObject.hpp

@@ -6,7 +6,7 @@
 
 #include "common/Common.hpp"
 #include "StateReporter.hpp"
-#include "SimClock.hpp"
+#include "ClockFactory.hpp"
 
 namespace msr { namespace airlib {
 
@@ -28,11 +28,8 @@ class UpdatableObject {
 
     virtual ClockBase* clock()
     {
-        return &clock_;
+        return ClockFactory::get();
     }
-
-private:
-    SimClock clock_;
 };
 
 }} //namespace

AirLib/include/common/common_utils/ScheduledExecutor.hpp

@@ -68,23 +68,6 @@ class ScheduledExecutor {
         return sleep_time_avg_;
     }
 
-    static void sleep_for(double dt)
-    {
-        /*
-            This is spin loop implementation which may be suitable for sub-millisecond resolution.
-            //TODO: investigate below alternatives
-            On Windows we can use multimedia timers however this requires including entire Win32 header.
-            On Linux we can use nanosleep however below 2ms delays in real-time scheduler settings this 
-                probbaly does spin loop anyway.
-            
-        */
-        static constexpr duration<double> MinSleepDuration(0);
-        clock::time_point start = clock::now();
-        while (duration<double>(clock::now() - start).count() < dt) {
-            std::this_thread::sleep_for(MinSleepDuration);
-        }
-    }
-
     unsigned long getPeriodCount()
     {
         return period_count_;
@@ -104,6 +87,23 @@ class ScheduledExecutor {
     template <typename T>
     using duration = std::chrono::duration<T>;
 
+    static void sleep_for(double dt)
+    {
+        /*
+        This is spin loop implementation which may be suitable for sub-millisecond resolution.
+        //TODO: investigate below alternatives
+        On Windows we can use multimedia timers however this requires including entire Win32 header.
+        On Linux we can use nanosleep however below 2ms delays in real-time scheduler settings this 
+        probbaly does spin loop anyway.
+
+        */
+        static constexpr duration<double> MinSleepDuration(0);
+        clock::time_point start = clock::now();
+        while (duration<double>(clock::now() - start).count() < dt) {
+            std::this_thread::sleep_for(MinSleepDuration);
+        }
+    }
+
     void executorLoop()
     {
         clock::time_point call_end = clock::now();

AirLib/include/controllers/Waiter.hpp

@@ -8,6 +8,7 @@
 #include <iostream>
 #include "common/Common.hpp"
 #include "common/common_utils/Utils.hpp"
+#include "common/ClockFactory.hpp"
 
 namespace msr { namespace airlib {
 
@@ -38,43 +39,42 @@ class CancelableBase {
 
 class Waiter {
 private:
-    typedef std::chrono::steady_clock steady_clock;
+    ClockBase* clock_ = ClockFactory::get();
 
-    std::chrono::time_point<std::chrono::steady_clock> proc_start_ = steady_clock::now();
-    std::chrono::time_point<std::chrono::steady_clock> loop_start_ = proc_start_;         
+    TTimePoint proc_start_;
+    TTimePoint loop_start_;
 
-    std::chrono::duration<double> sleep_duration_, timeout_duration_;
+    TTimeDelta sleep_duration_, timeout_duration_;
 public:
-    Waiter(double sleep_duration_seconds, double timeout_duration = std::numeric_limits<float>::max())
+    Waiter(TTimeDelta sleep_duration_seconds, TTimeDelta timeout_duration = std::numeric_limits<TTimeDelta>::max())
         : sleep_duration_(sleep_duration_seconds), timeout_duration_(timeout_duration)
-    {}
+    {
+        proc_start_ = loop_start_ = clock_->nowNanos();
+    }
 
     virtual bool sleep(CancelableBase& cancelable_action)
     {
         // Sleeps for the time needed to get current running time up to the requested sleep_duration_.
         // So this can be used to "throttle" any loop to check something every sleep_duration_ seconds.
-        auto running_time = std::chrono::duration<double>(steady_clock::now() - loop_start_);
-        double seconds = std::chrono::duration_cast<std::chrono::duration<double>>(sleep_duration_ - running_time).count();
-        bool completed = cancelable_action.sleep(seconds);
-        loop_start_ = steady_clock::now();
+        TTimeDelta running_time = clock_->elapsedSince(loop_start_);
+        double remaining = sleep_duration_ - running_time;
+        bool completed = cancelable_action.sleep(clock_->toWallDelta(remaining));
+        loop_start_ = clock_->nowNanos();
         return completed;
     }
 
     void resetSleep()
     {
-    	loop_start_ = steady_clock::now();
+    	loop_start_ = clock_->nowNanos();
     }
     void resetTimeout()
     {
-    	proc_start_ = steady_clock::now();
+    	proc_start_ = clock_->nowNanos();
     }
 
     bool is_timeout() const
     {
-    	bool y = std::chrono::duration_cast<std::chrono::duration<double>>
-            (steady_clock::now() - proc_start_).count() >= timeout_duration_.count();
-
-        return y;
+    	return clock_->elapsedSince(proc_start_) >= timeout_duration_;
     }
 };
 

AirLib/include/controllers/rosflight/AirSimRosFlightBoard.hpp

@@ -9,6 +9,7 @@
 #include "common/Common.hpp"
 #include "vehicles/MultiRotorParams.hpp"
 #include "sensors/SensorCollection.hpp"
+#include "common/ClockFactory.hpp"
 
 //sensors
 #include "sensors/barometer/BarometerSimple.hpp"
@@ -57,12 +58,12 @@ class AirSimRosFlightBoard : public rosflight::Board {
 
     virtual uint64_t micros() override 
     {
-        return static_cast<uint64_t>(Utils::getTimeSinceEpochNanos() / 1.0E3);
+        return static_cast<uint64_t>(clock_->nowNanos() / 1.0E3);
     }
 
     virtual uint32_t millis() override 
     {
-        return static_cast<uint32_t>(Utils::getTimeSinceEpochNanos() / 1.0E6);
+        return static_cast<uint32_t>(clock_->nowNanos() / 1.0E6);
     }
 
     virtual void init_sensors(uint16_t& acc1G, float& gyro_scale, int boardVersion, const std::function<void(void)>& imu_updated_callback) override 
@@ -220,32 +221,13 @@ class AirSimRosFlightBoard : public rosflight::Board {
         //for MPU6050
         return static_cast<int16_t>((temperature - 36.53f) * 340.0f);
     }
-    void sleep(float msec)
+    void sleep(double msec)
     {
-        if (msec <= 0)
-            return;
-
-        //if duration is too small, use spin wait otherwise use spin wait
-        if (msec >= 5) {
-            static constexpr duration<double> MinSleepDuration(0);
-            clock::time_point start = clock::now();
-            double dt = msec * 1000;
-            //spin wait
-            while (duration<double>(clock::now() - start).count() < dt) {
-                std::this_thread::sleep_for(MinSleepDuration);
-            }
-        }
-        else {
-            std::this_thread::sleep_for(duration<double>(msec * 1000));
-        }
+        clock_->sleep_for(msec * 1000.0);
     }
 
 
 private: //types and consts
-    typedef std::chrono::high_resolution_clock clock;
-    template <typename T>
-    using duration = std::chrono::duration<T>;
-
     const MultiRotorParams::EnabledSensors* enabled_sensors_;
     const SensorCollection* sensors_;
     const ImuBase* imu_;
@@ -263,6 +245,8 @@ class AirSimRosFlightBoard : public rosflight::Board {
     static constexpr uint InputChannelCount = 16;
 
 private:
+    ClockBase* clock_ = ClockFactory::get();
+
     //motor outputs
     uint16_t motors_pwm_[OutputMotorCount];
     uint16_t input_channels_[InputChannelCount];