diff --git a/AirLib/include/common/EarthUtils.hpp b/AirLib/include/common/EarthUtils.hpp index a8775fa9a0..45d595f846 100644 --- a/AirLib/include/common/EarthUtils.hpp +++ b/AirLib/include/common/EarthUtils.hpp @@ -306,6 +306,47 @@ namespace airlib return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, home_geo_point.home_geo_point.altitude - v.z()); } + static Vector3r GeodeticToEcef(const GeoPoint& geo) + { + // Convert geodetic coordinates to ECEF. + // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22 + double lat_rad = Utils::degreesToRadians(geo.latitude); + double lon_rad = Utils::degreesToRadians(geo.longitude); + double xi = sqrt(1 - (6.69437999014 * 0.001) * sin(lat_rad) * sin(lat_rad)); + real_T x = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * cos(lon_rad)); + real_T y = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * sin(lon_rad)); + real_T z = static_cast((EARTH_RADIUS / xi * (1 - (6.69437999014 * 0.001)) + geo.altitude) * sin(lat_rad)); + return Vector3r(x, y, z); + } + + static Vector3r EcefToNed(const Vector3r& ecefxyz, const Vector3r& ecefhome, const GeoPoint& geohome) + { + // Converts ECEF coordinate position into local-tangent-plane NED. + // Coordinates relative to given ECEF coordinate frame. + + Vector3r vect, ret; + Matrix3x3r ecef_to_ned_matrix_; + double lat_rad = Utils::degreesToRadians(geohome.latitude); + double lon_rad = Utils::degreesToRadians(geohome.longitude); + vect = ecefxyz - ecefhome; + ecef_to_ned_matrix_(0, 0) = static_cast(-sin(lat_rad) * cos(lon_rad)); + ecef_to_ned_matrix_(0, 1) = static_cast(-sin(lat_rad) * sin(lon_rad)); + ecef_to_ned_matrix_(0, 2) = static_cast(cos(lat_rad)); + ecef_to_ned_matrix_(1, 0) = static_cast(-sin(lon_rad)); + ecef_to_ned_matrix_(1, 1) = static_cast(cos(lon_rad)); + ecef_to_ned_matrix_(1, 2) = 0.0f; + ecef_to_ned_matrix_(2, 0) = static_cast(cos(lat_rad) * cos(lon_rad)); + ecef_to_ned_matrix_(2, 1) = static_cast(cos(lat_rad) * sin(lon_rad)); + ecef_to_ned_matrix_(2, 2) = static_cast(sin(lat_rad)); + ret = ecef_to_ned_matrix_ * vect; + return Vector3r(ret(0), ret(1), -ret(2)); + } + + static Vector3r GeodeticToNed(const GeoPoint& geo, const GeoPoint& geohome) + { + return EcefToNed(GeodeticToEcef(geo), GeodeticToEcef(geohome), geohome); + } + //below are approximate versions and would produce errors of more than 10m for points farther than 1km //for more accurate versions, please use the version in EarthUtils::nedToGeodetic static Vector3r GeodeticToNedFast(const GeoPoint& geo, const GeoPoint& home) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index ea2fb6b033..01a8108d37 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -91,6 +91,8 @@ namespace airlib virtual bool land(float timeout_sec); virtual bool goHome(float timeout_sec); + 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); diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index 351ed37968..67dfbca91c 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -26,6 +26,9 @@ namespace airlib MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); + MultirotorRpcLibClient* moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec = Utils::max(), + DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), + 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, diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 2c5662142d..3329798373 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -449,6 +449,24 @@ namespace airlib return waiter.isComplete(); } + bool MultirotorApiBase::moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) + { + SingleTaskCall lock(this); + GeoPoint target; + target.latitude = latitude; + target.longitude = longitude; + target.altitude = altitude; + if (!std::isnan(getHomeGeoPoint().latitude) && !std::isnan(getHomeGeoPoint().longitude) && !std::isnan(getHomeGeoPoint().altitude)) { + vector path{ msr::airlib::EarthUtils::GeodeticToNed(target, getHomeGeoPoint()) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } + else { + vector path{ Vector3r(getPosition().x(), getPosition().y(), getPosition().z()) }; + return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); + } + } + bool MultirotorApiBase::moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) { diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 73ab097f63..652d3e6507 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -156,6 +156,13 @@ __pragma(warning(disable : 4239)) 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(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) { diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 4fb8cdf8d5..90a0884776 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -89,6 +89,9 @@ namespace airlib MultirotorRpcLibAdaptors::to(path, conv_path); return getVehicleApi(vehicle_name)->moveOnPath(conv_path, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); }); + (static_cast(getServer()))->bind("moveToGPS", [&](float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { + return getVehicleApi(vehicle_name)->moveToGPS(latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); + }); (static_cast(getServer()))->bind("moveToPosition", [&](float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->moveToPosition(x, y, z, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); }); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index af96560178..c92282a70f 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -1139,6 +1139,10 @@ def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + def moveToGPSAsync(self, latitude, longitude, altitude, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), + lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): + return self.client.call_async('moveToGPS', latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) + def moveToZAsync(self, z, velocity, timeout_sec = 3e+38, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, adaptive_lookahead, vehicle_name)