fix px4 connection for wsl 2.

AirLib/include/common/UpdatableContainer.hpp

@@ -24,10 +24,21 @@ class UpdatableContainer : public UpdatableObject {
     const TUpdatableObjectPtr& at(uint index) const { members_.at(index);  }
     TUpdatableObjectPtr& at(uint index) { return members_.at(index);  }
     //allow to override membership modifications
-    virtual void clear() { members_.clear(); }
-    virtual void insert(TUpdatableObjectPtr member) { members_.push_back(member);  }
-    virtual void erase_remove(TUpdatableObjectPtr obj) { 
-        members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); }
+    virtual void clear() {
+        for (auto m : members_) {
+            m->setParent(nullptr);
+        }
+        members_.clear();
+    }
+    virtual void insert(TUpdatableObjectPtr member) {
+        member->setParent(this);
+        members_.push_back(member);
+    }
+    virtual void erase_remove(TUpdatableObjectPtr member) {
+        member->setParent(nullptr);
+        members_.erase(std::remove(members_.begin(), members_.end(), member), members_.end());
+    }
+
 
 public:
     //*** Start: UpdatableState implementation ***//

AirLib/include/common/UpdatableObject.hpp

@@ -24,7 +24,6 @@ Do not call reset() from constructor or initialization because that will produce
 init->reset calls for base-derived class that would be incorrect.
 */
 
-
 class UpdatableObject {
 public:
     void reset()
@@ -73,6 +72,22 @@ class UpdatableObject {
         return ClockFactory::get();
     }
 
+    UpdatableObject* getParent() {
+        return parent_;
+    }
+
+    void setParent(UpdatableObject* container) {
+        parent_ = container;
+    }
+
+    std::string getName(){
+        return name_;
+    }
+
+    void setName(const std::string& name) {
+        this->name_ = name;
+    }
+
 protected:
     virtual void resetImplementation() = 0;
     virtual void failResetUpdateOrdering(std::string err)
@@ -84,6 +99,8 @@ class UpdatableObject {
     bool reset_called = false;
     bool update_called = false;
     bool reset_in_progress = false;
+    UpdatableObject* parent_ = nullptr;
+    std::string name_;
 };
 
 }} //namespace

AirLib/include/common/common_utils/FileSystem.hpp

@@ -104,8 +104,7 @@ class FileSystem
                 break;
         }
         if (i < 0) return "";
-        auto ui = static_cast<uint>(i);
-        return str.substr(ui, len - ui);
+        return str.substr(i, len - i);
     }
 
     static std::string getLogFolderPath(bool folder_timestamp, const std::string& parent = "")

AirLib/include/common/common_utils/ScheduledExecutor.hpp

@@ -49,13 +49,21 @@ class ScheduledExecutor {
     void pause(bool is_paused)
     {
         paused_ = is_paused;
+        pause_period_start_ = 0; // cancel any pause period.
     }
 
     bool isPaused() const
     {
         return paused_;
     }
 
+    void pauseForTime(double seconds)
+    {
+        pause_period_start_ = nanos();
+        pause_period_ = static_cast<TTimeDelta>(1E9 * seconds);
+        paused_ = true;
+    }
+
     void continueForTime(double seconds)
     {
         pause_period_start_ = nanos();
@@ -65,6 +73,7 @@ class ScheduledExecutor {
 
     void continueForFrames(uint32_t frames)
     {
+        pause_period_start_ = 0; // cancel any pause period.
         frame_countdown_enabled_ = true;
         targetFrameNumber_ = frames + currentFrameNumber_;
         paused_ = false;
@@ -174,9 +183,7 @@ class ScheduledExecutor {
 
             if (pause_period_start_ > 0) {
                 if (nanos() - pause_period_start_ >= pause_period_) {
-                    if (! isPaused())
-                        pause(true);
-
+                    pause(!isPaused());
                     pause_period_start_ = 0;
                 }
             }

AirLib/include/physics/FastPhysicsEngine.hpp

@@ -20,7 +20,8 @@ class FastPhysicsEngine : public PhysicsEngineBase {
 public:
     FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero())
         : enable_ground_lock_(enable_ground_lock), wind_(wind)
-    { 
+    {
+        setName("FastPhysicsEngine");
     }
 
     //*** Start: UpdatableState implementation ***//

AirLib/include/physics/PhysicsBody.hpp

@@ -84,7 +84,9 @@ class PhysicsBody : public UpdatableObject {
         inertia_ = inertia;
         inertia_inv_ = inertia_.inverse();
         environment_ = environment;
+        environment_->setParent(this);
         kinematics_ = kinematics;
+        kinematics_->setParent(this);
     }
 
     //enable physics body detection

AirLib/include/physics/PhysicsEngineBase.hpp

@@ -10,7 +10,7 @@
 
 namespace msr { namespace airlib {
 
-class PhysicsEngineBase : public UpdatableObject {
+class PhysicsEngineBase : public UpdatableContainer<PhysicsBody*> {
 public:
     virtual void update() override
     {
@@ -23,31 +23,7 @@ class PhysicsEngineBase : public UpdatableObject {
         //default nothing to report for physics engine
     }
 
-    //TODO: reduce copy-past from UpdatableContainer which has same code
-    /********************** Container interface **********************/
-    typedef PhysicsBody* TUpdatableObjectPtr;
-    typedef vector<TUpdatableObjectPtr> MembersContainer;
-    typedef typename MembersContainer::iterator iterator;
-    typedef typename MembersContainer::const_iterator const_iterator;
-    typedef typename MembersContainer::value_type value_type;
-
-    iterator begin() { return members_.begin(); }
-    iterator end() { return members_.end(); }
-    const_iterator begin() const { return members_.begin(); }
-    const_iterator end() const { return members_.end(); }
-    uint size() const { return static_cast<uint>(members_.size()); }
-    const TUpdatableObjectPtr &at(uint index) const { return members_.at(index);  }
-    TUpdatableObjectPtr &at(uint index) { return members_.at(index);  }
-    //allow to override membership modifications
-    virtual void clear() { members_.clear(); }
-    virtual void insert(TUpdatableObjectPtr member) { members_.push_back(member);  }
-    virtual void erase_remove(TUpdatableObjectPtr obj) { 
-        members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); }
-
-    virtual void setWind(const Vector3r& wind) {};
-
-private:
-    MembersContainer members_;
+    virtual void setWind(const Vector3r& wind) { unused(wind); };
 };
 
 

AirLib/include/physics/PhysicsWorld.hpp

@@ -13,14 +13,15 @@
 
 namespace msr { namespace airlib {
 
-class PhysicsWorld {
+class PhysicsWorld : UpdatableObject {
 public:
     PhysicsWorld(std::unique_ptr<PhysicsEngineBase> physics_engine, const std::vector<UpdatableObject*>& bodies,
             uint64_t update_period_nanos = 3000000LL, bool state_reporter_enabled = false,
             bool start_async_updator = true
         )
         : world_(std::move(physics_engine))
     {
+        setName("PhysicsWorld");
         enableStateReport(state_reporter_enabled);
         update_period_nanos_ = update_period_nanos;
         initializeWorld(bodies, start_async_updator);
@@ -108,6 +109,8 @@ class PhysicsWorld {
         world_.setFrameNumber(frameNumber);
     }
 
+    void resetImplementation() override {}
+
 private:
     void initializeWorld(const std::vector<UpdatableObject*>& bodies, bool start_async_updator)
     {

AirLib/include/physics/World.hpp

@@ -20,7 +20,8 @@ class World : public UpdatableContainer<UpdatableObject*> {
         : physics_engine_(std::move(physics_engine))
     { 
         World::clear();
-
+        setName("World");
+        physics_engine_->setParent(this);
         if (physics_engine)
             physics_engine_->clear();
     }
@@ -118,6 +119,11 @@ class World : public UpdatableContainer<UpdatableObject*> {
         return executor_.isPaused();
     }
 
+    void pauseForTime(double seconds)
+    {
+        executor_.pauseForTime(seconds);
+    }
+
     void continueForTime(double seconds)
     {
         executor_.continueForTime(seconds);

AirLib/include/sensors/SensorBase.hpp

@@ -59,7 +59,7 @@ class SensorBase : public UpdatableObject  {
 
 private:
     //ground truth can be shared between many sensors
-    GroundTruth ground_truth_;
+    GroundTruth ground_truth_ = { nullptr, nullptr };
     std::string name_ = "";
 };
 

AirLib/include/sensors/SensorFactory.hpp

@@ -19,28 +19,28 @@ class SensorFactory {
 public:
 
     // creates one sensor from settings
-    virtual std::unique_ptr<SensorBase> createSensorFromSettings(
+    virtual std::shared_ptr<SensorBase> createSensorFromSettings(
         const AirSimSettings::SensorSetting* sensor_setting) const
     {
         switch (sensor_setting->sensor_type) {
         case SensorBase::SensorType::Imu:
-            return std::unique_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
+            return std::shared_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
         case SensorBase::SensorType::Magnetometer:
-            return std::unique_ptr<MagnetometerSimple>(new MagnetometerSimple(*static_cast<const AirSimSettings::MagnetometerSetting*>(sensor_setting)));
+            return std::shared_ptr<MagnetometerSimple>(new MagnetometerSimple(*static_cast<const AirSimSettings::MagnetometerSetting*>(sensor_setting)));
         case SensorBase::SensorType::Gps:
-            return std::unique_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
+            return std::shared_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
         case SensorBase::SensorType::Barometer:
-            return std::unique_ptr<BarometerSimple>(new BarometerSimple(*static_cast<const AirSimSettings::BarometerSetting*>(sensor_setting)));
+            return std::shared_ptr<BarometerSimple>(new BarometerSimple(*static_cast<const AirSimSettings::BarometerSetting*>(sensor_setting)));
         default:
             throw new std::invalid_argument("Unexpected sensor type");
         }
     }
 
     // creates sensor-collection
     virtual void createSensorsFromSettings(
-        const std::map<std::string, std::unique_ptr<AirSimSettings::SensorSetting>>& sensors_settings,
+        const std::map<std::string, std::shared_ptr<AirSimSettings::SensorSetting>>& sensors_settings,
         SensorCollection& sensors,
-        vector<unique_ptr<SensorBase>>& sensor_storage) const
+        vector<shared_ptr<SensorBase>>& sensor_storage) const
     {
         for (const auto& sensor_setting_pair : sensors_settings) {
             const AirSimSettings::SensorSetting* sensor_setting = sensor_setting_pair.second.get();
@@ -49,11 +49,10 @@ class SensorFactory {
             if (sensor_setting == nullptr || !sensor_setting->enabled)
                 continue;
 
-            std::unique_ptr<SensorBase> sensor = createSensorFromSettings(sensor_setting);
+            std::shared_ptr<SensorBase> sensor = createSensorFromSettings(sensor_setting);
             if (sensor) {
-                    SensorBase* sensor_temp = sensor.get();
-                    sensor_storage.push_back(std::move(sensor));
-                    sensors.insert(sensor_temp, sensor_setting->sensor_type);
+                sensor_storage.push_back(sensor);
+                sensors.insert(sensor.get(), sensor_setting->sensor_type);
             }
         }
     }

AirLib/include/sensors/barometer/BarometerSimple.hpp

@@ -27,7 +27,7 @@ class BarometerSimple  : public BarometerBase {
         //GM process that would do random walk for pressure factor
         pressure_factor_.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, 0);
 
-        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma);
+        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
         //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
 
         //initialize frequency limiter
@@ -76,7 +76,7 @@ class BarometerSimple  : public BarometerBase {
         auto altitude = ground_truth.environment->getState().geo_point.altitude;
         auto pressure = EarthUtils::getStandardPressure(altitude);
 
-        //add drift in pressure, about 10m change per hour
+        //add drift in pressure, about 10m change per hour using default settings.
         pressure_factor_.update();
         pressure += pressure * pressure_factor_.getOutput();
 

AirLib/include/sensors/barometer/BarometerSimpleParams.hpp

@@ -5,6 +5,7 @@
 #define msr_airlib_BarometerSimpleParams_hpp
 
 #include "common/Common.hpp"
+#include "common/AirSimSettings.hpp"
 
 
 namespace msr { namespace airlib {
@@ -33,14 +34,14 @@ struct BarometerSimpleParams {
 
          real_T correlated_noise_sigma = 0.27f;
          real_T correlated_noise_tau = 0.87f;
-         real_T unnorrelated_noise_sigma = 0.24f;
+         real_T uncorrelated_noise_sigma = 0.24f;
     */
 
     //Experiments for MEAS MS56112 sensor shows 0.021mbar, datasheet has resoultion of 0.027mbar @ 1024
     //http://www.te.com/commerce/DocumentDelivery/DDEController?Action=srchrtrv&DocNm=MS5611-01BA03&DocType=Data+Sheet&DocLang=English
-    real_T unnorrelated_noise_sigma = 0.027f * 100;
+    real_T uncorrelated_noise_sigma = 0.027f * 100;
     //jMavSim uses below
-    //real_T unnorrelated_noise_sigma = 0.1f;
+    //real_T uncorrelated_noise_sigma = 0.1f;
 
     //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
     real_T update_latency = 0.0f;    //sec
@@ -49,7 +50,13 @@ struct BarometerSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::BarometerSetting& settings)
     {
-        unused(settings);
+        const auto& json = settings.settings;
+        pressure_factor_sigma = json.getFloat("PressureFactorSigma", pressure_factor_sigma);
+        pressure_factor_tau = json.getFloat("PressureFactorTau", pressure_factor_tau);
+        uncorrelated_noise_sigma = json.getFloat("UncorrelatedNoiseSigma", uncorrelated_noise_sigma);
+        update_latency = json.getFloat("UpdateLatency", update_latency);
+        update_frequency = json.getFloat("UpdateFrequency", update_frequency);
+        startup_delay = json.getFloat("StartupDelay", startup_delay);
     }
 };
 

AirLib/include/sensors/distance/DistanceSimple.hpp

@@ -22,7 +22,7 @@ class DistanceSimple : public DistanceBase {
         // initialize params
         params_.initializeFromSettings(setting);
 
-        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma);
+        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
         //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);