refactor sensor architecture for easy pluggability for sensors derive…

…d from Unreal engine

.gitignore

@@ -275,6 +275,7 @@ ModelManifest.xml
 /AirLib/cmake_install.cmake
 /MavLinkCom/lib/libMavLinkCom.a
 /Unreal/Plugins/AirSim/Source/AirLib/
+/Unreal/Environments/Blocks/Plugins/AirSim/
 /enc_temp_folder
 /cmake/MavLinkCom/MavLinkTest/MavLinkTest
 /cmake/MavLinkCom/lib/libMavLinkCom.a

AirLib/AirLib.vcxproj

@@ -68,6 +68,7 @@
     <ClInclude Include="include\common\common_utils\AsyncTasker.hpp" />
     <ClInclude Include="include\common\ImageCaptureBase.hpp" />
     <ClInclude Include="include\controllers\VehicleConnectorBase.hpp" />
+    <ClInclude Include="include\sensors\SensorFactory.hpp" />
     <ClInclude Include="include\vehicles\car\api\CarApiBase.hpp" />
     <ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibAdapators.hpp" />
     <ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibClient.hpp" />

AirLib/AirLib.vcxproj.filters

@@ -471,6 +471,9 @@
     <ClInclude Include="include\common\ImageCaptureBase.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\sensors\SensorFactory.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\safety\ObstacleMap.cpp">

AirLib/include/common/VectorMath.hpp

@@ -62,6 +62,10 @@ class VectorMathT {
         {
             return VectorMathT::subtract(lhs, rhs);
         }
+        friend Pose operator+(const Pose& lhs, const Pose& rhs)
+        {
+            return VectorMathT::add(lhs, rhs);
+        }
         friend bool operator==(const Pose& lhs, const Pose& rhs)
         {
             return lhs.position == rhs.position && lhs.orientation.coeffs() == rhs.orientation.coeffs();
@@ -293,6 +297,7 @@ class VectorMathT {
         QuaternionT net_q(dq_unit.coeffs() * dt + orientation.coeffs());
         return net_q.normalized();
     }
+    //all angles in radians
     static QuaternionT toQuaternion(RealT pitch, RealT roll, RealT yaw)
     {
         QuaternionT q;
@@ -310,7 +315,7 @@ class VectorMathT {
         return q;
     }
 
-    //from http://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/Pose_8hh_source.html
+    //from https://github.com/arpg/Gazebo/blob/master/gazebo/math/Pose.cc
     static Vector3T coordPositionSubtract(const Pose& lhs, const Pose& rhs)
     {
         QuaternionT tmp(0,
@@ -329,11 +334,28 @@ class VectorMathT {
         result.normalize();
         return result;
     }
+    static Vector3T coordPositionAdd(const Pose& lhs, const Pose& rhs)
+    {
+        QuaternionT tmp(0, lhs.position.x(), lhs.position.y(), lhs.position.z());
+
+        tmp = rhs.orientation * (tmp * rhs.orientation.inverse());
+
+        return tmp.vec() + rhs.position;
+    }
+    static QuaternionT coordOrientationAdd(const QuaternionT& lhs, const QuaternionT& rhs)
+    {
+        QuaternionT result(rhs * lhs);
+        result.normalize();
+        return result;
+    }
     static Pose subtract(const Pose& lhs, const Pose& rhs)
     {
         return Pose(coordPositionSubtract(lhs, rhs), coordOrientationSubtract(lhs.orientation, rhs.orientation));
     }
-
+    static Pose add(const Pose& lhs, const Pose& rhs)
+    {
+        return Pose(coordPositionAdd(lhs, rhs), coordOrientationAdd(lhs.orientation, rhs.orientation));
+    }
 
     static std::string toString(const Vector3T& vect, const char* prefix = nullptr)
     {
@@ -399,6 +421,24 @@ class VectorMathT {
     static QuaternionT quaternionFromYaw(RealT yaw) {
         return QuaternionT(Eigen::AngleAxisd(yaw, Vector3T::UnitZ()));
     }
+
+    static const Vector3T front() 
+    {
+        static Vector3T v(1, 0, 0);
+        return v;
+    }
+
+    static const Vector3T down() 
+    {
+        static Vector3T v(0, 0, 1);
+        return v;
+    }
+
+    static const Vector3T right() 
+    {
+        static Vector3T v(0, 1, 0);
+        return v;
+    }
 };
 typedef VectorMathT<Eigen::Vector3d, Eigen::Quaternion<double,Eigen::DontAlign>, double> VectorMathd;
 typedef VectorMathT<Eigen::Vector3f, Eigen::Quaternion<float,Eigen::DontAlign>, float> VectorMathf;

AirLib/include/physics/Environment.hpp

@@ -78,15 +78,6 @@ class Environment : public UpdatableObject {
         return current_;
     }
 
-    void setDistance(const real_T distance)
-    {
-        distance_ = distance;
-    }
-    real_T getDistance() const
-    {
-        return distance_;
-    }
-
     //*** Start: UpdatableState implementation ***//
     virtual void reset()
     {
@@ -116,7 +107,6 @@ class Environment : public UpdatableObject {
 private:
     State initial_, current_;
     EarthUtils::HomeGeoPoint home_geo_point_;
-    real_T distance_;
 };
 
 }} //namespace

AirLib/include/physics/PhysicsBody.hpp

@@ -55,10 +55,7 @@ class PhysicsBody : public UpdatableObject {
     {
         collision_info_ = collision_info;
     }
-    virtual void setDistance(const real_T distance)
-    {
-        environment_->setDistance(distance);
-    }
+
 
 public: //methods
     //constructors

AirLib/include/sensors/SensorBase.hpp

@@ -19,6 +19,14 @@ namespace msr { namespace airlib {
     set the sensor in good-to-use state by call to reset.
 */
 class SensorBase : public UpdatableObject  {
+public:
+    enum class SensorType : uint {
+        Barometer = 1,
+        Imu = 2,
+        Gps = 3,
+        Magnetometer = 4,
+        Distance = 5
+    };
 protected:
     struct GroundTruth {
         const Kinematics::State* kinematics;

AirLib/include/sensors/SensorCollection.hpp

@@ -14,16 +14,9 @@ namespace msr { namespace airlib {
 
 class SensorCollection : UpdatableObject {
 public: //types
-    enum class SensorType : uint {
-        Barometer = 1,
-        Imu = 2,
-        Gps = 3,
-        Magnetometer = 4,
-        Distance = 5
-    };
     typedef SensorBase* SensorBasePtr;
 public:
-    void insert(SensorBasePtr sensor, SensorType type)
+    void insert(SensorBasePtr sensor, SensorBase::SensorType type)
     {
         auto type_int = static_cast<uint>(type);
         const auto& it = sensors_.find(type_int);
@@ -36,7 +29,7 @@ class SensorCollection : UpdatableObject {
         }
     }
 
-    const SensorBase* getByType(SensorType type, uint index = 0) const
+    const SensorBase* getByType(SensorBase::SensorType type, uint index = 0) const
     {
         auto type_int = static_cast<uint>(type);
         const auto& it = sensors_.find(type_int);
@@ -48,7 +41,7 @@ class SensorCollection : UpdatableObject {
         }
     }
 
-    uint size(SensorType type) const
+    uint size(SensorBase::SensorType type) const
     {
         auto type_int = static_cast<uint>(type);
         const auto& it = sensors_.find(type_int);
@@ -69,7 +62,11 @@ class SensorCollection : UpdatableObject {
         }
     }
 
-
+    void clear()
+    {
+        sensors_.clear();
+    }
+
     //*** Start: UpdatableState implementation ***//
     virtual void reset() override
     {

AirLib/include/sensors/SensorFactory.hpp

@@ -0,0 +1,38 @@
+#ifndef msr_airlib_SensorFactoryBase_hpp
+#define msr_airlib_SensorFactoryBase_hpp
+
+
+#include "SensorBase.hpp"
+#include <memory>
+
+//sensors
+#include "sensors/imu/ImuSimple.hpp"
+#include "sensors/magnetometer/MagnetometerSimple.hpp"
+#include "sensors/gps/GpsSimple.hpp"
+#include "sensors/barometer/BarometerSimple.hpp"
+
+namespace msr { namespace airlib {
+
+
+class SensorFactory {
+public:
+    virtual std::unique_ptr<SensorBase> createSensor(SensorBase::SensorType sensor_type) const
+    {
+        switch (sensor_type) {
+        case SensorBase::SensorType::Imu:
+            return std::unique_ptr<ImuSimple>(new ImuSimple());
+        case SensorBase::SensorType::Magnetometer:
+            return std::unique_ptr<MagnetometerSimple>(new MagnetometerSimple());
+        case SensorBase::SensorType::Gps:
+            return std::unique_ptr<GpsSimple>(new GpsSimple());
+        case SensorBase::SensorType::Barometer:
+            return std::unique_ptr<BarometerSimple>(new BarometerSimple());
+        default:
+            return std::unique_ptr<SensorBase>();
+        }
+    }
+};
+
+
+}} //namespace
+#endif

AirLib/include/sensors/distance/DistanceBase.hpp

@@ -13,12 +13,10 @@ namespace msr { namespace airlib {
 class DistanceBase  : public SensorBase {
 public: //types
     struct Output { //same fields as ROS message
-    real_T min_distance;//cm
-    real_T max_distance;//cm
-    real_T distance;    //meters
-    real_T sensor_type;
-    real_T sensor_id;
-    real_T orientation;
+        real_T distance;    //meters
+        real_T min_distance;//m
+        real_T max_distance;//m
+        Pose relative_pose;
     };
 
 

AirLib/include/sensors/distance/DistanceSimple.hpp

@@ -62,25 +62,28 @@ class DistanceSimple  : public DistanceBase {
 
     virtual ~DistanceSimple() = default;
 
+protected:
+    virtual real_T getRayLength(const Pose& pose) = 0;
+    const DistanceSimpleParams& getParams()
+    {
+        return params_;
+    }
+
 private: //methods
     Output getOutputInternal()
     {
         Output output;
         const GroundTruth& ground_truth = getGroundTruth();
 
-        auto distance = ground_truth.environment->getDistance();
+        auto distance = getRayLength(ground_truth.kinematics->pose + params_.relative_pose);
 
         //add noise in distance (about 0.2m sigma)
         distance += uncorrelated_noise_.next();
 
         output.distance = distance;
-
         output.min_distance = params_.min_distance;
         output.max_distance = params_.max_distance;
-        output.sensor_type  = params_.sensor_type;
-        output.sensor_id    = params_.sensor_id;
-        output.orientation  = params_.orientation;
-
+        output.relative_pose = params_.relative_pose;
 
         return output;
     }

AirLib/include/sensors/distance/DistanceSimpleParams.hpp

@@ -11,11 +11,9 @@ namespace msr { namespace airlib {
 
 
 struct DistanceSimpleParams {
-    real_T min_distance = 20;
-    real_T max_distance = 4000;
-    real_T sensor_type  = 0;   //TODO: allow changing in settings?
-    real_T sensor_id    = 77;  //TODO: should this be something real?
-    real_T orientation  = 25;  //Pitch270 (downwards) as defined in MavLinkMessages
+    real_T min_distance = 20.0f / 100; //m
+    real_T max_distance = 4000.0f / 100; //m
+    Pose relative_pose;
 
 /*
     Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters

AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp

@@ -10,6 +10,7 @@
 #include "MagnetometerSimpleParams.hpp"
 #include "MagnetometerBase.hpp"
 #include "common/FrequencyLimiter.hpp"
+#include "common/DelayLine.hpp"
 
 
 namespace msr { namespace airlib {

AirLib/include/vehicles/multirotor/MultiRotor.hpp

@@ -163,11 +163,6 @@ class MultiRotor : public PhysicsBody {
         getController()->setCollisionInfo(collision_info);
     }
 
-    virtual void setDistance(const real_T distance) override
-    {
-        PhysicsBody::setDistance(distance);
-    }
-
     virtual ~MultiRotor() = default;
 
 private: //methods