Rotor output API

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

@@ -123,6 +123,11 @@ class MultirotorApiBase : public VehicleApiBase {
         float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z);
 
     /************************* high level status APIs *********************************/
+    RotorStates getRotorStates() const
+    {
+        return rotor_states_;
+    }
+
     MultirotorState getMultirotorState() const
     {
         MultirotorState state;
@@ -143,6 +148,12 @@ class MultirotorApiBase : public VehicleApiBase {
         token_.cancel();
     }
 
+    /******************* rotors' states setter ********************/
+    void setRotorStates(const RotorStates& rotor_states)
+    {
+        rotor_states_ = rotor_states;
+    }
+
 protected: //utility methods
     typedef std::function<bool()> WaitFunction;
 
@@ -341,6 +352,7 @@ class MultirotorApiBase : public VehicleApiBase {
     float landing_vel_ = 0.2f; //velocity to use for landing
     float approx_zero_vel_ = 0.05f;
     float approx_zero_angular_vel_ = 0.01f;
+    RotorStates rotor_states_;
 };
 
 }} //namespace

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

@@ -16,6 +16,39 @@ enum class LandedState : uint {
     Landed = 0,
     Flying = 1
 };
+// Structs for rotor state API
+struct RotorParameters {
+    real_T thrust = 0;
+    real_T torque_scaler = 0;
+    real_T speed = 0;
+
+    RotorParameters()
+    {}
+
+    RotorParameters(const real_T& thrust_val, const real_T& torque_scaler_val, const real_T& speed_val)
+        : thrust(thrust_val), torque_scaler(torque_scaler_val), speed(speed_val)
+    {
+    }
+
+    void update(const real_T& thrust_val, const real_T& torque_scaler_val, const real_T& speed_val)
+    {
+        thrust = thrust_val;
+        torque_scaler = torque_scaler_val;
+        speed = speed_val;
+    }
+};
+
+struct RotorStates {
+    std::vector<RotorParameters> rotors;
+    uint64_t timestamp;
+
+    RotorStates()
+    {}
+    RotorStates(const std::vector<RotorParameters>& rotors_val, uint64_t timestamp_val)
+        : rotors(rotors_val), timestamp(timestamp_val)
+    {
+    }
+};
 
 //Yaw mode specifies if yaw should be set as angle or angular velocity around the center of drone
 struct YawMode {

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

@@ -39,6 +39,58 @@ class MultirotorRpcLibAdapators : public RpcLibAdapatorsBase {
         }
     };
 
+    struct RotorParameters {
+        msr::airlib::real_T thrust;
+        msr::airlib::real_T torque_scaler;
+        msr::airlib::real_T speed;
+
+        MSGPACK_DEFINE_MAP(thrust, torque_scaler, speed);
+
+        RotorParameters()
+        {}
+
+        RotorParameters(const msr::airlib::RotorParameters& s)
+        {
+            thrust = s.thrust;
+            torque_scaler = s.torque_scaler;
+            speed = s.speed;
+        }
+
+        msr::airlib::RotorParameters to() const
+        {
+            return msr::airlib::RotorParameters(thrust, torque_scaler, speed);
+        }
+    };
+
+    struct RotorStates {
+        std::vector<RotorParameters> rotors;
+        uint64_t timestamp;
+
+        MSGPACK_DEFINE_MAP(rotors, timestamp);
+
+        RotorStates()
+        {}
+
+        RotorStates(const msr::airlib::RotorStates& s)
+        {
+            for (const auto& r : s.rotors)
+            {
+                rotors.push_back(RotorParameters(r));
+            }
+            timestamp = s.timestamp;
+        }
+
+        msr::airlib::RotorStates to() const
+        {
+            std::vector<msr::airlib::RotorParameters> d;
+            for (const auto& r : rotors)
+            {
+                d.push_back(r.to());
+            }
+            return msr::airlib::RotorStates(d, timestamp);
+        }
+    };
+
     struct MultirotorState {
         CollisionInfo collision;
         KinematicsState kinematics_estimated;

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

@@ -57,6 +57,7 @@ class MultirotorRpcLibClient : public RpcLibClientBase {
     void moveByRC(const RCData& rc_data, const std::string& vehicle_name = "");
 
     MultirotorState getMultirotorState(const std::string& vehicle_name = "");
+    RotorStates getRotorStates(const std::string& vehicle_name = "");
 
     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, const std::string& vehicle_name = "");

AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp

@@ -221,6 +221,13 @@ bool MultirotorRpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_re
 }
 
 //status getters
+// Rotor state getter
+RotorStates MultirotorRpcLibClient::getRotorStates(const std::string& vehicle_name)
+{
+    return static_cast<rpc::client*>(getClient())->call("getRotorStates", vehicle_name).
+        as<MultirotorRpcLibAdapators::RotorStates>().to();
+}
+// Multirotor state getter
 MultirotorState MultirotorRpcLibClient::getMultirotorState(const std::string& vehicle_name)
 {
     return static_cast<rpc::client*>(getClient())->call("getMultirotorState", vehicle_name).

AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp

@@ -166,6 +166,12 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string
     });
 
     //getters
+    // Rotor state
+    (static_cast<rpc::server*>(getServer()))->
+        bind("getRotorStates", [&](const std::string& vehicle_name) -> MultirotorRpcLibAdapators::RotorStates {
+        return MultirotorRpcLibAdapators::RotorStates(getVehicleApi(vehicle_name)->getRotorStates());
+    });
+    // Multirotor state
     (static_cast<rpc::server*>(getServer()))->
         bind("getMultirotorState", [&](const std::string& vehicle_name) -> MultirotorRpcLibAdapators::MultirotorState {
         return MultirotorRpcLibAdapators::MultirotorState(getVehicleApi(vehicle_name)->getMultirotorState()); 

PythonClient/airsim/client.py

@@ -1282,7 +1282,20 @@ def getMultirotorState(self, vehicle_name = ''):
         """
         return MultirotorState.from_msgpack(self.client.call('getMultirotorState', vehicle_name))
     getMultirotorState.__annotations__ = {'return': MultirotorState}
+    # query rotor states
+    def getRotorStates(self, vehicle_name = ''):
+        """
+        Used to obtain the current state of all a multirotor's rotors. The state includes the speeds,
+        thrusts and torques for all rotors.
+
+        Args:
+            vehicle_name (str, optional): Vehicle to get the rotor state of
 
+        Returns:
+            RotorStates: Containing a timestamp and the speed, thrust and torque of all rotors.
+        """
+        return RotorStates.from_msgpack(self.client.call('getRotorStates', vehicle_name))
+    getRotorStates.__annotations__ = {'return': RotorStates}
 
 # -----------------------------------  Car APIs ---------------------------------------------
 class CarClient(VehicleClient, object):

PythonClient/airsim/types.py

@@ -359,6 +359,10 @@ class MultirotorState(MsgpackMixin):
     ready_message = ""
     can_arm = False
 
+class RotorStates(MsgpackMixin):
+    timestamp = np.uint64(0)
+    rotors = []
+
 class ProjectionMatrix(MsgpackMixin):
     matrix = []
 

Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp

@@ -33,6 +33,7 @@ void MultirotorPawnSimApi::initialize()
     pending_pose_status_ = PendingPoseStatus::NonePending;
     reset_pending_ = false;
     did_reset_ = false;
+    rotor_states_.rotors.assign(rotor_count_, RotorParameters());
 
 	//reset roll & pitch of vehicle as multirotors required to be on plain surface at start
     Pose pose = getPose();
@@ -70,6 +71,8 @@ void MultirotorPawnSimApi::updateRenderedState(float dt)
     //update rotor poses
     for (unsigned int i = 0; i < rotor_count_; ++i) {
         const auto& rotor_output = multirotor_physics_body_->getRotorOutput(i);
+        // update private rotor variable
+        rotor_states_.rotors[i].update(rotor_output.thrust, rotor_output.torque_scaler, rotor_output.speed);
         RotorActuatorInfo* info = &rotor_actuator_info_[i];
         info->rotor_speed = rotor_output.speed;
         info->rotor_direction = static_cast<int>(rotor_output.turning_direction);
@@ -81,6 +84,8 @@ void MultirotorPawnSimApi::updateRenderedState(float dt)
 
     if (getRemoteControlID() >= 0)
         vehicle_api_->setRCData(getRCData());
+    rotor_states_.timestamp = clock()->nowNanos();
+    vehicle_api_->setRotorStates(rotor_states_);
 }
 
 void MultirotorPawnSimApi::updateRendering(float dt)

Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h

@@ -83,4 +83,5 @@ class MultirotorPawnSimApi : public PawnSimApi
 
     Pose last_phys_pose_; //for trace lines showing vehicle path
     std::vector<std::string> vehicle_api_messages_;
+    RotorStates rotor_states_;
 };