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.

Sign up for GitHub

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

Jump to bottom

Added movetoGPS #3746

Merged
merged 6 commits into from
Dec 2, 2021
Merged

Added movetoGPS #3746

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
996d4c2
Added movetoGPS API
ahmed-elsaharti Feb 28, 2021
842787d
added GeodeticToNed util function
ahmed-elsaharti Mar 1, 2021
2a68ccc
use native getHomeGeoPoint()
ahmed-elsaharti Mar 9, 2021
a8bbbfa
apply style from clang-format
zimmy87 Dec 2, 2021
d694639
Merge remote-tracking branch 'upstream/master' into movetoGPS
zimmy87 Dec 2, 2021
0645a77
Update EarthUtils.hpp
zimmy87 Dec 2, 2021
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
Prev Previous commit
Next Next commit
Merge remote-tracking branch 'upstream/master' into movetoGPS
  • Loading branch information
@zimmy87
zimmy87 committed Dec 2, 2021
commit d69463969365181f9148ea7a27d661ec54c0ce48
14 changes: 13 additions & 1 deletion AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,7 @@ namespace airlib
virtual bool moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain,
const YawMode& yaw_mode, float lookahead, float adaptive_lookahead);
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);
Expand Down Expand Up @@ -127,6 +128,11 @@ namespace airlib
float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z);

/************************* high level status APIs *********************************/
const RotorStates& getRotorStates() const
{
return rotor_states_;
}

MultirotorState getMultirotorState() const
{
MultirotorState state;
Expand All @@ -147,6 +153,12 @@ namespace airlib
token_.cancel();
}

/******************* rotors' states setter ********************/
void setRotorStates(const RotorStates& rotor_states)
{
rotor_states_ = rotor_states;
}

protected: //utility methods
typedef std::function<bool()> WaitFunction;

Expand Down Expand Up @@ -354,8 +366,8 @@ namespace airlib
float landing_vel_ = 0.2f; //velocity to use for landing
float approx_zero_vel_ = 0.05f;
float approx_zero_angular_vel_ = 0.01f;
RotorStates rotor_states_;
};

}
} //namespace
#endif
4 changes: 3 additions & 1 deletion AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@ namespace airlib
float lookahead = -1, float adaptive_lookahead = 1, 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* 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 = "");
Expand Down Expand Up @@ -63,6 +65,7 @@ namespace airlib
void moveByRC(const RCData& rc_data, const std::string& vehicle_name = "");

MultirotorState getMultirotorState(const std::string& vehicle_name = "");
RotorStates getRotorStates(const std::string& vehicle_name = "");

bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name = "");
Expand All @@ -75,7 +78,6 @@ namespace airlib
struct impl;
std::unique_ptr<impl> pimpl_;
};

}
} //namespace
#endif
30 changes: 27 additions & 3 deletions AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,24 +86,49 @@ namespace airlib
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);
Expand Down Expand Up @@ -756,7 +781,7 @@ namespace airlib
//send commands
//try to maintain altitude if path was in XY plan only, velocity based control is not as good
if (std::abs(cur.z() - dest.z()) <= getDistanceAccuracy()) //for paths in XY plan current code leaves z untouched, so we can compare with strict equality
moveByVelocityZInternal(velocity_vect.x(), velocity_vect.y(), dest.z(), yaw_mode);
moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), 0, yaw_mode);
else
moveByVelocityInternal(velocity_vect.x(), velocity_vect.y(), velocity_vect.z(), yaw_mode);
}
Expand Down Expand Up @@ -902,7 +927,6 @@ namespace airlib
unused(risk_dist);
return max_obs_avoidance_vel;
}

}
} //namespace
#endif
42 changes: 27 additions & 15 deletions AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ __pragma(warning(disable : 4239))
namespace airlib
{

typedef msr::airlib_rpclib::MultirotorRpcLibAdapators MultirotorRpcLibAdapators;
typedef msr::airlib_rpclib::MultirotorRpcLibAdaptors MultirotorRpcLibAdaptors;

struct MultirotorRpcLibClient::impl
{
Expand Down Expand Up @@ -80,7 +80,14 @@ __pragma(warning(disable : 4239))
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);
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveByVelocityBodyFrame", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name);
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<rpc::client*>(getClient())->async_call("moveByVelocityZBodyFrame", vx, vy, z, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name);
return this;
}

Expand Down Expand Up @@ -129,50 +136,50 @@ __pragma(warning(disable : 4239))
MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityAsync(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("moveByVelocity", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name);
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveByVelocity", vx, vy, vz, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name);
return this;
}

MultirotorRpcLibClient* MultirotorRpcLibClient::moveByVelocityZAsync(float vx, float vy, float z, float duration,
DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name)
{
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveByVelocityZ", vx, vy, z, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name);
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveByVelocityZ", vx, vy, z, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name);
return this;
}

MultirotorRpcLibClient* MultirotorRpcLibClient::moveOnPathAsync(const vector<Vector3r>& path, float velocity, float duration,
DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name)
{
vector<MultirotorRpcLibAdapators::Vector3r> conv_path;
MultirotorRpcLibAdapators::from(path, conv_path);
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveOnPath", conv_path, velocity, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name);
vector<MultirotorRpcLibAdaptors::Vector3r> conv_path;
MultirotorRpcLibAdaptors::from(path, conv_path);
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveOnPath", conv_path, velocity, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name);
return this;
}

MultirotorRpcLibClient* MultirotorRpcLibClient::moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec,
DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name)
{
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("movetoGPS", latitude, longitude, altitude, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name);
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("movetoGPS", latitude, longitude, altitude, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name);
return this;
}

MultirotorRpcLibClient* MultirotorRpcLibClient::moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec,
DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name)
{
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveToPosition", x, y, z, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name);
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveToPosition", x, y, z, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name);
return this;
}

MultirotorRpcLibClient* MultirotorRpcLibClient::moveToZAsync(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name)
{
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveToZ", z, velocity, timeout_sec, MultirotorRpcLibAdapators::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name);
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveToZ", z, velocity, timeout_sec, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name);
return this;
}

MultirotorRpcLibClient* MultirotorRpcLibClient::moveByManualAsync(float vx_max, float vy_max, float z_min, float duration,
DrivetrainType drivetrain, const YawMode& yaw_mode, const std::string& vehicle_name)
{
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveByManual", vx_max, vy_max, z_min, duration, drivetrain, MultirotorRpcLibAdapators::YawMode(yaw_mode), vehicle_name);
pimpl_->last_future = static_cast<rpc::client*>(getClient())->async_call("moveByManual", vx_max, vy_max, z_min, duration, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), vehicle_name);
return this;
}

Expand Down Expand Up @@ -217,18 +224,24 @@ __pragma(warning(disable : 4239))
bool MultirotorRpcLibClient::setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy,
float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z, const std::string& vehicle_name)
{
return static_cast<rpc::client*>(getClient())->call("setSafety", static_cast<uint>(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, MultirotorRpcLibAdapators::Vector3r(origin), xy_length, max_z, min_z, vehicle_name).as<bool>();
return static_cast<rpc::client*>(getClient())->call("setSafety", static_cast<uint>(enable_reasons), obs_clearance, obs_startegy, obs_avoidance_vel, MultirotorRpcLibAdaptors::Vector3r(origin), xy_length, max_z, min_z, vehicle_name).as<bool>();
}

//status getters
// Rotor state getter
RotorStates MultirotorRpcLibClient::getRotorStates(const std::string& vehicle_name)
{
return static_cast<rpc::client*>(getClient())->call("getRotorStates", vehicle_name).as<MultirotorRpcLibAdaptors::RotorStates>().to();
}
// Multirotor state getter
MultirotorState MultirotorRpcLibClient::getMultirotorState(const std::string& vehicle_name)
{
return static_cast<rpc::client*>(getClient())->call("getMultirotorState", vehicle_name).as<MultirotorRpcLibAdapators::MultirotorState>().to();
return static_cast<rpc::client*>(getClient())->call("getMultirotorState", vehicle_name).as<MultirotorRpcLibAdaptors::MultirotorState>().to();
}

void MultirotorRpcLibClient::moveByRC(const RCData& rc_data, const std::string& vehicle_name)
{
static_cast<rpc::client*>(getClient())->call("moveByRC", MultirotorRpcLibAdapators::RCData(rc_data), vehicle_name);
static_cast<rpc::client*>(getClient())->call("moveByRC", MultirotorRpcLibAdaptors::RCData(rc_data), vehicle_name);
}

//return value of last task. It should be true if task completed without
Expand All @@ -251,7 +264,6 @@ __pragma(warning(disable : 4239))

return this;
}

}
} //namespace

Expand Down
Loading
You are viewing a condensed version of this merge commit. You can view the full changes here.