From 7adfbfa5ab1d68e0dfd8c1058cfcefd837934aa6 Mon Sep 17 00:00:00 2001 From: Ahmed Elsaharti Date: Wed, 25 Nov 2020 07:20:01 -0600 Subject: [PATCH 1/2] Added moveByVelocityBodyFrame added an API that works like movebyvelocity but in the vehicle's frame. --- .../multirotor/api/MultirotorApiBase.hpp | 1 + .../multirotor/api/MultirotorRpcLibClient.hpp | 2 ++ .../multirotor/api/MultirotorApiBase.cpp | 19 +++++++++++++++++++ .../multirotor/api/MultirotorRpcLibClient.cpp | 8 ++++++++ .../multirotor/api/MultirotorRpcLibServer.cpp | 5 +++++ PythonClient/airsim/client.py | 16 ++++++++++++++++ 6 files changed, 51 insertions(+) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index b95c22211e..d0a0e1c86d 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/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); diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index 05e63cbc9b..59eb996ac5 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/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(), 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 = ""); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 6bd1d4c191..7d3e4ba6f2 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/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) +{ + 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)); + SingleTaskCall lock(this); + + 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); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 769fad6849..717106f232 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/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(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(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 9035b304f3..10e650e620 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/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(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(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 6d619df68f..3240b7d600 100644 --- a/PythonClient/airsim/client.py +++ b/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) From 967ad1601985921236d350d3a2848fa07d54f327 Mon Sep 17 00:00:00 2001 From: Ahmed Elsaharti Date: Tue, 1 Dec 2020 15:24:43 -0600 Subject: [PATCH 2/2] Minor requested changes --- AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp | 4 ++-- PythonClient/airsim/client.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 7d3e4ba6f2..0dda206776 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -74,12 +74,12 @@ 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); 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)); - SingleTaskCall lock(this); - + if (duration <= 0) return true; diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 3240b7d600..ca8abc481b 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -839,7 +839,7 @@ def moveByVelocityBodyFrameAsync(self, vx, vy, vz, duration, drivetrain = Drivet """ 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. + 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):