Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added moveByVelocityBodyFrame #3169

Merged
merged 2 commits into from
Dec 2, 2020
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 = "");
Expand Down
19 changes: 19 additions & 0 deletions AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
8 changes: 8 additions & 0 deletions AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
5 changes: 5 additions & 0 deletions AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
16 changes: 16 additions & 0 deletions PythonClient/airsim/client.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down