merge from github/master

AirLib/AirLib.vcxproj

@@ -86,6 +86,8 @@
     <ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibAdapators.hpp" />
     <ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibClient.hpp" />
     <ClInclude Include="include\vehicles\multirotor\api\MultirotorRpcLibServer.hpp" />
+    <ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\ArduCopterSoloApi.hpp" />
+    <ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\ArduCopterSoloParams.hpp" />
     <ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\AirSimSimpleFlightBoard.hpp" />
     <ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\AirSimSimpleFlightCommLink.hpp" />
     <ClInclude Include="include\vehicles\multirotor\firmwares\simple_flight\AirSimSimpleFlightEstimator.hpp" />

AirLib/AirLib.vcxproj.filters

@@ -447,6 +447,12 @@
     <ClInclude Include="include\common\common_utils\ColorUtils.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\ArduCopterSoloApi.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="include\vehicles\multirotor\firmwares\mavlink\ArduCopterSoloParams.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\safety\ObstacleMap.cpp">

AirLib/include/api/RpcLibAdapatorsBase.hpp

@@ -461,6 +461,37 @@ class RpcLibAdapatorsBase {
             return response_adapter;
         }
     };
+
+    struct LidarData {
+
+        msr::airlib::TTimePoint time_stamp;    // timestamp
+        std::vector<float> point_cloud;        // data
+
+        MSGPACK_DEFINE_MAP(time_stamp, point_cloud);
+
+        LidarData()
+        {}
+
+        LidarData(const msr::airlib::LidarData& s)
+        {
+            time_stamp = s.time_stamp;
+            point_cloud = s.point_cloud;
+
+            //TODO: remove bug workaround for https://github.com/rpclib/rpclib/issues/152
+            if (point_cloud.size() == 0)
+                point_cloud.push_back(0);
+        }
+
+        msr::airlib::LidarData to() const
+        {
+            msr::airlib::LidarData d;
+
+            d.time_stamp = time_stamp;
+            d.point_cloud = point_cloud;
+
+            return d;
+        }
+    };
 };
 
 }} //namespace

AirLib/include/api/RpcLibClientBase.hpp

@@ -53,6 +53,9 @@ class RpcLibClientBase {
     void enableApiControl(bool is_enabled, const std::string& vehicle_name = "");
 
     msr::airlib::GeoPoint getHomeGeoPoint(const std::string& vehicle_name = "") const;
+
+    msr::airlib::LidarData getLidarData(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;
+
     Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
     void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = "");
 

AirLib/include/api/VehicleApiBase.hpp

@@ -11,6 +11,8 @@
 #include "safety/SafetyEval.hpp"
 #include "common/CommonStructs.hpp"
 #include "common/ImageCaptureBase.hpp"
+#include "sensors/SensorCollection.hpp"
+#include "sensors/lidar/LidarBase.hpp"
 #include <exception>
 #include <string>
 
@@ -84,7 +86,7 @@ class VehicleApiBase : public UpdatableObject {
     //get reading from RC bound to vehicle (if unsupported then RCData::is_valid = false)
     virtual RCData getRCData() const
     {
-        static const RCData invalid_rc_data;
+        static const RCData invalid_rc_data {};
         return invalid_rc_data;
     }
     //set external RC data to vehicle (if unsupported then returns false)
@@ -94,6 +96,35 @@ class VehicleApiBase : public UpdatableObject {
         return false;
     }
 
+    // Sensors APIs
+    virtual const SensorCollection& getSensors() const
+    {
+        throw VehicleCommandNotImplementedException("getSensors API is not supported for this vehicle");
+    }
+
+    // Lidar APIs
+    virtual LidarData getLidarData(const std::string& lidar_name) const
+    {
+        const LidarBase* lidar = nullptr;
+
+        // Find lidar with the given name
+        // Not efficient but should suffice given small number of lidars
+        uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
+        for (uint i = 0; i < count_lidars; i++)
+        {
+            const LidarBase* current_lidar = static_cast<const LidarBase*>(getSensors().getByType(SensorBase::SensorType::Lidar, i));
+            if (current_lidar != nullptr && current_lidar->getName() == lidar_name)
+            {
+                lidar = current_lidar;
+                break;
+            }
+        }
+        if (lidar == nullptr)
+            throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));
+
+        return lidar->getOutput();
+    }
+
     virtual ~VehicleApiBase() = default;
 
     //exceptions

AirLib/include/common/AirSimSettings.hpp

@@ -23,7 +23,8 @@ struct AirSimSettings {
 public: //types
     static constexpr int kSubwindowCount = 3; //must be >= 3 for now
     static constexpr char const * kVehicleTypePX4 = "px4multirotor";
-    static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight";
+	static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo";
+	static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight";
     static constexpr char const * kVehicleTypePhysXCar = "physxcar";
     static constexpr char const * kVehicleTypeComputerVision = "computervision";
 
@@ -240,13 +241,9 @@ struct AirSimSettings {
         std::string model = "Generic";
     };
 
-    struct PX4VehicleSetting : public VehicleSetting {
-        MavLinkConnectionInfo connection_info;
-    };
-
-    struct SimpleFlightVehicleSetting : public VehicleSetting {
-        MavLinkConnectionInfo connection_info;
-    };
+	struct MavLinkVehicleSetting : public VehicleSetting {
+		MavLinkConnectionInfo connection_info;
+	};
 
     struct SegmentationSetting {
         enum class InitMethodType {
@@ -571,11 +568,11 @@ struct AirSimSettings {
         }
     }
 
-    static std::unique_ptr<VehicleSetting> createPX4VehicleSetting(const Settings& settings_json) 
+    static std::unique_ptr<VehicleSetting> createMavLinkVehicleSetting(const Settings& settings_json) 
     {
         //these settings_json are expected in same section, not in another child
-        std::unique_ptr<VehicleSetting> vehicle_setting_p = std::unique_ptr<VehicleSetting>(new PX4VehicleSetting());
-        PX4VehicleSetting* vehicle_setting = static_cast<PX4VehicleSetting*>(vehicle_setting_p.get());
+        std::unique_ptr<VehicleSetting> vehicle_setting_p = std::unique_ptr<VehicleSetting>(new MavLinkVehicleSetting());
+		MavLinkVehicleSetting* vehicle_setting = static_cast<MavLinkVehicleSetting*>(vehicle_setting_p.get());
 
         //TODO: we should be selecting remote if available else keyboard
         //currently keyboard is not supported so use rc as default
@@ -633,8 +630,8 @@ struct AirSimSettings {
         auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", ""));
 
         std::unique_ptr<VehicleSetting> vehicle_setting;
-        if (vehicle_type == kVehicleTypePX4)
-            vehicle_setting = createPX4VehicleSetting(settings_json);
+        if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo)
+            vehicle_setting = createMavLinkVehicleSetting(settings_json);
         //for everything else we don't need derived class yet
         else {
             vehicle_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());

AirLib/include/common/CommonStructs.hpp

@@ -166,7 +166,7 @@ struct GeoPoint {
         return os << "[" << g.latitude << ", " << g.longitude << ", " << g.altitude << "]";
     }
 
-    std::string to_string()
+    std::string to_string() const
     {
         return std::to_string(latitude) + string(", ") + std::to_string(longitude) + string(", ") + std::to_string(altitude);
     }
@@ -310,5 +310,14 @@ struct RCData {
     }
 };
 
+struct LidarData {
+
+    TTimePoint time_stamp = 0;
+    vector<real_T> point_cloud;
+
+    LidarData()
+    {}
+};
+
 }} //namespace
 #endif

AirLib/include/common/VectorMath.hpp

@@ -185,9 +185,15 @@ class VectorMathT {
 
     static QuaternionT rotateQuaternion(const QuaternionT& q, const QuaternionT& ref, bool assume_unit_quat)
     {
-        QuaternionT ref_n = assume_unit_quat ? ref : ref.normalized();
-        QuaternionT ref_n_i = assume_unit_quat ? ref.conjugate() : ref.inverse();
-        return ref_n * q * ref_n_i;
+        if (assume_unit_quat) {
+            QuaternionT ref_n = ref;
+            QuaternionT ref_n_i = ref.conjugate();
+            return ref_n * q * ref_n_i;
+        } else {
+            QuaternionT ref_n = ref.normalized();
+            QuaternionT ref_n_i = ref.inverse();
+            return ref_n * q * ref_n_i;
+        }
     }
 
     static QuaternionT rotateQuaternionReverse(const QuaternionT& q, const QuaternionT& ref, bool assume_unit_quat)

AirLib/include/common/common_utils/Utils.hpp

@@ -127,10 +127,11 @@ class Utils {
     }
 
     static bool startsWith(const string& s, const string& prefix) {
-        return s.size() <= prefix.size() && s.compare(0, prefix.size(), prefix) == 0;
+        return s.size() >= prefix.size() && s.compare(0, prefix.size(), prefix) == 0;
     }
+
     template <template<class, class, class...> class TContainer, typename TKey, typename TVal, typename... Args>
-    static const TVal& findOrDefault(const TContainer<TKey, TVal, Args...>& m, TKey const& key, const TVal& default_val) 
+    static const TVal& findOrDefault(const TContainer<TKey, TVal, Args...>& m, TKey const& key, const TVal& default_val)
     {
         typename TContainer<TKey, TVal, Args...>::const_iterator it = m.find(key);
         if (it == m.end())
@@ -141,8 +142,8 @@ class Utils {
     template <template<class, class, class...> class TContainer, typename TKey, typename TVal, typename... Args>
     static const TVal& findOrDefault(const TContainer<TKey, TVal, Args...>& m, TKey const& key)
     {
-	static TVal default_val;
-	return findOrDefault(m, key, default_val);
+        static TVal default_val;
+        return findOrDefault(m, key, default_val);
     }
 
     static Logger* getSetLogger(Logger* logger = nullptr)

AirLib/include/physics/FastPhysicsEngine.hpp

@@ -31,8 +31,6 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         for (PhysicsBody* body_ptr : *this) {
             initPhysicsBody(body_ptr);
         }
-
-        grounded_ = false;
     }
 
     virtual void insert(PhysicsBody* body_ptr) override
@@ -54,8 +52,8 @@ class FastPhysicsEngine : public PhysicsEngineBase {
     {
         for (PhysicsBody* body_ptr : *this) {
             reporter.writeValue("Phys", debug_string_.str());
-            reporter.writeValue("Is Grounded", grounded_);
-            reporter.writeValue("Force (world)", body_ptr->getWrench().force);
+			reporter.writeValue("Is Gounded", body_ptr->isGrounded());
+			reporter.writeValue("Force (world)", body_ptr->getWrench().force);
             reporter.writeValue("Torque (body)", body_ptr->getWrench().torque);
         }
         //call base
@@ -80,15 +78,15 @@ class FastPhysicsEngine : public PhysicsEngineBase {
 
         //first compute the response as if there was no collision
         //this is necessary to take in to account forces and torques generated by body
-        getNextKinematicsNoCollision(dt, body, current, next, next_wrench, grounded_);
+        getNextKinematicsNoCollision(dt, body, current, next, next_wrench);
 
         //if there is collision, see if we need collision response
         const CollisionInfo collision_info = body.getCollisionInfo();
         CollisionResponse& collision_response = body.getCollisionResponseInfo();
         //if collision was already responded then do not respond to it until we get updated information
-        if (grounded_ || (collision_info.has_collided && collision_response.collision_time_stamp != collision_info.time_stamp)) {
+        if (body.isGrounded() || (collision_info.has_collided && collision_response.collision_time_stamp != collision_info.time_stamp)) {
             bool is_collision_response = getNextKinematicsOnCollision(dt, collision_info, body, 
-                current, next, next_wrench, enable_ground_lock_, grounded_);
+                current, next, next_wrench, enable_ground_lock_);
             updateCollisionResponseInfo(collision_info, next, is_collision_response, collision_response);
             //throttledLogOutput("*** has collision", 0.1);
         }
@@ -100,7 +98,14 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         body.setKinematics(next);
         body.setWrench(next_wrench);
         body.kinematicsUpdated();
-    }
+
+
+		//TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence
+        //with below commented out - Arducopter GPS may not work.
+		//body.getEnvironment().setPosition(next.pose.position);
+		//body.getEnvironment().update();
+
+	}
 
     static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next, 
         bool is_collision_response, CollisionResponse& collision_response)
@@ -115,8 +120,8 @@ class FastPhysicsEngine : public PhysicsEngineBase {
     }
 
     //return value indicates if collision response was generated
-    static bool getNextKinematicsOnCollision(TTimeDelta dt, const CollisionInfo& collision_info, const PhysicsBody& body, 
-        const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench, bool enable_ground_lock, bool& grounded)
+    static bool getNextKinematicsOnCollision(TTimeDelta dt, const CollisionInfo& collision_info, PhysicsBody& body, 
+        const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench, bool enable_ground_lock)
     {
         /************************* Collision response ************************/
         const real_T dt_real = static_cast<real_T>(dt);
@@ -221,11 +226,11 @@ class FastPhysicsEngine : public PhysicsEngineBase {
             // also eliminate any linear velocity due to twist - since we are sitting on the ground there shouldn't be any.
             next.twist.linear = Vector3r::Zero();
             next.pose.position = collision_info.position;
-            grounded = true;
+			body.setGrounded(true);
 
             // but we do want to "feel" the ground when we hit it (we should see a small z-acc bump)
             // equal and opposite our downward velocity.
-            next.accelerations.linear = -0.5 * body.getMass() * vcur_avg;
+            next.accelerations.linear = -0.5f * body.getMass() * vcur_avg;
 
             //throttledLogOutput("*** Triggering ground lock", 0.1);
         }
@@ -309,8 +314,8 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         return wrench;
     }
 
-    static void getNextKinematicsNoCollision(TTimeDelta dt, const PhysicsBody& body, const Kinematics::State& current, 
-        Kinematics::State& next, Wrench& next_wrench, bool& grounded)
+    static void getNextKinematicsNoCollision(TTimeDelta dt, PhysicsBody& body, const Kinematics::State& current, 
+        Kinematics::State& next, Wrench& next_wrench)
     {
         const real_T dt_real = static_cast<real_T>(dt);
 
@@ -321,14 +326,14 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         //set wrench sum to zero
         const Wrench body_wrench = getBodyWrench(body, current.pose.orientation);
 
-        if (grounded) {
+        if (body.isGrounded()) {
             // make it stick to the ground until we see body wrench force greater than gravity.
             float normalizedForce = body_wrench.force.squaredNorm();
             float normalizedGravity = body.getEnvironment().getState().gravity.squaredNorm();
             if (normalizedForce >= normalizedGravity)
             {
                 //throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1);
-                grounded = false;
+				body.setGrounded(false);
             }
             next_wrench.force = Vector3r::Zero();
             next_wrench.torque = Vector3r::Zero();
@@ -355,7 +360,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         }
 
 
-        if (grounded) {
+        if (body.isGrounded()) {
             // this stops vehicle from vibrating while it is on the ground doing nothing.
             next.accelerations.angular = Vector3r::Zero();
             next.twist.linear = Vector3r::Zero();
@@ -444,7 +449,6 @@ class FastPhysicsEngine : public PhysicsEngineBase {
     static constexpr float kDragMinVelocity = 0.1f;
 
     std::stringstream debug_string_;
-    bool grounded_ = false;
     bool enable_ground_lock_;
     TTimePoint last_message_time;
 };