From 34a15a8c1fbcab6cf239074c1063d0ea854f8220 Mon Sep 17 00:00:00 2001 From: Maxime Meurisse Date: Tue, 16 Mar 2021 14:08:36 +0100 Subject: [PATCH 1/2] Add moveByVelocityZBodyFrame --- .../multirotor/api/MultirotorApiBase.hpp | 1 + .../multirotor/api/MultirotorRpcLibClient.hpp | 2 ++ .../multirotor/api/MultirotorApiBase.cpp | 21 +++++++++++++++++++ .../multirotor/api/MultirotorRpcLibClient.cpp | 8 +++++++ .../multirotor/api/MultirotorRpcLibServer.cpp | 5 +++++ PythonClient/airsim/client.py | 5 ++++- 6 files changed, 41 insertions(+), 1 deletion(-) 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..445b6e6b44 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -91,6 +91,27 @@ bool MultirotorApiBase::moveByVelocityBodyFrame(float vx, float vy, float vz, fl 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); + 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([&]() { + 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..9513478c69 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -932,7 +932,10 @@ 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 = ''): + 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) From e587f848677961abf382a83b721f995b7803c2a1 Mon Sep 17 00:00:00 2001 From: Maxime Meurisse Date: Wed, 17 Mar 2021 13:18:50 +0100 Subject: [PATCH 2/2] Move preliminary check at the beginning of the function and add docstring to Python API function --- .../vehicles/multirotor/api/MultirotorApiBase.cpp | 14 ++++++++------ PythonClient/airsim/client.py | 14 ++++++++++++++ 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 445b6e6b44..6f02fcde03 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -75,14 +75,15 @@ 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)); - 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); @@ -95,14 +96,15 @@ bool MultirotorApiBase::moveByVelocityBodyFrame(float vx, float vy, float vz, fl 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)); - 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); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 9513478c69..d779595de2 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -934,6 +934,20 @@ def moveByVelocityBodyFrameAsync(self, vx, vy, vz, duration, drivetrain = Drivet 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 = ''):