geodesy: geodesy: test_wgs84.cpp Source File

00001 /* $Id: c63a7c1371eea1dfb24476fb3ed8161f38b3b218 $ */
00002 
00003 /*********************************************************************
00004 * Software License Agreement (BSD License)
00005 *
00006 *  Copyright (c) 2011 Jack O'Quin
00007 *  All rights reserved.
00008 *
00009 *  Redistribution and use in source and binary forms, with or without
00010 *  modification, are permitted provided that the following conditions
00011 *  are met:
00012 *
00013 *   * Redistributions of source code must retain the above copyright
00014 *     notice, this list of conditions and the following disclaimer.
00015 *   * Redistributions in binary form must reproduce the above
00016 *     copyright notice, this list of conditions and the following
00017 *     disclaimer in the documentation and/or other materials provided
00018 *     with the distribution.
00019 *   * Neither the name of the author nor other contributors may be
00020 *     used to endorse or promote products derived from this software
00021 *     without specific prior written permission.
00022 *
00023 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 *  POSSIBILITY OF SUCH DAMAGE.
00035 *********************************************************************/
00036 
00037 #include <gtest/gtest.h>
00038 #include <limits>
00039 #include "geodesy/wgs84.h"
00040 
00042 // Utility functions
00044 
00045 // check that normalize of (lat1, lon1) yields (lat2, lon2)
00046 void check_normalize(double lat1, double lon1, double lat2, double lon2)
00047 {
00048  double epsilon = 1e-9;
00049  geographic_msgs::GeoPoint pt;
00050  pt.latitude = lat1;
00051  pt.longitude = lon1;
00052  geodesy::normalize(pt);
00053  EXPECT_NEAR(lat2, pt.latitude, epsilon);
00054  EXPECT_NEAR(lon2, pt.longitude, epsilon);
00055 }
00056 
00058 // Test cases
00060 
00061 // Test null point constructor
00062 TEST(GeoPoint, nullConstructor)
00063 {
00064   geographic_msgs::GeoPoint pt;
00065 
00066   EXPECT_FALSE(geodesy::is2D(pt));
00067   EXPECT_TRUE(geodesy::isValid(pt));
00068 }
00069 
00070 // Test point with no altitude
00071 TEST(GeoPoint, noAltitude)
00072 {
00073   geographic_msgs::GeoPoint pt(geodesy::toMsg(0.0, 0.0));
00074   EXPECT_TRUE(geodesy::is2D(pt));
00075 }
00076 
00077 // Test creating point from NavSatFix message
00078 TEST(GeoPoint, fromNavSatFix)
00079 {
00080   double lat = 30.0;
00081   double lon = -97.0;
00082   double alt = 208.0;
00083   sensor_msgs::NavSatFix fix;
00084   fix.latitude = lat;
00085   fix.longitude = lon;
00086   fix.altitude = alt;
00087 
00088   geographic_msgs::GeoPoint pt(geodesy::toMsg(fix));
00089 
00090   EXPECT_FALSE(geodesy::is2D(pt));
00091   EXPECT_TRUE(geodesy::isValid(pt));
00092 
00093   EXPECT_EQ(pt.latitude, lat);
00094   EXPECT_EQ(pt.longitude, lon);
00095   EXPECT_EQ(pt.altitude, alt);
00096 }
00097 
00098 // Test point with valid altitude
00099 TEST(GeoPoint, hasAltitude)
00100 {
00101   geographic_msgs::GeoPoint pt(geodesy::toMsg(30.0, 206.0, -97.0));
00102   EXPECT_FALSE(geodesy::is2D(pt));
00103 
00104   pt.altitude = -100.0;
00105   EXPECT_FALSE(geodesy::is2D(pt));
00106 
00107   pt.altitude = 20000.0;
00108   EXPECT_FALSE(geodesy::is2D(pt));
00109 
00110   pt.altitude = 0.0;
00111   EXPECT_FALSE(geodesy::is2D(pt));
00112 }
00113 
00114 // Test valid latitudes and longitudes
00115 TEST(GeoPoint, validLatLong)
00116 {
00117   geographic_msgs::GeoPoint pt;
00118 
00119   pt.latitude = 90.0;
00120   EXPECT_TRUE(geodesy::isValid(pt));
00121 
00122   pt.latitude = -90.0;
00123   EXPECT_TRUE(geodesy::isValid(pt));
00124 
00125   pt.latitude = 30.0;
00126   pt.longitude = -97.0;
00127   EXPECT_TRUE(geodesy::isValid(pt));
00128 
00129   pt.longitude = 179.999999;
00130   EXPECT_TRUE(geodesy::isValid(pt));
00131 
00132   pt.longitude = -180.0;
00133   EXPECT_TRUE(geodesy::isValid(pt));
00134 }
00135 
00136 // Test valid latitudes and longitudes
00137 TEST(GeoPoint, invalidLatLong)
00138 {
00139   geographic_msgs::GeoPoint pt;
00140 
00141   pt.latitude = 90.001;
00142   EXPECT_FALSE(geodesy::isValid(pt));
00143 
00144   pt.latitude = -90.3;
00145   EXPECT_FALSE(geodesy::isValid(pt));
00146 
00147   pt.latitude = 30.0;
00148   pt.longitude = -1000.0;
00149   EXPECT_FALSE(geodesy::isValid(pt));
00150 
00151   pt.longitude = 180.0;
00152   EXPECT_FALSE(geodesy::isValid(pt));
00153 
00154   pt.longitude = 180.0001;
00155   EXPECT_FALSE(geodesy::isValid(pt));
00156 
00157   pt.longitude = -180.999;
00158   EXPECT_FALSE(geodesy::isValid(pt));
00159 }
00160 
00161 TEST(GeoPoint, normalize)
00162 {
00163  check_normalize(0, 0, 0, 0);
00164 
00165  // longitude range
00166  check_normalize(0, 90,  0, 90);
00167  check_normalize(0, 180, 0, -180);
00168  check_normalize(0, 270, 0, -90);
00169  check_normalize(0, 360, 0, 0);
00170  check_normalize(0, 450, 0, 90);
00171  check_normalize(0, 540, 0, -180);
00172  check_normalize(0, 630, 0, -90);
00173  check_normalize(0, 720, 0, 0);
00174 
00175  check_normalize(0, -90,  0, -90);
00176  check_normalize(0, -180, 0, -180);
00177  check_normalize(0, -270, 0, 90);
00178  check_normalize(0, -360, 0, 0);
00179  check_normalize(0, -450, 0, -90);
00180  check_normalize(0, -540, 0, -180);
00181  check_normalize(0, -630, 0, 90);
00182  check_normalize(0, -720, 0, 0);
00183 
00184  check_normalize(0, 45,   0, 45);
00185  check_normalize(0, 135,  0, 135);
00186  check_normalize(0, 225,  0, -135);
00187  check_normalize(0, 315,  0, -45);
00188 
00189  check_normalize(0, -45,   0, -45);
00190  check_normalize(0, -135,  0, -135);
00191  check_normalize(0, -225,  0, 135);
00192  check_normalize(0, -315,  0, 45);
00193 
00194  check_normalize(0, 91,  0, 91);
00195  check_normalize(0, 181, 0, -179);
00196  check_normalize(0, 271, 0, -89);
00197  check_normalize(0, 361, 0, 1);
00198  check_normalize(0, 451, 0, 91);
00199 
00200  check_normalize(0, -91,  0, -91);
00201  check_normalize(0, -181, 0, 179);
00202  check_normalize(0, -271, 0, 89);
00203  check_normalize(0, -361, 0, -1);
00204  check_normalize(0, -451, 0, -91);
00205 
00206  // latitude range
00207  check_normalize(15, 0, 15, 0);
00208  check_normalize(30, 0, 30, 0);
00209  check_normalize(45, 0, 45, 0);
00210  check_normalize(60, 0, 60, 0);
00211  check_normalize(75, 0, 75, 0);
00212  check_normalize(90, 0, 90, 0);
00213  check_normalize(105, 0, 90, 0);
00214  check_normalize(89.999999, 0, 89.999999, 0);
00215  check_normalize(90.000001, 0, 90, 0);
00216 
00217  check_normalize(-15, 0, -15, 0);
00218  check_normalize(-30, 0, -30, 0);
00219  check_normalize(-45, 0, -45, 0);
00220  check_normalize(-60, 0, -60, 0);
00221  check_normalize(-75, 0, -75, 0);
00222  check_normalize(-90, 0, -90, 0);
00223  check_normalize(-105, 0, -90, 0);
00224  check_normalize(-89.999999, 0, -89.999999, 0);
00225  check_normalize(-90.000001, 0, -90, 0);
00226 
00227 }
00228 
00229 // Test null pose constructor
00230 TEST(GeoPose, nullConstructor)
00231 {
00232   geographic_msgs::GeoPose pose;
00233 
00234   EXPECT_FALSE(geodesy::is2D(pose));
00235   EXPECT_FALSE(geodesy::isValid(pose));
00236 }
00237 
00238 // Test validity of various quaternions
00239 TEST(GeoPose, quaternionValidity)
00240 {
00241   geographic_msgs::GeoPose pose;
00242   EXPECT_FALSE(geodesy::isValid(pose));
00243 
00244   pose.orientation.x = 1.0;             // identity quaternion
00245   EXPECT_TRUE(geodesy::isValid(pose));
00246 
00247   pose.orientation.x = 0.7071;          // also valid
00248   pose.orientation.y = 0.7071;
00249   EXPECT_TRUE(geodesy::isValid(pose));
00250 
00251   pose.orientation.x = 0.8071;          // not normalized
00252   pose.orientation.y = 0.8071;
00253   EXPECT_FALSE(geodesy::isValid(pose));
00254 }
00255 
00256 // Test trivial point conversion
00257 TEST(Convert, GeoPointToGeoPoint)
00258 {
00259   geographic_msgs::GeoPoint pt1(geodesy::toMsg(30.0, 206.0, -97.0));
00260   geographic_msgs::GeoPoint pt2;
00261 
00262   geodesy::convert(pt1, pt2);
00263 
00264   EXPECT_EQ(pt1.latitude, pt2.latitude);
00265   EXPECT_EQ(pt1.longitude, pt2.longitude);
00266   EXPECT_EQ(pt1.altitude, pt2.altitude);
00267 }
00268 
00269 // Test trivial pose conversion
00270 TEST(Convert, GeoPoseToGeoPose)
00271 {
00272   geographic_msgs::GeoPoint pt(geodesy::toMsg(30.0, 206.0, -97.0));
00273   geometry_msgs::Quaternion q;
00274   geographic_msgs::GeoPose pose1(geodesy::toMsg(pt, q));
00275   geographic_msgs::GeoPose pose2;
00276 
00277   geodesy::convert(pose1, pose2);
00278 
00279   EXPECT_EQ(pose1.position.latitude, pose2.position.latitude);
00280   EXPECT_EQ(pose1.position.longitude, pose2.position.longitude);
00281   EXPECT_EQ(pose1.position.altitude, pose2.position.altitude);
00282 }
00283 
00284 
00285 // Run all the tests that were declared with TEST()
00286 int main(int argc, char **argv)
00287 {
00288   ros::init(argc, argv, "geotypes_unit_test");
00289   testing::InitGoogleTest(&argc, argv);
00290 
00291   // run the tests in this thread
00292   return RUN_ALL_TESTS();
00293 }