test_wgs84.cpp
Go to the documentation of this file.
1 /* $Id: c7a178b0bbba69e8bf70b12ea1fe76e0cc5a4bff $ */
2 
3 /*********************************************************************
4 * Software License Agreement (BSD License)
5 *
6 * Copyright (c) 2011 Jack O'Quin
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the author nor other contributors may be
20 * used to endorse or promote products derived from this software
21 * without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36 
37 #include <gtest/gtest.h>
38 #include <limits>
39 #include "geodesy/wgs84.h"
40 
42 // Utility functions
44 
45 // check that normalize of (lat1, lon1) yields (lat2, lon2)
46 void check_normalize(double lat1, double lon1, double lat2, double lon2)
47 {
48  double epsilon = 1e-9;
49  geographic_msgs::GeoPoint pt;
50  pt.latitude = lat1;
51  pt.longitude = lon1;
52  geodesy::normalize(pt);
53  EXPECT_NEAR(lat2, pt.latitude, epsilon);
54  EXPECT_NEAR(lon2, pt.longitude, epsilon);
55 }
56 
58 // Test cases
60 
61 // Test null point constructor
62 TEST(GeoPoint, nullConstructor)
63 {
64  geographic_msgs::GeoPoint pt;
65 
66  EXPECT_FALSE(geodesy::is2D(pt));
67  EXPECT_TRUE(geodesy::isValid(pt));
68 }
69 
70 // Test point with no altitude
71 TEST(GeoPoint, noAltitude)
72 {
73  geographic_msgs::GeoPoint pt(geodesy::toMsg(0.0, 0.0));
74  EXPECT_TRUE(geodesy::is2D(pt));
75 }
76 
77 // Test creating point from NavSatFix message
78 TEST(GeoPoint, fromNavSatFix)
79 {
80  double lat = 30.0;
81  double lon = -97.0;
82  double alt = 208.0;
83  sensor_msgs::NavSatFix fix;
84  fix.latitude = lat;
85  fix.longitude = lon;
86  fix.altitude = alt;
87 
88  geographic_msgs::GeoPoint pt(geodesy::toMsg(fix));
89 
90  EXPECT_FALSE(geodesy::is2D(pt));
91  EXPECT_TRUE(geodesy::isValid(pt));
92 
93  EXPECT_EQ(pt.latitude, lat);
94  EXPECT_EQ(pt.longitude, lon);
95  EXPECT_EQ(pt.altitude, alt);
96 }
97 
98 // Test point with valid altitude
99 TEST(GeoPoint, hasAltitude)
100 {
101  geographic_msgs::GeoPoint pt(geodesy::toMsg(30.0, 206.0, -97.0));
102  EXPECT_FALSE(geodesy::is2D(pt));
103 
104  pt.altitude = -100.0;
105  EXPECT_FALSE(geodesy::is2D(pt));
106 
107  pt.altitude = 20000.0;
108  EXPECT_FALSE(geodesy::is2D(pt));
109 
110  pt.altitude = 0.0;
111  EXPECT_FALSE(geodesy::is2D(pt));
112 }
113 
114 // Test valid latitudes and longitudes
115 TEST(GeoPoint, validLatLong)
116 {
117  geographic_msgs::GeoPoint pt;
118 
119  pt.latitude = 90.0;
120  EXPECT_TRUE(geodesy::isValid(pt));
121 
122  pt.latitude = -90.0;
123  EXPECT_TRUE(geodesy::isValid(pt));
124 
125  pt.latitude = 30.0;
126  pt.longitude = -97.0;
127  EXPECT_TRUE(geodesy::isValid(pt));
128 
129  pt.longitude = 179.999999;
130  EXPECT_TRUE(geodesy::isValid(pt));
131 
132  pt.longitude = -180.0;
133  EXPECT_TRUE(geodesy::isValid(pt));
134 }
135 
136 // Test valid latitudes and longitudes
137 TEST(GeoPoint, invalidLatLong)
138 {
139  geographic_msgs::GeoPoint pt;
140 
141  pt.latitude = 90.001;
142  EXPECT_FALSE(geodesy::isValid(pt));
143 
144  pt.latitude = -90.3;
145  EXPECT_FALSE(geodesy::isValid(pt));
146 
147  pt.latitude = 30.0;
148  pt.longitude = -1000.0;
149  EXPECT_FALSE(geodesy::isValid(pt));
150 
151  pt.longitude = 180.0;
152  EXPECT_FALSE(geodesy::isValid(pt));
153 
154  pt.longitude = 180.0001;
155  EXPECT_FALSE(geodesy::isValid(pt));
156 
157  pt.longitude = -180.999;
158  EXPECT_FALSE(geodesy::isValid(pt));
159 }
160 
161 TEST(GeoPoint, normalize)
162 {
163  check_normalize(0, 0, 0, 0);
164 
165  // longitude range
166  check_normalize(0, 90, 0, 90);
167  check_normalize(0, 180, 0, -180);
168  check_normalize(0, 270, 0, -90);
169  check_normalize(0, 360, 0, 0);
170  check_normalize(0, 450, 0, 90);
171  check_normalize(0, 540, 0, -180);
172  check_normalize(0, 630, 0, -90);
173  check_normalize(0, 720, 0, 0);
174 
175  check_normalize(0, -90, 0, -90);
176  check_normalize(0, -180, 0, -180);
177  check_normalize(0, -270, 0, 90);
178  check_normalize(0, -360, 0, 0);
179  check_normalize(0, -450, 0, -90);
180  check_normalize(0, -540, 0, -180);
181  check_normalize(0, -630, 0, 90);
182  check_normalize(0, -720, 0, 0);
183 
184  check_normalize(0, 45, 0, 45);
185  check_normalize(0, 135, 0, 135);
186  check_normalize(0, 225, 0, -135);
187  check_normalize(0, 315, 0, -45);
188 
189  check_normalize(0, -45, 0, -45);
190  check_normalize(0, -135, 0, -135);
191  check_normalize(0, -225, 0, 135);
192  check_normalize(0, -315, 0, 45);
193 
194  check_normalize(0, 91, 0, 91);
195  check_normalize(0, 181, 0, -179);
196  check_normalize(0, 271, 0, -89);
197  check_normalize(0, 361, 0, 1);
198  check_normalize(0, 451, 0, 91);
199 
200  check_normalize(0, -91, 0, -91);
201  check_normalize(0, -181, 0, 179);
202  check_normalize(0, -271, 0, 89);
203  check_normalize(0, -361, 0, -1);
204  check_normalize(0, -451, 0, -91);
205 
206  // latitude range
207  check_normalize(15, 0, 15, 0);
208  check_normalize(30, 0, 30, 0);
209  check_normalize(45, 0, 45, 0);
210  check_normalize(60, 0, 60, 0);
211  check_normalize(75, 0, 75, 0);
212  check_normalize(90, 0, 90, 0);
213  check_normalize(105, 0, 90, 0);
214  check_normalize(89.999999, 0, 89.999999, 0);
215  check_normalize(90.000001, 0, 90, 0);
216 
217  check_normalize(-15, 0, -15, 0);
218  check_normalize(-30, 0, -30, 0);
219  check_normalize(-45, 0, -45, 0);
220  check_normalize(-60, 0, -60, 0);
221  check_normalize(-75, 0, -75, 0);
222  check_normalize(-90, 0, -90, 0);
223  check_normalize(-105, 0, -90, 0);
224  check_normalize(-89.999999, 0, -89.999999, 0);
225  check_normalize(-90.000001, 0, -90, 0);
226 
227 }
228 
229 // Test null pose constructor
230 TEST(GeoPose, nullConstructor)
231 {
232  geographic_msgs::GeoPose pose;
233 
234  EXPECT_FALSE(geodesy::is2D(pose));
235  EXPECT_FALSE(geodesy::isValid(pose));
236 }
237 
238 // Test validity of various quaternions
239 TEST(GeoPose, quaternionValidity)
240 {
241  geographic_msgs::GeoPose pose;
242  EXPECT_FALSE(geodesy::isValid(pose));
243 
244  pose.orientation.x = 1.0; // identity quaternion
245  EXPECT_TRUE(geodesy::isValid(pose));
246 
247  pose.orientation.x = 0.7071; // also valid
248  pose.orientation.y = 0.7071;
249  EXPECT_TRUE(geodesy::isValid(pose));
250 
251  pose.orientation.x = 0.8071; // not normalized
252  pose.orientation.y = 0.8071;
253  EXPECT_FALSE(geodesy::isValid(pose));
254 }
255 
256 // Test trivial point conversion
257 TEST(Convert, GeoPointToGeoPoint)
258 {
259  geographic_msgs::GeoPoint pt1(geodesy::toMsg(30.0, 206.0, -97.0));
260  geographic_msgs::GeoPoint pt2;
261 
262  geodesy::convert(pt1, pt2);
263 
264  EXPECT_EQ(pt1.latitude, pt2.latitude);
265  EXPECT_EQ(pt1.longitude, pt2.longitude);
266  EXPECT_EQ(pt1.altitude, pt2.altitude);
267 }
268 
269 // Test trivial pose conversion
270 TEST(Convert, GeoPoseToGeoPose)
271 {
272  geographic_msgs::GeoPoint pt(geodesy::toMsg(30.0, 206.0, -97.0));
273  geometry_msgs::Quaternion q;
274  geographic_msgs::GeoPose pose1(geodesy::toMsg(pt, q));
275  geographic_msgs::GeoPose pose2;
276 
277  geodesy::convert(pose1, pose2);
278 
279  EXPECT_EQ(pose1.position.latitude, pose2.position.latitude);
280  EXPECT_EQ(pose1.position.longitude, pose2.position.longitude);
281  EXPECT_EQ(pose1.position.altitude, pose2.position.altitude);
282 }
283 
284 
285 // Run all the tests that were declared with TEST()
286 int main(int argc, char **argv)
287 {
288  testing::InitGoogleTest(&argc, argv);
289 
290  // run the tests in this thread
291  return RUN_ALL_TESTS();
292 }
epsilon
double epsilon
wgs84.h
WGS 84 geodetic system for ROS latitude and longitude messages.
main
int main(int argc, char **argv)
Definition: test_wgs84.cpp:286
TEST
TEST(GeoPoint, nullConstructor)
Definition: test_wgs84.cpp:62
check_normalize
void check_normalize(double lat1, double lon1, double lat2, double lon2)
Definition: test_wgs84.cpp:46
geodesy::is2D
static bool is2D(const UTMPoint &pt)
Definition: utm.h:186
geodesy::normalize
static void normalize(UTMPoint &pt)
Definition: utm.h:206
geodesy::isValid
bool isValid(const UTMPoint &pt)
Definition: utm_conversions.cpp:275
geodesy::convert
void convert(const From &from, To &to)
Definition: wgs84.h:227
geodesy::toMsg
geographic_msgs::GeoPoint toMsg(const UTMPoint &from)
Definition: utm_conversions.cpp:119

geodesy
Author(s): Jack O'Quin
autogenerated on Sat May 8 2021 02:29:15