138 lines
5.2 KiB
Python
Executable File
138 lines
5.2 KiB
Python
Executable File
#!/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)
|