diff --git a/AirLib/include/common/CommonStructs.hpp b/AirLib/include/common/CommonStructs.hpp index 04af12dc5e..95c440f94d 100644 --- a/AirLib/include/common/CommonStructs.hpp +++ b/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 { diff --git a/AirLib/include/controllers/DroneControllerBase.hpp b/AirLib/include/controllers/DroneControllerBase.hpp index ace1936ef0..28481a0a7d 100644 --- a/AirLib/include/controllers/DroneControllerBase.hpp +++ b/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 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 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. diff --git a/AirLib/include/controllers/DroneControllerCancelable.hpp b/AirLib/include/controllers/DroneControllerCancelable.hpp index 0998935954..5feb0cfba9 100644 --- a/AirLib/include/controllers/DroneControllerCancelable.hpp +++ b/AirLib/include/controllers/DroneControllerCancelable.hpp @@ -189,6 +189,11 @@ class DroneControllerCancelable { return controller_->getLandedState(); } + CollisionInfo getCollisionInfo() + { + return controller_->getCollisionInfo(); + } + RCData getRCData() { return controller_->getRCData(); diff --git a/AirLib/include/physics/PhysicsBody.hpp b/AirLib/include/physics/PhysicsBody.hpp index 6bc5de75e9..c34fb3dc41 100644 --- a/AirLib/include/physics/PhysicsBody.hpp +++ b/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; } diff --git a/AirLib/include/rpc/RpcLibAdapators.hpp b/AirLib/include/rpc/RpcLibAdapators.hpp index 7b86c3a396..7ff0b4dcf8 100644 --- a/AirLib/include/rpc/RpcLibAdapators.hpp +++ b/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_; diff --git a/AirLib/include/rpc/RpcLibClient.hpp b/AirLib/include/rpc/RpcLibClient.hpp index ec83230a86..e8511cd657 100644 --- a/AirLib/include/rpc/RpcLibClient.hpp +++ b/AirLib/include/rpc/RpcLibClient.hpp @@ -43,6 +43,7 @@ class RpcLibClient { bool hover(); Vector3r getPosition(); + CollisionInfo getCollisionInfo(); Vector3r getVelocity(); Quaternionr getOrientation(); DroneControllerBase::LandedState getLandedState(); diff --git a/AirLib/include/vehicles/MultiRotor.hpp b/AirLib/include/vehicles/MultiRotor.hpp index 62370217b6..8384f9e036 100644 --- a/AirLib/include/vehicles/MultiRotor.hpp +++ b/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 diff --git a/AirLib/src/controllers/DroneControllerBase.cpp b/AirLib/src/controllers/DroneControllerBase.cpp index 6b529c21d0..ecd66505ff 100644 --- a/AirLib/src/controllers/DroneControllerBase.cpp +++ b/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 diff --git a/AirLib/src/rpc/RpcLibClient.cpp b/AirLib/src/rpc/RpcLibClient.cpp index 75ed921ca8..d77f46b4a8 100644 --- a/AirLib/src/rpc/RpcLibClient.cpp +++ b/AirLib/src/rpc/RpcLibClient.cpp @@ -152,12 +152,10 @@ Vector3r RpcLibClient::getPosition() { return pimpl_->client.call("getPosition").as().to(); } - Vector3r RpcLibClient::getVelocity() { return pimpl_->client.call("getVelocity").as().to(); } - Quaternionr RpcLibClient::getOrientation() { return pimpl_->client.call("getOrientation").as().to(); @@ -174,6 +172,11 @@ RCData RpcLibClient::getRCData() return pimpl_->client.call("getRCData").as().to(); } +CollisionInfo RpcLibClient::getCollisionInfo() +{ + return pimpl_->client.call("getCollisionInfo").as().to(); +} + TTimePoint RpcLibClient::timestampNow() { return pimpl_->client.call("timestampNow").as(); diff --git a/AirLib/src/rpc/RpcLibServer.cpp b/AirLib/src/rpc/RpcLibServer.cpp index cb620ec1dc..55805b589d 100644 --- a/AirLib/src/rpc/RpcLibServer.cpp +++ b/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); } diff --git a/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp b/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp index 30edd6f408..79effe2b59 100644 --- a/Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp +++ b/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