added CollisionInfo in APIs

AirLib/include/common/CommonStructs.hpp

@@ -179,6 +179,18 @@ struct CollisionInfo {
     Vector3r position = Vector3r::Zero();
     real_T penetration_depth = 0;
     TTimePoint time_stamp = 0;
+
+    CollisionInfo()
+    {}
+
+    CollisionInfo(bool has_collided_val, const Vector3r& normal_val, 
+        const Vector3r& impact_point_val, const Vector3r& position_val, 
+        real_T penetration_depth_val, TTimePoint time_stamp_val)
+        : has_collided(has_collided_val), normal(normal_val),
+        impact_point(impact_point), position(position_val),
+        penetration_depth(penetration_depth_val), time_stamp(time_stamp_val)
+    {
+    }
 };
 
 struct CollisonResponseInfo {

AirLib/include/controllers/DroneControllerBase.hpp

@@ -218,6 +218,12 @@ class DroneControllerBase : public VehicleControllerBase {
     /// Get the current GPS location of the drone.
     virtual GeoPoint getGpsLocation() = 0;
 
+    //below are for passing information from simulator to API layer
+    //in non simulation mode default would be no collison unless
+    //controller implements otherwise.
+    virtual CollisionInfo getCollisionInfo();
+    virtual void setCollisionInfo(const CollisionInfo& collision_info);
+
     //safety settings
     virtual void setSafetyEval(const shared_ptr<SafetyEval> safety_eval_ptr);
     virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
@@ -369,6 +375,7 @@ class DroneControllerBase : public VehicleControllerBase {
     shared_ptr<SafetyEval> safety_eval_ptr_;
     float obs_avoidance_vel_ = 0.5f;
     bool log_to_file_ = false;
+    CollisionInfo collision_info_;
 
     // we make this recursive so that DroneControllerBase subclass can grab StatusLock then call a 
     // base class method on DroneControllerBase that also grabs the StatusLock.

AirLib/include/controllers/DroneControllerCancelable.hpp

@@ -189,6 +189,11 @@ class DroneControllerCancelable  {
         return controller_->getLandedState();
     }
 
+    CollisionInfo getCollisionInfo()
+    {
+        return controller_->getCollisionInfo();
+    }
+
     RCData getRCData()
     {
         return controller_->getRCData();

AirLib/include/physics/PhysicsBody.hpp

@@ -182,8 +182,8 @@ class PhysicsBody : public UpdatableObject {
     {
         return collison_info_;
     }
-    //ability to get reference so individual fields can be modified
-    void setCollisionInfo(const CollisionInfo& collison_info)
+
+    virtual void setCollisionInfo(const CollisionInfo& collison_info)
     {
         collison_info_ = collison_info;
     }

AirLib/include/rpc/RpcLibAdapators.hpp

@@ -52,7 +52,7 @@ class RpcLibAdapators {
     };
 
     struct Vector3r {
-        msr::airlib::real_T x_, y_, z_;
+        msr::airlib::real_T x_ = 0, y_ = 0, z_ = 0;
         MSGPACK_DEFINE_ARRAY(x_, y_, z_);
 
         Vector3r()
@@ -70,6 +70,35 @@ class RpcLibAdapators {
         }
     };
 
+    struct CollisionInfo {
+        bool has_collided = false;
+        Vector3r normal;
+        Vector3r impact_point;
+        Vector3r position;
+        msr::airlib::real_T penetration_depth = 0;
+        msr::airlib::TTimePoint time_stamp = 0;
+
+        MSGPACK_DEFINE_ARRAY(has_collided, penetration_depth, time_stamp, normal, impact_point, position);
+
+        CollisionInfo()
+        {}
+
+        CollisionInfo(const msr::airlib::CollisionInfo& s)
+        {
+            has_collided = s.has_collided;
+            penetration_depth = s.penetration_depth;
+            time_stamp = s.time_stamp;
+            normal = s.normal;
+            impact_point = s.impact_point;
+            position = s.position;
+        }
+
+        msr::airlib::CollisionInfo to() const
+        {
+            return msr::airlib::CollisionInfo(has_collided, normal.to(), impact_point.to(), position.to(),
+                penetration_depth, time_stamp);
+        }
+    };
 
     struct Quaternionr {
         msr::airlib::real_T w_, x_, y_, z_;

AirLib/include/rpc/RpcLibClient.hpp

@@ -43,6 +43,7 @@ class RpcLibClient {
     bool hover();
 
     Vector3r getPosition();
+    CollisionInfo getCollisionInfo();
     Vector3r getVelocity();
     Quaternionr getOrientation();
     DroneControllerBase::LandedState getLandedState();

AirLib/include/vehicles/MultiRotor.hpp

@@ -146,6 +146,12 @@ class MultiRotor : public PhysicsBody {
         return rotors_.at(rotor_index).getOutput();
     }
 
+    virtual void setCollisionInfo(const CollisionInfo& collison_info) override
+    {
+        PhysicsBody::setCollisionInfo(collison_info);
+        getController()->setCollisionInfo(collison_info);
+    }
+
     virtual ~MultiRotor() = default;
 
 private: //methods

AirLib/src/controllers/DroneControllerBase.cpp

@@ -677,6 +677,14 @@ Pose DroneControllerBase::getDebugPose()
     return Pose::nanPose();
 }
 
+CollisionInfo DroneControllerBase::getCollisionInfo()
+{
+    return collision_info_;
+}
+void DroneControllerBase::setCollisionInfo(const CollisionInfo& collision_info)
+{
+    collision_info_ = collision_info;
+}
 
 }} //namespace
 #endif

AirLib/src/rpc/RpcLibClient.cpp

@@ -152,12 +152,10 @@ Vector3r RpcLibClient::getPosition()
 {
     return pimpl_->client.call("getPosition").as<RpcLibAdapators::Vector3r>().to();
 }
-
 Vector3r RpcLibClient::getVelocity()
 {
     return pimpl_->client.call("getVelocity").as<RpcLibAdapators::Vector3r>().to();
 }
-
 Quaternionr RpcLibClient::getOrientation()
 {
     return pimpl_->client.call("getOrientation").as<RpcLibAdapators::Quaternionr>().to();
@@ -174,6 +172,11 @@ RCData RpcLibClient::getRCData()
     return pimpl_->client.call("getRCData").as<RpcLibAdapators::RCData>().to();
 }
 
+CollisionInfo RpcLibClient::getCollisionInfo()
+{
+    return pimpl_->client.call("getCollisionInfo").as<RpcLibAdapators::CollisionInfo>().to();
+}
+
 TTimePoint RpcLibClient::timestampNow()
 {
     return pimpl_->client.call("timestampNow").as<TTimePoint>();

AirLib/src/rpc/RpcLibServer.cpp

@@ -106,6 +106,7 @@ RpcLibServer::RpcLibServer(DroneControllerCancelable* drone, string server_addre
     pimpl_->server.bind("isOffboardMode", [&]() -> bool { return drone_->isOffboardMode(); });
     pimpl_->server.bind("isSimulationMode", [&]() -> bool { return drone_->isSimulationMode(); });
     pimpl_->server.bind("getServerDebugInfo", [&]() -> std::string { return drone_->getServerDebugInfo(); });
+    pimpl_->server.bind("getCollisionInfo", [&]() -> RpcLibAdapators::CollisionInfo { return drone_->getCollisionInfo(); });
 
     pimpl_->server.suppress_exceptions(true);
 }

Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp

@@ -139,7 +139,9 @@ float MultiRotorConnector::joyStickToRC(int16_t val)
 void MultiRotorConnector::updateRenderedState()
 {
     //move collison info from rendering engine to vehicle
-    vehicle_.setCollisionInfo(vehicle_pawn_->getCollisonInfo());
+    const CollisionInfo& collision_info = vehicle_pawn_->getCollisonInfo();
+    vehicle_.setCollisionInfo(collision_info);
+
     //update ground level
     environment_.getState().min_z_over_ground = vehicle_pawn_->getMinZOverGround();
     //update pose of object for rendering engine