full kinematics API for car and multirotor

AirLib/AirLib.vcxproj

@@ -138,7 +138,7 @@
     <ClInclude Include="include\vehicles\car\api\CarRpcLibClient.hpp" />
     <ClInclude Include="include\vehicles\car\api\CarRpcLibServer.hpp" />
     <ClInclude Include="include\safety\SafetyEval.hpp" />
-    <ClInclude Include="include\vehicles\multirotor\api\DroneApi.hpp" />
+    <ClInclude Include="include\vehicles\multirotor\api\MultirotorApi.hpp" />
     <ClInclude Include="include\controllers\Settings.hpp" />
     <ClInclude Include="include\safety\SphereGeoFence.hpp" />
     <ClInclude Include="include\controllers\VehicleControllerBase.hpp" />

AirLib/AirLib.vcxproj.filters

@@ -450,9 +450,6 @@
     <ClInclude Include="include\api\RpcLibClientBase.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="include\vehicles\multirotor\api\DroneApi.hpp">
-      <Filter>Header Files</Filter>
-    </ClInclude>
     <ClInclude Include="include\api\VehicleApiBase.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
@@ -468,6 +465,9 @@
     <ClInclude Include="include\controllers\ImageCaptureBase.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\vehicles\multirotor\api\MultirotorApi.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\safety\ObstacleMap.cpp">

AirLib/include/api/RpcLibAdapatorsBase.hpp

@@ -6,6 +6,7 @@
 
 #include "common/Common.hpp"
 #include "common/CommonStructs.hpp"
+#include "physics/Kinematics.hpp"
 #include "controllers/ImageCaptureBase.hpp"
 #include "safety/SafetyEval.hpp"
 #include "rpc/msgpack.hpp"
@@ -196,6 +197,46 @@ class RpcLibAdapatorsBase {
             return d;
         }
     };
+
+    struct KinematicsState {
+        Vector3r position;
+        Quaternionr orientation;
+
+        Vector3r linear_velocity;
+        Vector3r angular_velocity;
+
+        Vector3r linear_acceleration;
+        Vector3r angular_acceleration;
+
+        MSGPACK_DEFINE_MAP(position, orientation, linear_velocity, angular_velocity, linear_acceleration, angular_acceleration);
+
+
+        KinematicsState()
+        {}
+
+        KinematicsState(const msr::airlib::Kinematics::State& s)
+        {
+            position = s.pose.position;
+            orientation = s.pose.orientation;
+            linear_velocity = s.twist.linear;
+            angular_velocity = s.twist.angular;
+            linear_acceleration = s.accelerations.linear;
+            angular_acceleration = s.accelerations.angular;
+        }
+
+        msr::airlib::Kinematics::State to() const
+        {
+            msr::airlib::Kinematics::State s;
+            s.pose.position = position.to();
+            s.pose.orientation = orientation.to();
+            s.twist.linear = linear_velocity.to();
+            s.twist.angular = angular_velocity.to();
+            s.accelerations.linear = linear_acceleration.to();
+            s.accelerations.angular = angular_acceleration.to();
+
+            return s;
+        }
+    };
 
     struct ImageRequest {
         uint8_t camera_id;

AirLib/include/controllers/VehicleConnectorBase.hpp

@@ -33,6 +33,7 @@ class VehicleConnectorBase : public UpdatableObject
     virtual int getSegmentationObjectID(const std::string& mesh_name) = 0;
     virtual void printLogMessage(const std::string& message, std::string message_param = "", unsigned char severity = 0) = 0;
     virtual Pose getActorPose(const std::string& actor_name) = 0;
+    virtual Kinematics::State getTrueKinematics() = 0;
 
 };
 

AirLib/include/vehicles/car/api/CarApiBase.hpp

@@ -8,6 +8,7 @@
 #include "common/VectorMath.hpp"
 #include "common/CommonStructs.hpp"
 #include "api/VehicleApiBase.hpp"
+#include "physics/Kinematics.hpp"
 
 namespace msr { namespace airlib {
 
@@ -49,16 +50,13 @@ class CarApiBase : public VehicleApiBase  {
     struct CarState {
         float speed;
         int gear;
-        Vector3r position;
-        Vector3r velocity;
-        Quaternionr orientation;
         CollisionInfo collision;
+        Kinematics::State kinematics_true;
         uint64_t timestamp;
 
-        CarState(float speed_val, int gear_val, const Vector3r& position_val, const Vector3r& velocity_val,
-            const Quaternionr& orientation_val, const CollisionInfo& collision_val, uint64_t timestamp_val)
-            : speed(speed_val), gear(gear_val), position(position_val), velocity(velocity_val),
-            orientation(orientation_val), collision(collision_val), timestamp(timestamp_val)
+        CarState(float speed_val, int gear_val, const CollisionInfo& collision_val, 
+            const Kinematics::State& kinematics_true_val, uint64_t timestamp_val)
+            : speed(speed_val), gear(gear_val), collision(collision_val), kinematics_true(kinematics_true_val), timestamp(timestamp_val)
         {
         }
     };

AirLib/include/vehicles/car/api/CarRpcLibAdapators.hpp

@@ -50,13 +50,11 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase {
     struct CarState {
         float speed;
         int gear;
-        Vector3r position;
-        Vector3r velocity;
-        Quaternionr orientation;
         CollisionInfo collision;
+        KinematicsState kinematics_true; //ground truth
         uint64_t timestamp;
 
-        MSGPACK_DEFINE_MAP(speed, gear, position, velocity, orientation, collision, timestamp);
+        MSGPACK_DEFINE_MAP(speed, gear, collision, kinematics_true, timestamp);
 
         CarState()
         {}
@@ -65,16 +63,14 @@ class CarRpcLibAdapators : public RpcLibAdapatorsBase {
         {
             speed = s.speed;
             gear = s.gear;
-            position = s.position;
-            velocity = s.velocity;
-            orientation = s.orientation;
             collision = s.collision;
+            kinematics_true = s.kinematics_true;
             timestamp = s.timestamp;
         }
         msr::airlib::CarApiBase::CarState to() const
         {
             return msr::airlib::CarApiBase::CarState(
-                speed, gear, position.to(), velocity.to(), orientation.to(), collision.to(), timestamp);
+                speed, gear, collision.to(), kinematics_true.to(), timestamp);
         }
     };
 };

AirLib/include/vehicles/multirotor/api/DroneApi.hpp → AirLib/include/vehicles/multirotor/api/MultirotorApi.hpp

@@ -31,10 +31,9 @@ namespace msr { namespace airlib {
 // here that only one operation is allowed at a time and that is what the CallLock object is for.  It cancels previous operation then
 // sets up the new operation.
 
-class DroneApi : public VehicleApiBase {
-
+class MultirotorApi : public VehicleApiBase {
 public:
-    DroneApi(VehicleConnectorBase* vehicle)
+    MultirotorApi(VehicleConnectorBase* vehicle)
         : vehicle_(vehicle)
     {
         controller_ = static_cast<DroneControllerBase*>(vehicle->getController());
@@ -43,7 +42,7 @@ class DroneApi : public VehicleApiBase {
         //auto fence = std::make_shared<CubeGeoFence>(VectorMath::Vector3f(-1E10, -1E10, -1E10), VectorMath::Vector3f(1E10, 1E10, 1E10), vehicle_params.distance_accuracy);
         //auto safety_eval = std::make_shared<SafetyEval>(vehicle_params, fence);
     }
-    virtual ~DroneApi() = default;
+    virtual ~MultirotorApi() = default;
 
     bool armDisarm(bool arm)
     {
@@ -157,48 +156,39 @@ class DroneApi : public VehicleApiBase {
         return enqueueCommand(cmd);
     }
 
-    //status getters
-    //TODO: add single call to get all of the state
-    Vector3r getPosition()
+    /************************* State APIs *********************************/
+    MultirotorState getMultirotorState()
     {
-        return controller_->getPosition();
-    }
+        MultirotorState state;
+        state.kinematics_estimated = controller_->getKinematicsEstimated();
+        state.collision = controller_->getCollisionInfo();
+        state.kinematics_true = vehicle_->getTrueKinematics();
+        state.gps_location = controller_->getGpsLocation();
+        state.timestamp = controller_->clock()->nowNanos();
 
-    Vector3r getVelocity()
-    {
-        return controller_->getVelocity();
-    }
-
-    virtual void simSetPose(const Pose& pose, bool ignore_collision) override
-    {
-        vehicle_->setPose(pose, ignore_collision);
-    }
-    virtual Pose simGetPose() override
-    {
-        return vehicle_->getPose();
+        return state;
     }
 
-    virtual bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id,
-        bool is_name_regex = false) override
+    Vector3r getPosition()
     {
-        return vehicle_->setSegmentationObjectID(mesh_name, object_id, is_name_regex);
+        return controller_->getPosition();
     }
 
-    virtual int simGetSegmentationObjectID(const std::string& mesh_name) override
+    Vector3r getVelocity()
     {
-        return vehicle_->getSegmentationObjectID(mesh_name);
+        return controller_->getVelocity();
     }
 
     Quaternionr getOrientation()
     {
         return controller_->getOrientation();
     }
+
     DroneControllerBase::LandedState getLandedState()
     {
         return controller_->getLandedState();
     }
 
-
     virtual CollisionInfo getCollisionInfo() override
     {
         return controller_->getCollisionInfo();
@@ -208,10 +198,6 @@ class DroneApi : public VehicleApiBase {
     {
         return controller_->getRCData();
     }
-    TTimePoint timestampNow()
-    {
-        return controller_->clock()->nowNanos();
-    }
 
     //TODO: add GPS health, accuracy in API
     GeoPoint getGpsLocation()
@@ -223,11 +209,6 @@ class DroneApi : public VehicleApiBase {
     {
         return controller_->isSimulationMode();
     }
-    std::string getServerDebugInfo()
-    {
-        //for now this method just allows to see if server was started
-        return std::to_string(Utils::getUnixTimeStamp());
-    }
 
     void getStatusMessages(std::vector<std::string>& messages)
     {
@@ -239,6 +220,7 @@ class DroneApi : public VehicleApiBase {
         offboard_thread_.cancel();
     }
 
+
     /******************* VehicleApiBase implementtaion ********************/
     virtual GeoPoint getHomeGeoPoint() override
     {
@@ -278,6 +260,26 @@ class DroneApi : public VehicleApiBase {
         return vehicle_->getActorPose(actor_name);
     }
 
+    virtual void simSetPose(const Pose& pose, bool ignore_collision) override
+    {
+        vehicle_->setPose(pose, ignore_collision);
+    }
+    virtual Pose simGetPose() override
+    {
+        return vehicle_->getPose();
+    }
+
+    virtual bool simSetSegmentationObjectID(const std::string& mesh_name, int object_id,
+        bool is_name_regex = false) override
+    {
+        return vehicle_->setSegmentationObjectID(mesh_name, object_id, is_name_regex);
+    }
+
+    virtual int simGetSegmentationObjectID(const std::string& mesh_name) override
+    {
+        return vehicle_->getSegmentationObjectID(mesh_name);
+    }
+
 
     virtual bool isApiControlEnabled() const override
     {

AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp

@@ -37,6 +37,33 @@ class MultirotorRpcLibAdapators : public RpcLibAdapatorsBase {
         }
     };
 
+    struct MultirotorState {
+        CollisionInfo collision;
+        KinematicsState kinematics_estimated;
+        KinematicsState kinematics_true;
+        GeoPoint gps_location;
+        uint64_t timestamp;
+
+        MSGPACK_DEFINE_MAP(collision, kinematics_estimated, kinematics_true, gps_location, timestamp);
+
+        MultirotorState()
+        {}
+
+        MultirotorState(const msr::airlib::MultirotorState& s)
+        {
+            collision = s.collision;
+            kinematics_estimated = s.kinematics_estimated;
+            kinematics_true = s.kinematics_true;
+            gps_location = s.gps_location;
+            timestamp = s.timestamp;
+        }
+
+        msr::airlib::MultirotorState to() const
+        {
+            return msr::airlib::MultirotorState(collision.to(), kinematics_estimated.to(), 
+                kinematics_true.to(), gps_location.to(), timestamp);
+        }
+    };
 };
 
 }} //namespace

AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp

@@ -41,7 +41,7 @@ class MultirotorRpcLibClient : public RpcLibClientBase {
     bool rotateByYawRate(float yaw_rate, float duration);
     bool hover();
 
-
+    MultirotorState getMultirotorState();
     Vector3r getPosition();
     Vector3r getVelocity();
     Quaternionr getOrientation();
@@ -51,7 +51,6 @@ class MultirotorRpcLibClient : public RpcLibClientBase {
     std::string getDebugInfo();
 
     DroneControllerBase::LandedState getLandedState();
-    TTimePoint timestampNow();
 
     bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
         float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z);

AirLib/include/vehicles/multirotor/api/MultirotorRpcLibServer.hpp

@@ -6,19 +6,19 @@
 
 #include "common/Common.hpp"
 #include <functional>
-#include "vehicles/multirotor/api/DroneApi.hpp"
+#include "vehicles/multirotor/api/MultirotorApi.hpp"
 #include "api/RpcLibServerBase.hpp"
 
 
 namespace msr { namespace airlib {
 
 class MultirotorRpcLibServer : public RpcLibServerBase {
 public:
-    MultirotorRpcLibServer(DroneApi* drone, string server_address, uint16_t port = 41451);
+    MultirotorRpcLibServer(MultirotorApi* drone, string server_address, uint16_t port = 41451);
     virtual ~MultirotorRpcLibServer();
 
 private:
-    DroneApi* getDroneApi();
+    MultirotorApi* getDroneApi();
 };
 
 }} //namespace