parent adb2b8d

author Ahmed Elsaharti <[email protected]> 1608383085 -0600 committer Ahmed Elsaharti <[email protected]> 1612289167 -0600 Added API Added timestamp added getRotorStates, cleaned up Got rid of global var Generalized RotorVector
microsoft · Feb 2, 2021 · c8436d1 · c8436d1
1 parent 238c8e7
commit c8436d1
Show file tree

Hide file tree

Showing 13 changed files with 159 additions and 1 deletion.
diff --git a/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirLib/include/api/RpcLibAdapatorsBase.hpp
@@ -36,6 +36,64 @@ class RpcLibAdapatorsBase {
             d.push_back(TDest(s.at(i)));
     }
 
+    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
+        {
+            msr::airlib::RotorParameters d;
+
+            d.thrust = thrust;
+            d.torque_scaler = torque_scaler;
+            d.speed = speed;
+
+            return d;
+        }
+    };
+    struct RotorVector {
+        std::vector<RotorParameters> rotor;
+
+        MSGPACK_DEFINE_MAP(rotor);
+
+        RotorVector()
+        {}
+
+        RotorVector(const msr::airlib::RotorVector& s)
+        {
+            rotor.clear();
+            for (unsigned int i = 0; i < s.rotor.size(); i++)
+            {
+                rotor.push_back(RotorParameters());
+                rotor[i] = s.rotor[i];
+            }
+        }
+
+        msr::airlib::RotorVector to() const
+        {
+            msr::airlib::RotorVector d;
+            for (unsigned int i = 0; i < d.rotor.size(); i++)
+            {
+                d.rotor[i].speed = rotor[i].speed;
+                d.rotor[i].thrust = rotor[i].thrust;
+                d.rotor[i].torque_scaler = rotor[i].torque_scaler;
+            }
+            return d;
+        }
+    };
     struct Vector3r {
         msr::airlib::real_T x_val = 0, y_val = 0, z_val = 0;
         MSGPACK_DEFINE_MAP(x_val, y_val, z_val);

diff --git a/AirLib/include/common/CommonStructs.hpp b/AirLib/include/common/CommonStructs.hpp
@@ -8,6 +8,21 @@
 #include <ostream>
 
 namespace msr { namespace airlib {
+//rotor feedback
+struct RotorParameters {
+    real_T thrust = 0;
+    real_T torque_scaler = 0;
+    real_T speed = 0;
+
+    RotorParameters()
+    {}
+};
+struct RotorVector {
+    std::vector<RotorParameters> rotor;
+
+    RotorVector()
+    {}
+};
 
 //velocity
 struct Twist {

diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/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(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

diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorCommon.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorCommon.hpp
@@ -16,7 +16,18 @@ enum class LandedState : uint {
     Landed = 0,
     Flying = 1
 };
+// Struct for rotor state API
+struct RotorStates {
+    RotorVector rotors;
+    uint64_t timestamp;
 
+    RotorStates()
+    {}
+    RotorStates(const RotorVector& 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 {
     bool is_rate = true;

diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibAdapators.hpp
@@ -39,6 +39,27 @@ class MultirotorRpcLibAdapators : public RpcLibAdapatorsBase {
         }
     };
 
+    struct RotorStates {
+        RotorVector rotors;
+        uint64_t timestamp;
+
+        MSGPACK_DEFINE_MAP(rotors, timestamp);
+
+        RotorStates()
+        {}
+
+        RotorStates(const msr::airlib::RotorStates& s)
+        {
+            rotors = s.rotors;
+            timestamp = s.timestamp;
+        }
+
+        msr::airlib::RotorStates to() const
+        {
+            return msr::airlib::RotorStates(rotors.to(), timestamp);
+        }
+    };
+
     struct MultirotorState {
         CollisionInfo collision;
         KinematicsState kinematics_estimated;

diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/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 = "");

diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp
@@ -849,6 +849,5 @@ float MultirotorApiBase::getObsAvoidanceVelocity(float risk_dist, float max_obs_
     unused(risk_dist);
     return max_obs_avoidance_vel;
 }
-
 }} //namespace
 #endif
diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/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).

diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/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()); 

diff --git a/PythonClient/airsim/client.py b/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, trust 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):

diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py
@@ -217,6 +217,8 @@ def nanPose():
     def containsNan(self):
         return (self.position.containsNan() or self.orientation.containsNan())
 
+class RotorVector(MsgpackMixin):
+    parameters = []
 
 class CollisionInfo(MsgpackMixin):
     has_collided = False
@@ -359,6 +361,10 @@ class MultirotorState(MsgpackMixin):
     ready_message = ""
     can_arm = False
 
+class RotorStates(MsgpackMixin):
+    timestamp = np.uint64(0)
+    rotors = RotorVector()
+
 class ProjectionMatrix(MsgpackMixin):
     matrix = []
 

diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp
@@ -68,8 +68,14 @@ void MultirotorPawnSimApi::updateRenderedState(float dt)
     collision_response = multirotor_physics_body_->getCollisionResponseInfo();
 
     //update rotor poses
+    rotor_states_.rotors.rotor.clear();
     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.rotor.push_back(RotorParameters());
+        rotor_states_.rotors.rotor[i].speed = rotor_output.speed;
+        rotor_states_.rotors.rotor[i].thrust = rotor_output.thrust;
+        rotor_states_.rotors.rotor[i].torque_scaler = rotor_output.torque_scaler;
         RotorActuatorInfo* info = &rotor_actuator_info_[i];
         info->rotor_speed = rotor_output.speed;
         info->rotor_direction = static_cast<int>(rotor_output.turning_direction);
@@ -81,6 +87,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)

diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h b/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_;
 };