Added movetoGPS

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<real_T>((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * cos(lon_rad));
+            real_T y = static_cast<real_T>((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * sin(lon_rad));
+            real_T z = static_cast<real_T>((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<float>(-sin(lat_rad) * cos(lon_rad));
+            ecef_to_ned_matrix_(0, 1) = static_cast<float>(-sin(lat_rad) * sin(lon_rad));
+            ecef_to_ned_matrix_(0, 2) = static_cast<float>(cos(lat_rad));
+            ecef_to_ned_matrix_(1, 0) = static_cast<float>(-sin(lon_rad));
+            ecef_to_ned_matrix_(1, 1) = static_cast<float>(cos(lon_rad));
+            ecef_to_ned_matrix_(1, 2) = 0.0f;
+            ecef_to_ned_matrix_(2, 0) = static_cast<float>(cos(lat_rad) * cos(lon_rad));
+            ecef_to_ned_matrix_(2, 1) = static_cast<float>(cos(lat_rad) * sin(lon_rad));
+            ecef_to_ned_matrix_(2, 2) = static_cast<float>(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)

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);

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<float>(), const std::string& vehicle_name = "");
 
+        MultirotorRpcLibClient* moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec = Utils::max<float>(),
+                                               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,

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<Vector3r> path{ msr::airlib::EarthUtils::GeodeticToNed(target, getHomeGeoPoint()) };
+            return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead);
+        }
+        else {
+            vector<Vector3r> 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)
     {

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<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)
         {

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<rpc::server*>(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<rpc::server*>(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);
         });

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)