diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index 4bd95661d8..93c8349c9b 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -90,6 +90,7 @@ class MultirotorApiBase : public VehicleApiBase { 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 moveByVelocityZBodyFrame(float vx, float vy, float z, 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); diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index 93817e9abd..66a378c394 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -25,6 +25,8 @@ class MultirotorRpcLibClient : public RpcLibClientBase { 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* moveByVelocityZBodyFrameAsync(float vx, float vy, float z, 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 = ""); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 0dda206776..6f02fcde03 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -75,22 +75,45 @@ bool MultirotorApiBase::goHome(float timeout_sec) bool MultirotorApiBase::moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) { SingleTaskCall lock(this); + + if (duration <= 0) + return true; + 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)); + 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::moveByVelocityZBodyFrame(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode) +{ + SingleTaskCall lock(this); + if (duration <= 0) return true; - + + 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)); + 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); + moveByVelocityZInternal(vx_new, vy_new, z, 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); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 8b3f59caf6..0783279438 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -83,6 +83,14 @@ MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityBodyFrameAsync(flo return this; } +MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityZBodyFrameAsync(float vx, float vy, float z, float duration, + DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name) +{ + pimpl_->last_future = static_cast(getClient())->async_call("moveByVelocityZBodyFrame", vx, vy, z, 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(getClient())->async_call("moveByMotorPWMs", front_right_pwm, rear_left_pwm, front_left_pwm, rear_right_pwm, duration, vehicle_name); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 6f742fc58e..a170d9067c 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -58,6 +58,11 @@ MultirotorRpcLibServer::MultirotorRpcLibServer(ApiProvider* api_provider, string 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(getServer()))-> + bind("moveByVelocityZBodyFrame", [&](float vx, float vy, float z, float duration, DrivetrainType drivetrain, + const MultirotorRpcLibAdapators::YawMode& yaw_mode, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveByVelocityZBodyFrame(vx, vy, z, duration, drivetrain, yaw_mode.to()); + }); (static_cast(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); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 0d4026261d..d779595de2 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -932,7 +932,24 @@ def moveByVelocityBodyFrameAsync(self, vx, vy, vz, duration, drivetrain = Drivet 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 moveByVelocityZBodyFrameAsync(self, vx, vy, z, 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 + z (float): desired Z value (in local NED frame of the vehicle) + 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('moveByVelocityZBodyFrame', vx, vy, z, 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)