diff --git a/CMakeLists.txt b/CMakeLists.txt index ceacfa58e..fda68c173 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -256,6 +256,10 @@ if(BUILD_TESTING) WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" ) + #### NAVSAT CONVERSION TESTS #### + ament_add_gtest(test_navsat_conversions test/test_navsat_conversions.cpp) + target_link_libraries(test_navsat_conversions ${library_name}) + ament_add_gtest_executable(test_ros_robot_localization_listener test/test_ros_robot_localization_listener.cpp) target_link_libraries(test_ros_robot_localization_listener ${library_name}) @@ -290,6 +294,7 @@ if(BUILD_TESTING) #test_ekf_localization_node_bag2 #test_ekf_localization_node_bag3 test_robot_localization_estimator + test_navsat_conversions test_ros_robot_localization_listener test_ros_robot_localization_listener_publisher #test_ukf_localization_node_bag1 diff --git a/include/robot_localization/navsat_conversions.hpp b/include/robot_localization/navsat_conversions.hpp index 55ebf5095..6eb865ab4 100644 --- a/include/robot_localization/navsat_conversions.hpp +++ b/include/robot_localization/navsat_conversions.hpp @@ -51,6 +51,9 @@ #include #include +#include +#include + #include #include @@ -140,276 +143,74 @@ static inline void UTM(double lat, double lon, double * x, double * y) } /** - * Determine the correct UTM letter designator for the - * given latitude - * - * @returns 'Z' if latitude is outside the UTM limits of 84N to 80S - * - * Written by Chuck Gantz- chuck.gantz@globalstar.com - */ -static inline char UTMLetterDesignator(double Lat) -{ - char LetterDesignator; - - if ((84 >= Lat) && (Lat >= 72)) { - LetterDesignator = 'X'; - } else if ((72 > Lat) && (Lat >= 64)) { - LetterDesignator = 'W'; - } else if ((64 > Lat) && (Lat >= 56)) { - LetterDesignator = 'V'; - } else if ((56 > Lat) && (Lat >= 48)) { - LetterDesignator = 'U'; - } else if ((48 > Lat) && (Lat >= 40)) { - LetterDesignator = 'T'; - } else if ((40 > Lat) && (Lat >= 32)) { - LetterDesignator = 'S'; - } else if ((32 > Lat) && (Lat >= 24)) { - LetterDesignator = 'R'; - } else if ((24 > Lat) && (Lat >= 16)) { - LetterDesignator = 'Q'; - } else if ((16 > Lat) && (Lat >= 8)) { - LetterDesignator = 'P'; - } else if ((8 > Lat) && (Lat >= 0)) { - LetterDesignator = 'N'; - } else if ((0 > Lat) && (Lat >= -8)) { - LetterDesignator = 'M'; - } else if ((-8 > Lat) && (Lat >= -16)) { - LetterDesignator = 'L'; - } else if ((-16 > Lat) && (Lat >= -24)) { - LetterDesignator = 'K'; - } else if ((-24 > Lat) && (Lat >= -32)) { - LetterDesignator = 'J'; - } else if ((-32 > Lat) && (Lat >= -40)) { - LetterDesignator = 'H'; - } else if ((-40 > Lat) && (Lat >= -48)) { - LetterDesignator = 'G'; - } else if ((-48 > Lat) && (Lat >= -56)) { - LetterDesignator = 'F'; - } else if ((-56 > Lat) && (Lat >= -64)) { - LetterDesignator = 'E'; - } else if ((-64 > Lat) && (Lat >= -72)) { - LetterDesignator = 'D'; - } else if ((-72 > Lat) && (Lat >= -80)) { - LetterDesignator = 'C'; - } else { - // 'Z' is an error flag, the Latitude is outside the UTM limits - LetterDesignator = 'Z'; - } - return LetterDesignator; -} - -/** - * Convert lat/long to UTM coords. Equations from USGS Bulletin 1532 + * Convert lat/long to UTM coords. * * East Longitudes are positive, West longitudes are negative. * North latitudes are positive, South latitudes are negative * Lat and Long are in fractional degrees - * Meridian convergence is computed as for Spherical Transverse Mercator, - * which gives quite good approximation. * * @param[out] gamma meridian convergence at point (degrees). - * - * Written by Chuck Gantz- chuck.gantz@globalstar.com */ static inline void LLtoUTM( const double Lat, const double Long, double & UTMNorthing, double & UTMEasting, std::string & UTMZone, double & gamma) { - double a = WGS84_A; - double eccSquared = UTM_E2; - double k0 = UTM_K0; - - double LongOrigin; - double eccPrimeSquared; - double N, T, C, A, M; - - // Make sure the longitude is between -180.00 .. 179.9 - double LongTemp = - (Long + 180) - static_cast((Long + 180) / 360) * 360 - 180; - - double LatRad = Lat * RADIANS_PER_DEGREE; - double LongRad = LongTemp * RADIANS_PER_DEGREE; - double LongOriginRad; - int ZoneNumber; - - ZoneNumber = static_cast((LongTemp + 180) / 6) + 1; - - if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0) { - ZoneNumber = 32; - } - - // Special zones for Svalbard - if (Lat >= 72.0 && Lat < 84.0) { - if (LongTemp >= 0.0 && LongTemp < 9.0) { - ZoneNumber = 31; - } else if (LongTemp >= 9.0 && LongTemp < 21.0) { - ZoneNumber = 33; - } else if (LongTemp >= 21.0 && LongTemp < 33.0) { - ZoneNumber = 35; - } else if (LongTemp >= 33.0 && LongTemp < 42.0) { - ZoneNumber = 37; - } - } - // +3 puts origin in middle of zone - LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3; - LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; - - // Compute the UTM Zone from the latitude and longitude - char zone_buf[] = {0, 0, 0, 0}; - // We &0x3fU to let GCC know the size of ZoneNumber. In this case, it's under - // 63 (6bits) - snprintf( - zone_buf, sizeof(zone_buf), "%d%c", ZoneNumber & 0x3fU, - UTMLetterDesignator(Lat)); - UTMZone = std::string(zone_buf); - - eccPrimeSquared = (eccSquared) / (1 - eccSquared); - - N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad)); - T = tan(LatRad) * tan(LatRad); - C = eccPrimeSquared * cos(LatRad) * cos(LatRad); - A = cos(LatRad) * (LongRad - LongOriginRad); - - M = a * - ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - - 5 * eccSquared * eccSquared * eccSquared / 256) * - LatRad - - (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + - 45 * eccSquared * eccSquared * eccSquared / 1024) * - sin(2 * LatRad) + - (15 * eccSquared * eccSquared / 256 + - 45 * eccSquared * eccSquared * eccSquared / 1024) * - sin(4 * LatRad) - - (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad)); - - UTMEasting = static_cast( - k0 * N * - (A + (1 - T + C) * A * A * A / 6 + - (5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * - A * A / 120) + - 500000.0); - - UTMNorthing = static_cast( - k0 * - (M + N * tan(LatRad) * - (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 + - (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * - A * A * A * A * A / 720))); - - gamma = atan(tan(LongRad - LongOriginRad) * sin(LatRad)) * DEGREES_PER_RADIAN; - - if (Lat < 0) { - // 10000000 meter offset for southern hemisphere - UTMNorthing += 10000000.0; - } + int zone; + bool northp; + double k_unused; + GeographicLib::UTMUPS::Forward( + Lat, Long, zone, northp, UTMEasting, UTMNorthing, gamma, + k_unused, GeographicLib::UTMUPS::zonespec::MATCH); + GeographicLib::MGRS::Forward(zone, northp, UTMEasting, UTMNorthing, -1, UTMZone); } /** - * Convert lat/long to UTM coords. Equations from USGS Bulletin 1532 + * Convert lat/long to UTM coords. * * East Longitudes are positive, West longitudes are negative. * North latitudes are positive, South latitudes are negative * Lat and Long are in fractional degrees - * - * Written by Chuck Gantz- chuck.gantz@globalstar.com */ static inline void LLtoUTM( const double Lat, const double Long, double & UTMNorthing, double & UTMEasting, std::string & UTMZone) { - double gamma; + double gamma = 0.0; LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, UTMZone, gamma); } /** - * Converts UTM coords to lat/long. Equations from USGS Bulletin 1532 + * Converts UTM coords to lat/long. * * East Longitudes are positive, West longitudes are negative. * North latitudes are positive, South latitudes are negative * Lat and Long are in fractional degrees. - * Meridian convergence is computed as for Spherical Transverse Mercator, - * which gives quite good approximation. * * @param[out] gamma meridian convergence at point (degrees). - * - * Written by Chuck Gantz- chuck.gantz@globalstar.com */ static inline void UTMtoLL( const double UTMNorthing, const double UTMEasting, const std::string & UTMZone, double & Lat, double & Long, double & gamma) { - double k0 = UTM_K0; - double a = WGS84_A; - double eccSquared = UTM_E2; - double eccPrimeSquared; - double e1 = (1 - sqrt(1 - eccSquared)) / (1 + sqrt(1 - eccSquared)); - double N1, T1, C1, R1, D, M; - double LongOrigin; - double mu, phi1Rad; - double x, y; - int ZoneNumber; - char * ZoneLetter; - - x = UTMEasting - 500000.0; // remove 500,000 meter offset for longitude - y = UTMNorthing; - - ZoneNumber = strtoul(UTMZone.c_str(), &ZoneLetter, 10); - if ((*ZoneLetter - 'N') < 0) { - // remove 10,000,000 meter offset used for southern hemisphere - y -= 10000000.0; - } - - // +3 puts origin in middle of zone - LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3; - eccPrimeSquared = (eccSquared) / (1 - eccSquared); - - M = y / k0; - mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - - 5 * eccSquared * eccSquared * eccSquared / 256)); - - phi1Rad = - mu + ((3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * mu) + - (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * mu) + - (151 * e1 * e1 * e1 / 96) * sin(6 * mu)); - - N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad)); - T1 = tan(phi1Rad) * tan(phi1Rad); - C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad); - R1 = a * (1 - eccSquared) / - pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5); - D = x / (N1 * k0); - - Lat = phi1Rad - ((N1 * tan(phi1Rad) / R1) * - (D * D / 2 - - (5 + 3 * T1 + 10 * C1 - 4 * C1 * C1 - 9 * eccPrimeSquared) * - D * D * D * D / 24 + - (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - - 252 * eccPrimeSquared - 3 * C1 * C1) * - D * D * D * D * D * D / 720)); - - Lat = Lat * DEGREES_PER_RADIAN; - - Long = ((D - (1 + 2 * T1 + C1) * D * D * D / 6 + - (5 - 2 * C1 + 28 * T1 - 3 * C1 * C1 + 8 * eccPrimeSquared + - 24 * T1 * T1) * - D * D * D * D * D / 120) / - cos(phi1Rad)); - Long = LongOrigin + Long * DEGREES_PER_RADIAN; - - gamma = atan(tanh(x / (k0 * a)) * tan(y / (k0 * a))) * DEGREES_PER_RADIAN; + int zone; + bool northp; + double x_unused; + double y_unused; + int prec_unused; + double k_unused; + GeographicLib::MGRS::Reverse(UTMZone, zone, northp, x_unused, y_unused, prec_unused, true); + GeographicLib::UTMUPS::Reverse(zone, northp, UTMEasting, UTMNorthing, Lat, Long, gamma, k_unused); } /** -* Converts UTM coords to lat/long. Equations from USGS Bulletin 1532 +* Converts UTM coords to lat/long. * * East Longitudes are positive, West longitudes are negative. * North latitudes are positive, South latitudes are negative * Lat and Long are in fractional degrees. -* -* Written by Chuck Gantz- chuck.gantz@globalstar.com */ static inline void UTMtoLL( const double UTMNorthing, const double UTMEasting, diff --git a/test/test_navsat_conversions.cpp b/test/test_navsat_conversions.cpp new file mode 100644 index 000000000..4b386bfe8 --- /dev/null +++ b/test/test_navsat_conversions.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2021, Charles River Analytics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include + +#include "robot_localization/navsat_conversions.hpp" + +void NavsatConversionsTest( + const double lat, const double lon, + const double UTMNorthing, const double UTMEasting, + const std::string UTMZone, const double gamma) +{ + double UTMNorthing_new; + double UTMEasting_new; + std::string UTMZone_new; + double gamma_new; + robot_localization::navsat_conversions::LLtoUTM( + lat, lon, UTMNorthing_new, UTMEasting_new, UTMZone_new, gamma_new); + EXPECT_NEAR(UTMNorthing, UTMNorthing_new, 1e-2); + EXPECT_NEAR(UTMEasting, UTMEasting_new, 1e-2); + EXPECT_EQ(UTMZone, UTMZone_new); + EXPECT_NEAR(gamma, gamma_new, 1e-2); + double lat_new; + double lon_new; + robot_localization::navsat_conversions::UTMtoLL( + UTMNorthing, UTMEasting, UTMZone, lat_new, lon_new); + EXPECT_NEAR(lat_new, lat, 1e-5); + EXPECT_NEAR(lon_new, lon, 1e-5); +} + +TEST(NavsatConversionsTest, UtmTest) +{ + NavsatConversionsTest(51.423964, 5.494271, 5699924.709, 673409.989, "31U", 1.950); + NavsatConversionsTest(-43.530955, 172.636645, 5178919.718, 632246.802, "59G", -1.127); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}