Skip to content

Commit

Permalink
Move preliminary check at the beginning of the function and add docst…
Browse files Browse the repository at this point in the history
…ring to Python API function
  • Loading branch information
meurissemax committed Mar 17, 2021
1 parent 34a15a8 commit e587f84
Show file tree
Hide file tree
Showing 2 changed files with 22 additions and 6 deletions.
14 changes: 8 additions & 6 deletions AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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);

Expand Down
14 changes: 14 additions & 0 deletions PythonClient/airsim/client.py
Original file line number Diff line number Diff line change
Expand Up @@ -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 = ''):
Expand Down

0 comments on commit e587f84

Please sign in to comment.