git commit -m "first commit"
This commit is contained in:
292
navigations/geographic_info/geodesy/tests/test_wgs84.cpp
Executable file
292
navigations/geographic_info/geodesy/tests/test_wgs84.cpp
Executable file
@@ -0,0 +1,292 @@
|
||||
/* $Id: c7a178b0bbba69e8bf70b12ea1fe76e0cc5a4bff $ */
|
||||
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2011 Jack O'Quin
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * 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.
|
||||
* * Neither the name of the author nor other 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 OWNER 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 <gtest/gtest.h>
|
||||
#include <limits>
|
||||
#include "geodesy/wgs84.h"
|
||||
|
||||
///////////////////////////////////////////////////////////////
|
||||
// Utility functions
|
||||
///////////////////////////////////////////////////////////////
|
||||
|
||||
// check that normalize of (lat1, lon1) yields (lat2, lon2)
|
||||
void check_normalize(double lat1, double lon1, double lat2, double lon2)
|
||||
{
|
||||
double epsilon = 1e-9;
|
||||
geographic_msgs::GeoPoint pt;
|
||||
pt.latitude = lat1;
|
||||
pt.longitude = lon1;
|
||||
geodesy::normalize(pt);
|
||||
EXPECT_NEAR(lat2, pt.latitude, epsilon);
|
||||
EXPECT_NEAR(lon2, pt.longitude, epsilon);
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////
|
||||
// Test cases
|
||||
///////////////////////////////////////////////////////////////
|
||||
|
||||
// Test null point constructor
|
||||
TEST(GeoPoint, nullConstructor)
|
||||
{
|
||||
geographic_msgs::GeoPoint pt;
|
||||
|
||||
EXPECT_FALSE(geodesy::is2D(pt));
|
||||
EXPECT_TRUE(geodesy::isValid(pt));
|
||||
}
|
||||
|
||||
// Test point with no altitude
|
||||
TEST(GeoPoint, noAltitude)
|
||||
{
|
||||
geographic_msgs::GeoPoint pt(geodesy::toMsg(0.0, 0.0));
|
||||
EXPECT_TRUE(geodesy::is2D(pt));
|
||||
}
|
||||
|
||||
// Test creating point from NavSatFix message
|
||||
TEST(GeoPoint, fromNavSatFix)
|
||||
{
|
||||
double lat = 30.0;
|
||||
double lon = -97.0;
|
||||
double alt = 208.0;
|
||||
sensor_msgs::NavSatFix fix;
|
||||
fix.latitude = lat;
|
||||
fix.longitude = lon;
|
||||
fix.altitude = alt;
|
||||
|
||||
geographic_msgs::GeoPoint pt(geodesy::toMsg(fix));
|
||||
|
||||
EXPECT_FALSE(geodesy::is2D(pt));
|
||||
EXPECT_TRUE(geodesy::isValid(pt));
|
||||
|
||||
EXPECT_EQ(pt.latitude, lat);
|
||||
EXPECT_EQ(pt.longitude, lon);
|
||||
EXPECT_EQ(pt.altitude, alt);
|
||||
}
|
||||
|
||||
// Test point with valid altitude
|
||||
TEST(GeoPoint, hasAltitude)
|
||||
{
|
||||
geographic_msgs::GeoPoint pt(geodesy::toMsg(30.0, 206.0, -97.0));
|
||||
EXPECT_FALSE(geodesy::is2D(pt));
|
||||
|
||||
pt.altitude = -100.0;
|
||||
EXPECT_FALSE(geodesy::is2D(pt));
|
||||
|
||||
pt.altitude = 20000.0;
|
||||
EXPECT_FALSE(geodesy::is2D(pt));
|
||||
|
||||
pt.altitude = 0.0;
|
||||
EXPECT_FALSE(geodesy::is2D(pt));
|
||||
}
|
||||
|
||||
// Test valid latitudes and longitudes
|
||||
TEST(GeoPoint, validLatLong)
|
||||
{
|
||||
geographic_msgs::GeoPoint pt;
|
||||
|
||||
pt.latitude = 90.0;
|
||||
EXPECT_TRUE(geodesy::isValid(pt));
|
||||
|
||||
pt.latitude = -90.0;
|
||||
EXPECT_TRUE(geodesy::isValid(pt));
|
||||
|
||||
pt.latitude = 30.0;
|
||||
pt.longitude = -97.0;
|
||||
EXPECT_TRUE(geodesy::isValid(pt));
|
||||
|
||||
pt.longitude = 179.999999;
|
||||
EXPECT_TRUE(geodesy::isValid(pt));
|
||||
|
||||
pt.longitude = -180.0;
|
||||
EXPECT_TRUE(geodesy::isValid(pt));
|
||||
}
|
||||
|
||||
// Test valid latitudes and longitudes
|
||||
TEST(GeoPoint, invalidLatLong)
|
||||
{
|
||||
geographic_msgs::GeoPoint pt;
|
||||
|
||||
pt.latitude = 90.001;
|
||||
EXPECT_FALSE(geodesy::isValid(pt));
|
||||
|
||||
pt.latitude = -90.3;
|
||||
EXPECT_FALSE(geodesy::isValid(pt));
|
||||
|
||||
pt.latitude = 30.0;
|
||||
pt.longitude = -1000.0;
|
||||
EXPECT_FALSE(geodesy::isValid(pt));
|
||||
|
||||
pt.longitude = 180.0;
|
||||
EXPECT_FALSE(geodesy::isValid(pt));
|
||||
|
||||
pt.longitude = 180.0001;
|
||||
EXPECT_FALSE(geodesy::isValid(pt));
|
||||
|
||||
pt.longitude = -180.999;
|
||||
EXPECT_FALSE(geodesy::isValid(pt));
|
||||
}
|
||||
|
||||
TEST(GeoPoint, normalize)
|
||||
{
|
||||
check_normalize(0, 0, 0, 0);
|
||||
|
||||
// longitude range
|
||||
check_normalize(0, 90, 0, 90);
|
||||
check_normalize(0, 180, 0, -180);
|
||||
check_normalize(0, 270, 0, -90);
|
||||
check_normalize(0, 360, 0, 0);
|
||||
check_normalize(0, 450, 0, 90);
|
||||
check_normalize(0, 540, 0, -180);
|
||||
check_normalize(0, 630, 0, -90);
|
||||
check_normalize(0, 720, 0, 0);
|
||||
|
||||
check_normalize(0, -90, 0, -90);
|
||||
check_normalize(0, -180, 0, -180);
|
||||
check_normalize(0, -270, 0, 90);
|
||||
check_normalize(0, -360, 0, 0);
|
||||
check_normalize(0, -450, 0, -90);
|
||||
check_normalize(0, -540, 0, -180);
|
||||
check_normalize(0, -630, 0, 90);
|
||||
check_normalize(0, -720, 0, 0);
|
||||
|
||||
check_normalize(0, 45, 0, 45);
|
||||
check_normalize(0, 135, 0, 135);
|
||||
check_normalize(0, 225, 0, -135);
|
||||
check_normalize(0, 315, 0, -45);
|
||||
|
||||
check_normalize(0, -45, 0, -45);
|
||||
check_normalize(0, -135, 0, -135);
|
||||
check_normalize(0, -225, 0, 135);
|
||||
check_normalize(0, -315, 0, 45);
|
||||
|
||||
check_normalize(0, 91, 0, 91);
|
||||
check_normalize(0, 181, 0, -179);
|
||||
check_normalize(0, 271, 0, -89);
|
||||
check_normalize(0, 361, 0, 1);
|
||||
check_normalize(0, 451, 0, 91);
|
||||
|
||||
check_normalize(0, -91, 0, -91);
|
||||
check_normalize(0, -181, 0, 179);
|
||||
check_normalize(0, -271, 0, 89);
|
||||
check_normalize(0, -361, 0, -1);
|
||||
check_normalize(0, -451, 0, -91);
|
||||
|
||||
// latitude range
|
||||
check_normalize(15, 0, 15, 0);
|
||||
check_normalize(30, 0, 30, 0);
|
||||
check_normalize(45, 0, 45, 0);
|
||||
check_normalize(60, 0, 60, 0);
|
||||
check_normalize(75, 0, 75, 0);
|
||||
check_normalize(90, 0, 90, 0);
|
||||
check_normalize(105, 0, 90, 0);
|
||||
check_normalize(89.999999, 0, 89.999999, 0);
|
||||
check_normalize(90.000001, 0, 90, 0);
|
||||
|
||||
check_normalize(-15, 0, -15, 0);
|
||||
check_normalize(-30, 0, -30, 0);
|
||||
check_normalize(-45, 0, -45, 0);
|
||||
check_normalize(-60, 0, -60, 0);
|
||||
check_normalize(-75, 0, -75, 0);
|
||||
check_normalize(-90, 0, -90, 0);
|
||||
check_normalize(-105, 0, -90, 0);
|
||||
check_normalize(-89.999999, 0, -89.999999, 0);
|
||||
check_normalize(-90.000001, 0, -90, 0);
|
||||
|
||||
}
|
||||
|
||||
// Test null pose constructor
|
||||
TEST(GeoPose, nullConstructor)
|
||||
{
|
||||
geographic_msgs::GeoPose pose;
|
||||
|
||||
EXPECT_FALSE(geodesy::is2D(pose));
|
||||
EXPECT_FALSE(geodesy::isValid(pose));
|
||||
}
|
||||
|
||||
// Test validity of various quaternions
|
||||
TEST(GeoPose, quaternionValidity)
|
||||
{
|
||||
geographic_msgs::GeoPose pose;
|
||||
EXPECT_FALSE(geodesy::isValid(pose));
|
||||
|
||||
pose.orientation.x = 1.0; // identity quaternion
|
||||
EXPECT_TRUE(geodesy::isValid(pose));
|
||||
|
||||
pose.orientation.x = 0.7071; // also valid
|
||||
pose.orientation.y = 0.7071;
|
||||
EXPECT_TRUE(geodesy::isValid(pose));
|
||||
|
||||
pose.orientation.x = 0.8071; // not normalized
|
||||
pose.orientation.y = 0.8071;
|
||||
EXPECT_FALSE(geodesy::isValid(pose));
|
||||
}
|
||||
|
||||
// Test trivial point conversion
|
||||
TEST(Convert, GeoPointToGeoPoint)
|
||||
{
|
||||
geographic_msgs::GeoPoint pt1(geodesy::toMsg(30.0, 206.0, -97.0));
|
||||
geographic_msgs::GeoPoint pt2;
|
||||
|
||||
geodesy::convert(pt1, pt2);
|
||||
|
||||
EXPECT_EQ(pt1.latitude, pt2.latitude);
|
||||
EXPECT_EQ(pt1.longitude, pt2.longitude);
|
||||
EXPECT_EQ(pt1.altitude, pt2.altitude);
|
||||
}
|
||||
|
||||
// Test trivial pose conversion
|
||||
TEST(Convert, GeoPoseToGeoPose)
|
||||
{
|
||||
geographic_msgs::GeoPoint pt(geodesy::toMsg(30.0, 206.0, -97.0));
|
||||
geometry_msgs::Quaternion q;
|
||||
geographic_msgs::GeoPose pose1(geodesy::toMsg(pt, q));
|
||||
geographic_msgs::GeoPose pose2;
|
||||
|
||||
geodesy::convert(pose1, pose2);
|
||||
|
||||
EXPECT_EQ(pose1.position.latitude, pose2.position.latitude);
|
||||
EXPECT_EQ(pose1.position.longitude, pose2.position.longitude);
|
||||
EXPECT_EQ(pose1.position.altitude, pose2.position.altitude);
|
||||
}
|
||||
|
||||
|
||||
// Run all the tests that were declared with TEST()
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// run the tests in this thread
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user