HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
navigations/geographic_info/geodesy/tests/test_bounding_box.py Executable file
View File

@@ -0,0 +1,50 @@
#!/usr/bin/env python
import unittest
from geodesy.bounding_box import * # module being tested
class TestPythonBoundingBox(unittest.TestCase):
"""Unit tests for Python bounding box functions. """
def test_global_bounding_box(self):
bb = makeGlobal()
self.assertTrue(isGlobal(bb))
def test_2d_bounding_box(self):
min_lat = 30.3787400
min_lon = -97.7344500
max_lat = 30.3947700
max_lon = -97.7230800
bb = makeBounds2D(min_lat, min_lon, max_lat, max_lon)
self.assertFalse(isGlobal(bb))
self.assertTrue(is2D(bb))
min_lat2, min_lon2, max_lat2, max_lon2 = getLatLong(bb)
self.assertEqual(min_lat, min_lat2)
self.assertEqual(min_lon, min_lon2)
self.assertEqual(max_lat, max_lat2)
self.assertEqual(max_lon, max_lon2)
def test_3d_bounding_box(self):
min_lat = 30.3787400
min_lon = -97.7344500
min_alt = 200.0
max_lat = 30.3947700
max_lon = -97.7230800
max_alt = 400.0
bb = makeBounds3D(min_lat, min_lon, min_alt,
max_lat, max_lon, max_alt)
self.assertFalse(isGlobal(bb))
self.assertFalse(is2D(bb))
min_lat2, min_lon2, max_lat2, max_lon2 = getLatLong(bb)
self.assertEqual(min_lat, min_lat2)
self.assertEqual(min_lon, min_lon2)
self.assertEqual(min_alt, bb.min_pt.altitude)
self.assertEqual(max_lat, max_lat2)
self.assertEqual(max_lon, max_lon2)
self.assertEqual(max_alt, bb.max_pt.altitude)
if __name__ == '__main__':
import rosunit
PKG='geodesy'
rosunit.unitrun(PKG, 'test_uuid_py', TestPythonBoundingBox)

76
navigations/geographic_info/geodesy/tests/test_props.py Executable file
View File

@@ -0,0 +1,76 @@
#!/usr/bin/env python
import sys
import unittest
from geodesy.props import * # module being tested
from geographic_msgs.msg import KeyValue
from geographic_msgs.msg import MapFeature
from geographic_msgs.msg import RouteSegment
from geographic_msgs.msg import WayPoint
class TestPythonProps(unittest.TestCase):
"""Unit tests for Python KeyValue property handling.
"""
def test_empty_feature_match(self):
f = MapFeature()
self.assertEqual(match(f, set(['no', 'such', 'property'])), None)
def test_empty_property_set(self):
f = MapFeature()
put(f, 'valid', 'any')
self.assertEqual(match(f, set()), None)
def test_feature_match(self):
f = MapFeature()
put(f, 'different')
put(f, 'valid', 'any')
prop = match(f, set(['a', 'valid', 'property']))
self.assertNotEqual(prop, None)
self.assertEqual(prop, ('valid', 'any'))
k, v = prop
self.assertEqual(k, 'valid')
self.assertEqual(v, 'any')
def test_empty_waypoint_match(self):
p = WayPoint()
self.assertEqual(match(p, set(['nothing', 'defined'])), None)
def test_waypoint_match(self):
p = WayPoint()
put(p, 'another', 'anything')
put(p, 'name', 'myself')
prop = match(p, set(['name']))
self.assertNotEqual(prop, None)
k, v = prop
self.assertEqual(k, 'name')
self.assertEqual(v, 'myself')
def test_segment_value_change(self):
s = RouteSegment()
put(s, 'another', 'anything')
put(s, 'name', 'myself')
self.assertEqual(get(s, 'name'), 'myself')
put(s, 'name', 'alias')
self.assertEqual(get(s, 'name'), 'alias')
def test_segment_null_value(self):
s = RouteSegment()
put(s, 'another', 'anything')
put(s, 'name')
self.assertEqual(get(s, 'name'), '')
put(s, 'name', 'myself')
self.assertEqual(get(s, 'name'), 'myself')
def test_invalid_key_set(self):
f = MapFeature()
put(f, 'notset', 'any')
self.assertEqual(get(f, 'notset'), 'any')
self.assertRaises(ValueError, match, f, 'notset')
if __name__ == '__main__':
import rosunit
PKG='geodesy'
rosunit.unitrun(PKG, 'test_uuid_py', TestPythonProps)

478
navigations/geographic_info/geodesy/tests/test_utm.cpp Executable file
View File

@@ -0,0 +1,478 @@
/* $Id: 47db7b5025f4ca800961335f30ef67b18cfe69ca $ */
/*********************************************************************
* 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 <sstream>
#include <gtest/gtest.h>
#include <angles/angles.h>
#include "geodesy/utm.h"
///////////////////////////////////////////////////////////////
// Utility functions
///////////////////////////////////////////////////////////////
// check that two UTM points are near each other
void check_utm_near(const geodesy::UTMPoint &pt1,
const geodesy::UTMPoint &pt2,
double abs_err)
{
EXPECT_NEAR(pt1.easting, pt2.easting, abs_err);
EXPECT_NEAR(pt1.northing, pt2.northing, abs_err);
EXPECT_NEAR(pt1.altitude, pt2.altitude, abs_err);
EXPECT_EQ(pt1.zone, pt2.zone);
EXPECT_EQ(pt1.band, pt2.band);
}
///////////////////////////////////////////////////////////////
// Test cases
///////////////////////////////////////////////////////////////
// Test null constructor
TEST(UTMPoint, nullConstructor)
{
geodesy::UTMPoint pt;
EXPECT_EQ(pt.easting, 0.0);
EXPECT_EQ(pt.northing, 0.0);
EXPECT_TRUE(geodesy::is2D(pt));
EXPECT_EQ(pt.zone, 0);
EXPECT_EQ(pt.band, ' ');
EXPECT_FALSE(geodesy::isValid(pt));
}
// Test 2D constructor
TEST(UTMPoint, flatConstructor)
{
double e = 1000.0;
double n = 2400.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt(e, n, z, b);
EXPECT_TRUE(geodesy::is2D(pt));
EXPECT_TRUE(geodesy::isValid(pt));
EXPECT_EQ(pt.easting, e);
EXPECT_EQ(pt.northing, n);
EXPECT_EQ(pt.zone, z);
EXPECT_EQ(pt.band, b);
}
// Test 3D constructor
TEST(UTMPoint, hasAltitude)
{
double e = 1000.0;
double n = 2400.0;
double a = 200.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt(e, n, a, z, b);
EXPECT_FALSE(geodesy::is2D(pt));
EXPECT_TRUE(geodesy::isValid(pt));
EXPECT_EQ(pt.easting, e);
EXPECT_EQ(pt.northing, n);
EXPECT_EQ(pt.zone, z);
EXPECT_EQ(pt.band, b);
}
// Test copy constructor
TEST(UTMPoint, copyConstructor)
{
double e = 1000.0;
double n = 2400.0;
double a = 200.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt1(e, n, a, z, b);
geodesy::UTMPoint pt2(pt1);
EXPECT_FALSE(geodesy::is2D(pt2));
EXPECT_TRUE(geodesy::isValid(pt2));
EXPECT_EQ(pt2.easting, e);
EXPECT_EQ(pt2.northing, n);
EXPECT_EQ(pt2.zone, z);
EXPECT_EQ(pt2.band, b);
}
// Test UTM point constructor from WGS 84
TEST(UTMPoint, fromLatLong)
{
// University of Texas, Austin, Pickle Research Campus
double lat = 30.385315;
double lon = -97.728524;
double alt = 209.0;
geographic_msgs::GeoPoint ll;
ll.latitude = lat;
ll.longitude = lon;
ll.altitude = alt;
// create UTM from point
geodesy::UTMPoint pt(ll);
double e = 622159.34;
double n = 3362168.30;
uint8_t z = 14;
char b = 'R';
double abs_err = 0.01;
EXPECT_FALSE(geodesy::is2D(pt));
EXPECT_TRUE(geodesy::isValid(pt));
EXPECT_NEAR(pt.easting, e, abs_err);
EXPECT_NEAR(pt.northing, n, abs_err);
EXPECT_NEAR(pt.altitude, alt, abs_err);
EXPECT_EQ(pt.zone, z);
EXPECT_EQ(pt.band, b);
}
// Test zone numbers
TEST(UTMPoint, testZones)
{
geodesy::UTMPoint pt;
pt.band = 'X'; // supply a valid band letter
pt.zone = 0;
EXPECT_FALSE(geodesy::isValid(pt));
pt.zone = 61;
EXPECT_FALSE(geodesy::isValid(pt));
pt.zone = 255;
EXPECT_FALSE(geodesy::isValid(pt));
// these should all work
for (uint8_t b = 1; b <= 60; ++b)
{
pt.zone = b;
EXPECT_TRUE(geodesy::isValid(pt));
}
}
// Test band letters
TEST(UTMPoint, testBands)
{
geodesy::UTMPoint pt;
pt.zone = 14; // supply a valid zone number
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = '9';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = ';';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = 'I';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = 'O';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = 'Y';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = 'r';
EXPECT_FALSE(geodesy::isValid(pt));
// this should work
pt.band = 'X';
EXPECT_TRUE(geodesy::isValid(pt));
}
// Test null pose constructor
TEST(UTMPose, nullConstructor)
{
geodesy::UTMPose pose;
EXPECT_TRUE(geodesy::is2D(pose));
EXPECT_FALSE(geodesy::isValid(pose));
}
// Test pose constructor from point and quaternion
TEST(UTMPose, pointQuaternionConstructor)
{
double e = 1000.0;
double n = 2400.0;
double a = 200.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt(e, n, a, z, b);
geometry_msgs::Quaternion q; // identity quaternion
q.x = 1.0;
q.y = 0.0;
q.z = 0.0;
q.w = 0.0;
geodesy::UTMPose pose(pt, q);
EXPECT_FALSE(geodesy::is2D(pose));
EXPECT_TRUE(geodesy::isValid(pose));
EXPECT_EQ(pose.position.easting, pt.easting);
EXPECT_EQ(pose.position.northing, pt.northing);
EXPECT_EQ(pose.position.altitude, pt.altitude);
EXPECT_EQ(pose.position.zone, pt.zone);
EXPECT_EQ(pose.position.band, pt.band);
EXPECT_EQ(pose.orientation.w, q.w);
EXPECT_EQ(pose.orientation.x, q.x);
EXPECT_EQ(pose.orientation.y, q.y);
EXPECT_EQ(pose.orientation.z, q.z);
}
// Test pose quaternion validation
TEST(UTMPose, quaternionValidation)
{
double e = 1000.0;
double n = 2400.0;
double a = 200.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt(e, n, a, z, b);
// identity quaternion
geometry_msgs::Quaternion q;
q.x = 1.0;
q.y = 0.0;
q.z = 0.0;
q.w = 0.0;
geodesy::UTMPose pose1(pt, q);
EXPECT_TRUE(geodesy::isValid(pose1));
// valid quaternion
q.x = 0.7071;
q.y = 0.0;
q.z = 0.0;
q.w = 0.7071;
geodesy::UTMPose pose2(pt, q);
EXPECT_TRUE(geodesy::isValid(pose2));
// quaternion not normalized
q.x = 0.8071;
q.y = 0.0;
q.z = 0.0;
q.w = 0.8071;
geodesy::UTMPose pose3(pt, q);
EXPECT_FALSE(geodesy::isValid(pose3));
// zero quaternion (not normalized)
q.x = 0.0;
q.y = 0.0;
q.z = 0.0;
q.w = 0.0;
geodesy::UTMPose pose4(pt, q);
EXPECT_FALSE(geodesy::isValid(pose4));
}
// Test UTM pose conversion
TEST(UTMConvert, fromUtmPoseToLatLongAndBack)
{
double e = 500000.0; // central meridian of each zone
double n = 1000.0;
double alt = 100.0;
char b = 'N';
// try every possible zone of longitude
for (uint8_t z = 1; z <= 60; ++z)
{
for (unsigned int heading = 0; heading < 360; heading++)
{
geodesy::UTMPose ps1(geodesy::UTMPoint(e, n, alt, z, b),
tf::createQuaternionMsgFromYaw(angles::from_degrees(heading)));
geographic_msgs::GeoPose ll;
convert(ps1, ll);
geodesy::UTMPose ps2;
convert(ll, ps2);
EXPECT_TRUE(geodesy::isValid(ps1));
EXPECT_TRUE(geodesy::isValid(ps2));
check_utm_near(ps1.position, ps2.position, 0.000001);
EXPECT_DOUBLE_EQ(tf::getYaw(ps1.orientation), tf::getYaw(ps2.orientation));
}
}
}
// Test conversion from UTM to WGS 84 and back
TEST(UTMConvert, fromUtmToLatLongAndBack)
{
double e = 500000.0; // central meridian of each zone
double n = 1000.0;
double alt = 100.0;
char b = 'N';
// try every possible zone of longitude
for (uint8_t z = 1; z <= 60; ++z)
{
geodesy::UTMPoint pt1(e, n, alt, z, b);
geographic_msgs::GeoPoint ll;
convert(pt1, ll);
geodesy::UTMPoint pt2;
convert(ll, pt2);
EXPECT_TRUE(geodesy::isValid(pt1));
EXPECT_TRUE(geodesy::isValid(pt2));
check_utm_near(pt1, pt2, 0.000001);
}
}
// Test conversion from WGS 84 to UTM and back
TEST(UTMConvert, fromLatLongToUtmAndBack)
{
double alt = 100.0;
// Try every possible degree of latitude and longitude. Avoid the
// international date line. Even though the converted longitude is
// close, it may end up less than -180 and hence inValid().
for (double lon = -179.5; lon < 180.0; lon += 1.0)
{
for (double lat = -80.0; lat <= 84.0; lat += 1.0)
{
geographic_msgs::GeoPoint pt1(geodesy::toMsg(lat, lon, alt));
EXPECT_TRUE(geodesy::isValid(pt1));
geodesy::UTMPoint utm(pt1);
EXPECT_TRUE(geodesy::isValid(utm));
geographic_msgs::GeoPoint pt2(geodesy::toMsg(utm));
EXPECT_TRUE(geodesy::isValid(pt2));
EXPECT_NEAR(pt1.latitude, pt2.latitude, 0.0000001);
EXPECT_NEAR(pt1.longitude, pt2.longitude, 0.0000012);
EXPECT_NEAR(pt1.altitude, pt2.altitude, 0.000001);
}
}
}
// Test conversion from WGS 84 to UTM and back at international date line
TEST(UTMConvert, internationalDateLine)
{
double alt = 100.0;
double lon = -180.0;
for (double lat = -80.0; lat <= 84.0; lat += 1.0)
{
geographic_msgs::GeoPoint pt1(geodesy::toMsg(lat, lon, alt));
EXPECT_TRUE(geodesy::isValid(pt1));
geodesy::UTMPoint utm;
geodesy::fromMsg(pt1, utm);
EXPECT_TRUE(geodesy::isValid(utm));
geographic_msgs::GeoPoint pt2(geodesy::toMsg(utm));
EXPECT_TRUE(geodesy::isValid(pt2));
EXPECT_NEAR(pt1.latitude, pt2.latitude, 0.0000001);
EXPECT_NEAR(pt1.altitude, pt2.altitude, 0.000001);
if (pt2.longitude - pt1.longitude > 359.0)
{
// pt2 seems to be slightly across the international date
// line from pt2, so de-normalize it
pt2.longitude -= 360.0;
}
EXPECT_NEAR(pt1.longitude, pt2.longitude, 0.0000012);
}
}
// Test point output stream operator
TEST(OStream, point)
{
geodesy::UTMPoint pt1;
std::ostringstream out1;
out1 << pt1;
std::string expected("(0, 0, nan [0 ])");
EXPECT_EQ(out1.str(), expected);
geodesy::UTMPoint pt2(622159.338, 3362168.303, 209, 14, 'R');
std::ostringstream out2;
out2 << pt2;
expected = "(622159.338, 3362168.303, 209 [14R])";
EXPECT_EQ(out2.str(), expected);
}
// Test pose output stream operator
TEST(OStream, pose)
{
geodesy::UTMPoint pt(1000.0, 2400.0, 200.0, 14, 'R');
geometry_msgs::Quaternion q;
q.w = 1.0;
q.x = 0.0;
q.y = 0.0;
q.z = 0.0;
geodesy::UTMPose pose(pt, q);
std::ostringstream out;
out << pose;
std::string expected("(1000, 2400, 200 [14R]), ([0, 0, 0], 1)");
EXPECT_EQ(out.str(), expected);
}
TEST(ForceUTMZone, point)
{
geographic_msgs::GeoPoint zone2, zone3;
zone2.latitude=24.02;
zone2 = geodesy::toMsg(24.02, 5.999);
zone3 = geodesy::toMsg(24.02, 6.001);
geodesy::UTMPoint pt2, pt3, pt4;
geodesy::fromMsg(zone2, pt2);
geodesy::fromMsg(zone3, pt3);
EXPECT_FALSE(geodesy::sameGridZone(pt2, pt3) );
double diffx = pt2.easting - pt3.easting;
double diffy = pt2.northing - pt3.northing;
double distance = std::sqrt(diffx*diffx + diffy*diffy);
//Now force the pt3 into pt2's grid zone
geodesy::fromMsg(zone3, pt4, true, pt2.band, pt2.zone);
diffx = pt2.easting - pt4.easting;
diffy = pt2.northing - pt4.northing;
double distance2 = std::sqrt(diffx*diffx + diffy*diffy);
ROS_INFO("Prev Distance %f, Actual Distance %f", distance, distance2);
EXPECT_LT(distance2, distance);
}
// 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();
}

58
navigations/geographic_info/geodesy/tests/test_utm.py Executable file
View File

@@ -0,0 +1,58 @@
#!/usr/bin/env python
import sys
import unittest
from geodesy.utm import *
## A sample python unit test
class TestUTMPoint(unittest.TestCase):
def test_null_constructor(self):
pt = UTMPoint()
self.assertFalse(pt.valid(),
msg='uninitialized UTMPoint should be invalid: ' + str(pt))
self.assertTrue(pt.is2D(),
msg='this UTMPoint should be 2D: ' + str(pt))
self.assertRaises(ValueError, pt.toMsg)
def test_wgs84_utm_conversion_3d(self):
# UTM point in Pickle Research Campus, University of Texas, Austin
ll = GeoPoint(latitude = 30.385315,
longitude = -97.728524,
altitude = 209.0)
pt = fromMsg(ll)
self.assertEqual(str(pt), 'UTM: [622159.338, 3362168.303, 209.000, 14R]',
msg='conversion failed: ' + str(pt))
self.assertTrue(pt.valid(), msg='invalid UTMPoint: ' + str(pt))
self.assertFalse(pt.is2D(), msg='this UTMPoint should be 3D: ' + str(pt))
self.assertEqual(pt.gridZone(), (14, 'R'))
self.assertEqual(str(pt.toMsg()), str(ll),
msg='GeoPoint conversion failed for: ' + str(pt))
point_xyz = pt.toPoint()
self.assertAlmostEqual(point_xyz.x, 622159.338, places = 3)
self.assertAlmostEqual(point_xyz.y, 3362168.303, places = 3)
self.assertAlmostEqual(point_xyz.z, 209.0, places = 3)
def test_wgs84_utm_conversion_2d(self):
# same point, but without altitude
lat = 30.385315
lon = -97.728524
alt = float('nan')
pt = fromLatLong(lat, lon)
self.assertEqual(str(pt), 'UTM: [622159.338, 3362168.303, nan, 14R]',
msg='conversion failed: ' + str(pt))
self.assertTrue(pt.valid(), msg='invalid UTMPoint: ' + str(pt))
self.assertTrue(pt.is2D(), msg='this UTMPoint should be 2D: ' + str(pt))
self.assertEqual(pt.gridZone(), (14, 'R'))
self.assertEqual(str(pt.toMsg()), str(GeoPoint(lat, lon, alt)),
msg='GeoPoint conversion failed for: ' + str(pt))
point_xy = pt.toPoint()
self.assertAlmostEqual(point_xy.x, 622159.338, places = 3)
self.assertAlmostEqual(point_xy.y, 3362168.303, places = 3)
self.assertAlmostEqual(point_xy.z, 0.0, places = 3)
if __name__ == '__main__':
import rosunit
PKG='geodesy'
rosunit.unitrun(PKG, 'test_utm_py', TestUTMPoint)

292
navigations/geographic_info/geodesy/tests/test_wgs84.cpp Executable file
View 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();
}

137
navigations/geographic_info/geodesy/tests/test_wu_point.py Executable file
View File

@@ -0,0 +1,137 @@
#!/usr/bin/env python
import unittest
from geographic_msgs.msg import GeographicMap
from geographic_msgs.msg import GeoPoint
from geographic_msgs.msg import WayPoint
from geometry_msgs.msg import Point
from uuid_msgs.msg import UniqueID
from geodesy.wu_point import *
def fromLatLong(lat, lon, alt=float('nan')):
"""Generate WayPoint from latitude, longitude and (optional) altitude.
:returns: minimal WayPoint object just for test cases.
"""
geo_pt = GeoPoint(latitude = lat, longitude = lon, altitude = alt)
return WayPoint(position = geo_pt)
class TestWuPoint(unittest.TestCase):
"""Unit tests for WuPoint classes.
"""
def test_real_point(self):
ll = GeoPoint(latitude = 30.385315,
longitude = -97.728524,
altitude = 209.0)
msg = WayPoint(position = ll)
pt = WuPoint(msg)
self.assertEqual(pt.toWayPoint(), msg)
self.assertEqual(str(pt.utm),
'UTM: [622159.338, 3362168.303, 209.000, 14R]')
point_xyz = pt.toPoint()
self.assertAlmostEqual(point_xyz.x, 622159.338, places = 3)
self.assertAlmostEqual(point_xyz.y, 3362168.303, places = 3)
self.assertAlmostEqual(point_xyz.z, 209.0, places = 3)
point_xy = pt.toPointXY()
self.assertAlmostEqual(point_xy.x, 622159.338, places = 3)
self.assertAlmostEqual(point_xy.y, 3362168.303, places = 3)
self.assertAlmostEqual(point_xy.z, 0.0, places = 3)
def test_valid_points(self):
lon = -177.0
zone = 1
while lon < 180.0:
pt = WuPoint(fromLatLong(-80.0, lon))
self.assertEqual(pt.utm.gridZone(), (zone, 'C'))
pt = WuPoint(fromLatLong(-30.385315, lon))
self.assertEqual(pt.utm.gridZone(), (zone, 'J'))
pt = WuPoint(fromLatLong(-0.000001, lon))
self.assertEqual(pt.utm.gridZone(), (zone, 'M'))
pt = WuPoint(fromLatLong(0.0, lon))
self.assertEqual(pt.utm.gridZone(), (zone, 'N'))
pt = WuPoint(fromLatLong(30.385315, lon))
self.assertEqual(pt.utm.gridZone(), (zone, 'R'))
pt = WuPoint(fromLatLong(84.0, lon))
self.assertEqual(pt.utm.gridZone(), (zone, 'X'))
lon += 6.0
zone += 1
def test_invalid_points(self):
self.assertRaises(ValueError, WuPoint,
fromLatLong(90.385315, -97.728524))
self.assertRaises(ValueError, WuPoint,
fromLatLong(30.385315, -197.728524))
# this will be valid when we add UPS support for the poles:
self.assertRaises(ValueError, WuPoint,
fromLatLong(-80.385315,-97.728524))
def test_empty_point_set(self):
# test WuPointSet iterator with empty list
wupts = WuPointSet(GeographicMap().points)
i = 0
for w in wupts:
self.fail(msg='there are no points in this map')
i += 1
self.assertEqual(i, 0)
uu = 'da7c242f-2efe-5175-9961-49cc621b80b9'
self.assertEqual(wupts.get(uu), None)
def test_three_point_set(self):
# test data
uuids = ['da7c242f-2efe-5175-9961-49cc621b80b9',
'812f1c08-a34b-5a21-92b9-18b2b0cf4950',
'6f0606f6-a776-5940-b5ea-5e889b32c712']
latitudes = [30.3840168, 30.3857290, 30.3866750]
longitudes = [-97.7282100, -97.7316754, -97.7270967]
eastings = [622191.124, 621856.023, 622294.785]
northings = [3362024.764, 3362210.790, 3362320.569]
points = []
for i in range(len(uuids)):
latlon = GeoPoint(latitude = latitudes[i],
longitude = longitudes[i])
points.append(WayPoint(id = UniqueID(uuid = uuids[i]),
position = latlon))
# test iterator
wupts = WuPointSet(points)
i = 0
for w in wupts:
self.assertEqual(w.uuid(), uuids[i])
self.assertEqual(wupts[uuids[i]].uuid(), uuids[i])
self.assertAlmostEqual(w.utm.easting, eastings[i], places=3)
self.assertAlmostEqual(w.utm.northing, northings[i], places=3)
point_xy = w.toPointXY()
self.assertAlmostEqual(point_xy.x, eastings[i], places = 3)
self.assertAlmostEqual(point_xy.y, northings[i], places = 3)
self.assertAlmostEqual(point_xy.z, 0.0, places = 3)
i += 1
self.assertEqual(i, 3)
self.assertEqual(len(wupts), 3)
bogus = '00000000-c433-5c42-be2e-fbd97ddff9ac'
self.assertFalse(bogus in wupts)
self.assertEqual(wupts.get(bogus), None)
uu = uuids[1]
self.assertTrue(uu in wupts)
wpt = wupts[uu]
self.assertEqual(wpt.uuid(), uu)
self.assertNotEqual(wupts.get(uu), None)
self.assertEqual(wupts.get(uu).uuid(), uu)
# test index() function
for i in xrange(len(uuids)):
self.assertEqual(wupts.index(uuids[i]), i)
self.assertEqual(wupts.points[i].id.uuid, uuids[i])
if __name__ == '__main__':
import rosunit
PKG='geodesy'
rosunit.unitrun(PKG, 'test_xml_map_py', TestWuPoint)