Merge pull request #1 from Microsoft/master

sync

.gitignore

@@ -53,6 +53,7 @@ obj/
 
 # Visual Studio 2015 cache/options directory
 .vs/
+.vscode/
 
 # MSTest test Results
 [Tt]est[Rr]esult*/

AirLib/include/api/VehicleApiBase.hpp

@@ -12,25 +12,26 @@ namespace msr { namespace airlib {
 
 class VehicleApiBase {
 public:
-    virtual GeoPoint getHomeGeoPoint() = 0;
+    virtual GeoPoint getHomeGeoPoint() const = 0;
     virtual void enableApiControl(bool is_enabled) = 0;
     virtual bool isApiControlEnabled() const = 0;
     virtual void reset() = 0;
 
-    virtual vector<ImageCaptureBase::ImageResponse> simGetImages(const vector<ImageCaptureBase::ImageRequest>& request) = 0;
-    virtual vector<uint8_t> simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) = 0;
+    virtual vector<ImageCaptureBase::ImageResponse> simGetImages(const vector<ImageCaptureBase::ImageRequest>& request) const = 0;
+    virtual vector<uint8_t> simGetImage(uint8_t camera_id, ImageCaptureBase::ImageType image_type) const = 0;
 
     virtual void simSetPose(const Pose& pose, bool ignore_collision) = 0;
-    virtual Pose simGetPose() = 0;
+    virtual Pose simGetPose() const = 0;
 
     virtual bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex = false) = 0;
-    virtual int simGetSegmentationObjectID(const std::string& mesh_name) = 0;
+    virtual int simGetSegmentationObjectID(const std::string& mesh_name) const = 0;
 
-    virtual void simPrintLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0;
+    virtual void simPrintLogMessage(const std::string& message, 
+        const std::string& message_param = "", unsigned char severity = 0) = 0;
 
-    virtual CollisionInfo getCollisionInfo() = 0;
+    virtual CollisionInfo getCollisionInfo() const = 0;
 
-    virtual Pose simGetObjectPose(const std::string& object_name) = 0;
+    virtual Pose simGetObjectPose(const std::string& object_name) const = 0;
 
     virtual CameraInfo getCameraInfo(int camera_id) const = 0;
     virtual void setCameraOrientation(int camera_id, const Quaternionr& orientation) = 0;

AirLib/include/common/AirSimSettings.hpp

@@ -48,12 +48,17 @@ struct AirSimSettings {
         }
     };
 
-    struct CarMeshPaths {
-        std::string skeletal = "/AirSim/VehicleAdv/Vehicle/Vehicle_SkelMesh.Vehicle_SkelMesh";
-        std::string bp = "/AirSim/VehicleAdv/Vehicle/VehicleAnimationBlueprint";
-        std::string slippery_mat = "/AirSim/VehicleAdv/PhysicsMaterials/Slippery.Slippery";
-        std::string non_slippery_mat = "/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery";
-
+    struct PawnPath {
+        std::string pawn_bp;
+        std::string slippery_mat;
+        std::string non_slippery_mat;
+
+        PawnPath(const std::string& pawn_bp_val = "",
+            const std::string& slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/Slippery.Slippery",
+            const std::string& non_slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery") 
+            : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val)
+        {
+        }
     };
 
     struct VehicleSettings {
@@ -196,7 +201,7 @@ struct AirSimSettings {
     bool engine_sound;
     bool log_messages_visible;
     HomeGeoPoint origin_geopoint;
-    CarMeshPaths car_mesh_paths;
+    std::map<std::string, PawnPath> pawn_paths;
 
 public: //methods
     static AirSimSettings& singleton() 
@@ -227,6 +232,7 @@ struct AirSimSettings {
         loadCaptureSettings(settings);
         loadCameraNoiseSettings(settings);
         loadSegmentationSettings(settings);
+        loadPawnPaths(settings);
         loadOtherSettings(settings);
 
         return static_cast<unsigned int>(warning_messages.size());
@@ -370,6 +376,8 @@ struct AirSimSettings {
             initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP;
         else if (view_mode_string == "NoDisplay")
             initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY;
+        else if (view_mode_string == "Front")
+            initial_view_mode = 8; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT;
         else
             warning_messages.push_back("ViewMode setting is not recognized: " + view_mode_string);
     }
@@ -433,6 +441,45 @@ struct AirSimSettings {
         }
     }
 
+    void loadPawnPaths(const Settings& settings)
+    {
+        pawn_paths.clear();
+        pawn_paths.emplace("BareboneCar",
+            PawnPath("Class'/AirSim/VehicleAdv/Vehicle/VehicleAdvPawn.VehicleAdvPawn_C'"));
+        pawn_paths.emplace("DefaultCar",
+            PawnPath("Class'/AirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C'"));
+        pawn_paths.emplace("DefaultQuadrotor",
+            PawnPath("Class'/AirSim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'"));
+
+
+        msr::airlib::Settings pawn_paths_child;
+        if (settings.getChild("PawnPaths", pawn_paths_child)) {
+            std::vector<std::string> keys;
+            pawn_paths_child.getChildNames(keys);
+
+            for (const auto& key : keys) {
+                msr::airlib::Settings child;
+                pawn_paths_child.getChild(key, child);
+                pawn_paths[key] = createPathPawn(child);
+            }
+        }
+    }
+
+    PawnPath createPathPawn(const Settings& settings)
+    {
+        auto paths = PawnPath();
+        paths.pawn_bp = settings.getString("PawnBP", "");
+        auto slippery_mat = settings.getString("SlipperyMat", "");
+        auto non_slippery_mat = settings.getString("NonSlipperyMat", "");
+
+        if (slippery_mat != "")
+            paths.slippery_mat = slippery_mat;
+        if (non_slippery_mat != "")
+            paths.non_slippery_mat = non_slippery_mat;
+
+        return paths;
+    }
+
     void loadSegmentationSettings(const Settings& settings)
     {
         Settings json_parent;

AirLib/include/common/ClockBase.hpp

@@ -15,6 +15,13 @@ class ClockBase {
     //returns value indicating nanoseconds elapsed since some reference timepoint in history
     //typically nanoseconds from Unix epoch
     virtual TTimePoint nowNanos() const = 0;
+    virtual TTimePoint getStart() const = 0;
+
+
+    ClockBase()
+    {
+        wall_clock_start_ = Utils::getTimeSinceEpochNanos();
+    }
 
     TTimeDelta elapsedSince(TTimePoint since) const
     {
@@ -63,11 +70,23 @@ class ClockBase {
             std::this_thread::sleep_for(MinSleepDuration);
     }
 
+    double getTrueScaleWrtWallClock()
+    {
+        TTimePoint wall_clock_now = Utils::getTimeSinceEpochNanos();
+        TTimeDelta wall_clock_elapsed = elapsedBetween(wall_clock_now, wall_clock_start_);
+
+        TTimePoint clock_now = nowNanos();
+        TTimeDelta clock_elapsed = elapsedBetween(clock_now, getStart());
+
+        return static_cast<double>(clock_elapsed) / wall_clock_elapsed;
+    }
+
 private:
     template <typename T>
     using duration = std::chrono::duration<T>;
 
     uint64_t step_count_ = 0;
+    TTimePoint wall_clock_start_;
 };
 
 }} //namespace

AirLib/include/common/ScalableClock.hpp

@@ -17,6 +17,8 @@ class ScalableClock : public ClockBase {
         : scale_(scale), latency_(latency)
     {
         offset_ = latency * (scale_ - 1);
+        start_ = nowNanos();
+
         unused(latency_);
     }
 
@@ -36,6 +38,11 @@ class ScalableClock : public ClockBase {
         }
     }
 
+    virtual TTimePoint getStart() const override
+    {
+        return start_;
+    }
+
     virtual void sleep_for(TTimeDelta dt) override
     {
         //for intervals > 2ms just sleep otherwise do spilling otherwise delay won't be accurate
@@ -69,6 +76,8 @@ class ScalableClock : public ClockBase {
     double scale_;
     TTimeDelta latency_;
     double offset_;
+    TTimePoint start_;
+
 };
 
 }} //namespace

AirLib/include/common/Settings.hpp

@@ -128,6 +128,14 @@ class Settings {
         return doc_.size();
     }
 
+    template<typename Container>
+    void getChildNames(Container& c) const
+    {
+        for (auto it = doc_.begin(); it != doc_.end(); ++it) {
+            c.push_back(it.key());
+        }
+    }
+
     bool getChild(size_t index, Settings& child) const
     {
         if (doc_.size() > index && 

AirLib/include/common/SteppableClock.hpp

@@ -19,7 +19,7 @@ class SteppableClock : public ClockBase {
     SteppableClock(TTimeDelta step = 20E-3f, TTimePoint start = 0)
         : current_(start), step_(step)
     {
-        current_ = start ? start : Utils::getTimeSinceEpochNanos();
+        start_ = current_ = start ? start : Utils::getTimeSinceEpochNanos();
     }
 
     TTimePoint stepBy(TTimeDelta amount)
@@ -46,8 +46,16 @@ class SteppableClock : public ClockBase {
         return current_;
     }
 
+    virtual TTimePoint getStart() const override
+    {
+        return start_;
+    }
+
+
 private:
     std::atomic<TTimePoint> current_;
+    std::atomic<TTimePoint> start_;
+
     TTimeDelta step_;
 };
 

AirLib/include/common/VectorMath.hpp

@@ -397,13 +397,13 @@ class VectorMathT {
             , 1.0f - 2.0f * (q.x() * q.x() + q.y() * q.y()));
     }
 
-    static RealT normalizeAngleDegrees(RealT angle)
+    static RealT normalizeAngle(RealT angle, RealT max_angle = static_cast<RealT>(360))
     {
-        angle = static_cast<RealT>(std::fmod(angle, 360));
-        if (angle > 180)
-            return angle - 360;
-        else if (angle < -180)
-            return angle + 360;
+        angle = static_cast<RealT>(std::fmod(angle, max_angle));
+        if (angle > max_angle/2)
+            return angle - max_angle;
+        else if (angle < -max_angle/2)
+            return angle + max_angle;
         else
             return angle;
     }

AirLib/include/common/common_utils/ScheduledExecutor.hpp

@@ -31,21 +31,36 @@ class ScheduledExecutor {
         callback_ = callback;
         period_nanos_ = period_nanos;
         started_ = false;
+        paused_ = false;
     }
 
     void start()
     {
         started_ = true;
+        paused_ = false;
+
         sleep_time_avg_ = 0;
         period_count_ = 0;
         Utils::cleanupThread(th_);
         th_ = std::thread(&ScheduledExecutor::executorLoop, this);
     }
 
+    void pause(bool is_paused)
+    {
+        paused_ = is_paused;
+    }
+
+    bool isPaused() const
+    {
+        return paused_;
+    }
+
     void stop()
     {
         if (started_) {
             started_ = false;
+            paused_ = false;
+
             try {
                 if (th_.joinable()) {
                     th_.join();
@@ -56,20 +71,20 @@ class ScheduledExecutor {
         }
     }
 
-    bool isRunning()
+    bool isRunning() const
     {
-        return started_;
+        return started_ && !paused_;
     }
 
-    double getSleepTimeAvg()
+    double getSleepTimeAvg() const
     {
         //TODO: make this function thread safe by using atomic types
         //right now this is not implemented for performance and that
         //return of this function is purely informational/debugging purposes
         return sleep_time_avg_;
     }
 
-    uint64_t getPeriodCount()
+    uint64_t getPeriodCount() const
     {
         return period_count_;
     }
@@ -126,7 +141,7 @@ class ScheduledExecutor {
             TTimeDelta since_last_call = period_start - call_end;
 
             //is this first loop?
-            if (period_count_ > 0) {
+            if (period_count_ > 0 && !paused_) {
                 //when we are doing work, don't let other thread to cause contention
                 std::lock_guard<std::mutex> locker(mutex_);
 
@@ -153,7 +168,7 @@ class ScheduledExecutor {
     uint64_t period_nanos_;
     std::thread th_;
     std::function<bool(uint64_t)> callback_;
-    std::atomic_bool started_;
+    std::atomic_bool started_, paused_;
 
     double sleep_time_avg_;
     uint64_t period_count_;

AirLib/include/controllers/ControllerBase.hpp

@@ -21,8 +21,8 @@ namespace msr { namespace airlib {
 class ControllerBase : public UpdatableObject {
 public:
     //return 0 to 1 (corresponds to zero to full thrust)
-    virtual real_T getVertexControlSignal(unsigned int rotor_index) = 0;
-    virtual size_t getVertexCount() = 0;
+    virtual real_T getVertexControlSignal(unsigned int rotor_index) const = 0;
+    virtual size_t getVertexCount() const = 0;
 
     virtual void getStatusMessages(std::vector<std::string>& messages)
     {

AirLib/include/controllers/VehicleControllerBase.hpp

@@ -19,12 +19,12 @@ class VehicleControllerBase : public ControllerBase {
     //tells the controller to switch from human operated mode to computer operated mode
     virtual void enableApiControl(bool is_enabled) = 0;
     virtual void setSimulationMode(bool is_set) = 0;
-    virtual bool isApiControlEnabled() = 0;
-    virtual bool isSimulationMode() = 0;
+    virtual bool isApiControlEnabled() const = 0;
+    virtual bool isSimulationMode() const = 0;
 
     //if controller connects via USB/UDP and connection fails then this
     //should return false
-    virtual bool isAvailable(std::string& message) = 0;
+    virtual bool isAvailable(std::string& message) const = 0;
 
     //TODO: below method is needed to support firmwares without state estimation. In future, we should probably remove this support.
     virtual void setGroundTruth(PhysicsBody* physics_body)