Merge pull request #3169 from ahmed-elsaharti/Local-move-API

Added moveByVelocityBodyFrame

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

@@ -89,6 +89,7 @@ class MultirotorApiBase : public VehicleApiBase {
     virtual bool land(float timeout_sec);
     virtual bool goHome(float timeout_sec);
 
+    virtual bool moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode);
     virtual bool moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration);
     virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration);
     virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration);

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

@@ -23,6 +23,8 @@ class MultirotorRpcLibClient : public RpcLibClientBase {
     MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = "");
     MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max<float>(), const std::string& vehicle_name = "");
 
+    MultirotorRpcLibClient* moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration,
+        DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = "");
     MultirotorRpcLibClient* moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name = "");
     MultirotorRpcLibClient* moveByRollPitchYawZAsync(float roll, float pitch, float yaw, float z, float duration, const std::string& vehicle_name = "");
     MultirotorRpcLibClient* moveByRollPitchYawThrottleAsync(float roll, float pitch, float yaw, float throttle, float duration, const std::string& vehicle_name = "");

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

@@ -72,6 +72,25 @@ bool MultirotorApiBase::goHome(float timeout_sec)
     return moveToPosition(0, 0, 0, 0.5f, timeout_sec, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1);
 }
 
+bool MultirotorApiBase::moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode)
+{
+    SingleTaskCall lock(this);
+    float pitch, roll, yaw;
+    VectorMath::toEulerianAngle(getKinematicsEstimated().pose.orientation, pitch, roll, yaw);
+    float vx_new = (vx * (float)std::cos(yaw)) - (vy * (float)std::sin(yaw));
+    float vy_new = (vx * (float)std::sin(yaw)) + (vy * (float)std::cos(yaw));
+
+    if (duration <= 0)
+        return true;
+
+    YawMode adj_yaw_mode(yaw_mode.is_rate, yaw_mode.yaw_or_rate);
+    adjustYaw(vx_new, vy_new, drivetrain, adj_yaw_mode);
+
+    return waitForFunction([&]() {
+        moveByVelocityInternal(vx_new, vy_new, vz, adj_yaw_mode);
+        return false; //keep moving until timeout
+        }, duration).isTimeout();
+}
 bool MultirotorApiBase::moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration)
 {
     SingleTaskCall lock(this);

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

@@ -75,6 +75,14 @@ MultirotorRpcLibClient* MultirotorRpcLibClient::goHomeAsync(float timeout_sec, c
     return this;
 }
 
+MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration,
+    DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name)
+{
+    pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveByVelocityBodyFrame", vx, vy, vz, duration,
+        drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name);
+    return this;
+}
+
 MultirotorRpcLibClient* MultirotorRpcLibClient::moveByMotorPWMsAsync(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name)
 {
     pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveByMotorPWMs", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name);

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

@@ -53,6 +53,11 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string
         bind("goHome", [&](float timeout_sec, const std::string& vehicle_name) -> bool { 
         return getVehicleApi(vehicle_name)->goHome(timeout_sec); 
     });
+    (static_cast<rpc::server*>(getServer()))->
+        bind("moveByVelocityBodyFrame", [&](float vx, float vy, float vz, float duration, DrivetrainType drivetrain,
+            const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool {
+        return getVehicleApi(vehicle_name)->moveByVelocityBodyFrame(vx, vy, vz, duration, drivetrain, yaw_mode.to());
+    });
     (static_cast<rpc::server*>(getServer()))->
         bind("moveByMotorPWMs", [&](float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration, const std::string& vehicle_name) ->
         bool { return getVehicleApi(vehicle_name)->moveByMotorPWMs(front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration);

PythonClient/airsim/client.py

@@ -835,6 +835,22 @@ def goHomeAsync(self, timeout_sec = 3e+38, vehicle_name = ''):
         return self.client.call_async('goHome', timeout_sec, vehicle_name)
 
     # APIs for control
+    def moveByVelocityBodyFrameAsync(self, vx, vy, vz, duration, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), vehicle_name = ''):
+        """
+        Args:
+            vx (float): desired velocity in the X axis of the vehicle's local NED frame.
+            vy (float): desired velocity in the Y axis of the vehicle's local NED frame.
+            vz (float): desired velocity in the Z axis of the vehicle's local NED frame.
+            duration (float): Desired amount of time (seconds), to send this command for
+            drivetrain (DrivetrainType, optional):
+            yaw_mode (YawMode, optional):
+            vehicle_name (str, optional): Name of the multirotor to send this command to
+
+        Returns:
+            msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join()
+        """
+        return self.client.call_async('moveByVelocityBodyFrame', vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name)
+
     def moveByAngleZAsync(self, pitch, roll, z, yaw, duration, vehicle_name = ''):
         return self.client.call_async('moveByAngleZ', pitch, roll, z, yaw, duration, vehicle_name)