Source code for geodesy.utm

# Software License Agreement (BSD License)
#
# Copyright (C) 2012, 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 of 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.

"""
.. module:: utm

Universal Transverse Mercator coordinate module.

:todo: add Universal Polar Stereographic (UPS_) support

.. _MGRS: http://en.wikipedia.org/wiki/Military_grid_reference_system
.. _`PROJ.4`: http://trac.osgeo.org/proj/
.. _pyproj: http://pypi.python.org/pypi/pyproj 
.. _UPS: http://en.wikipedia.org/wiki/Universal_Polar_Stereographic_coordinate_system
.. _UTM: http://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system

.. _`geometry_msgs/Point`: http://ros.org/doc/api/geometry_msgs/html/msg/Point.html
.. _`geographic_msgs/GeoPoint`: http://ros.org/doc/api/geographic_msgs/html/msg/GeoPoint.html

"""

import math
import pyproj

PKG = 'geodesy'
import roslib; roslib.load_manifest(PKG)
import rospy

from geographic_msgs.msg import GeoPoint
from geometry_msgs.msg   import Point

[docs]class UTMPoint: """Universal Transverse Mercator (UTM_) point class. For outdoor robotics applications, Euclidean projections like UTM_ are easier to work with than latitude and longitude. This system is slightly more general than strict UTM. It is based on the Military Grid Reference System (MGRS_), which can be extended to cover the poles, allowing well-defined transformations for every latitude and longitude. This implementation uses the pyproj_ wrapper for the `PROJ.4`_ Cartographic Projections Library. :param easting: UTM easting (meters) :param northing: UTM northing (meters) :param altitude: altitude above the WGS84 ellipsoid (meters), none if NaN. :param zone: UTM longitude zone :param band: MGRS latitude band letter """ def __init__(self, easting=float('nan'), northing=float('nan'), altitude=float('nan'), zone=0, band=' '): """Construct UTMPoint object. """ self.easting = easting self.northing = northing self.altitude = altitude self.zone = zone self.band = band def __str__(self): """ :returns: string representation of :class:`UTMPoint`. """ # uses python3-compatible str.format() method: return 'UTM: [{0:.3f}, {1:.3f}, {2:.3f}, {3}{4}]'.format( self.easting, self.northing, self.altitude, self.zone, self.band)
[docs] def gridZone(self): """:returns: (zone, band) tuple. """ return (self.zone, self.band)
[docs] def is2D(self): """:returns: True if altitude is not defined.""" return self.altitude != self.altitude
[docs] def toPoint(self): """:returns: corresponding `geometry_msgs/Point`_ message. :todo: clamp message longitude to [-180..180] """ if not self.valid(): raise ValueError('invalid UTM point: ' + str(self)) pt = Point(x = self.easting, y = self.northing) if not self.is2D(): pt.z = self.altitude return pt
[docs] def toMsg(self): """:returns: corresponding `geographic_msgs/GeoPoint`_ message. :todo: clamp message longitude to [-180..180] """ if not self.valid(): raise ValueError('invalid UTM point: ' + str(self)) utm_proj = pyproj.Proj(proj='utm', zone=self.zone, datum='WGS84') msg = GeoPoint(altitude=self.altitude) msg.longitude, msg.latitude = utm_proj(self.easting, self.northing, inverse=True) return msg
[docs] def valid(self): """:returns: True if this is a valid UTM point. """ return (self.easting == self.easting and self.northing == self.northing and self.band != ' ')
[docs]def fromLatLong(latitude, longitude, altitude=float('nan')): """Generate :class:`UTMPoint` from latitude, longitude and (optional) altitude. Latitude and longitude are expressed in degrees, relative to the WGS84 ellipsoid. :param latitude: [degrees], negative is South. :param longitude: [degrees], negative is West. :param altitude: [meters], negative is below the ellipsoid. :returns: :class:`UTMPoint` object. """ z, b = gridZone(latitude, longitude) utm_proj = pyproj.Proj(proj='utm', zone=z, datum='WGS84') e, n = utm_proj(longitude, latitude) return UTMPoint(easting=e, northing=n, altitude=altitude, zone=z, band=b)
[docs]def fromMsg(msg): """ :param msg: `geographic_msgs/GeoPoint`_ message. :returns: :class:`UTMPoint` object. """ return fromLatLong(msg.latitude, msg.longitude, msg.altitude)
[docs]def gridZone(lat, lon): """Find UTM zone and MGRS band for latitude and longitude. :param lat: latitude in degrees, negative is South. :param lon: longitude in degrees, negative is West. :returns: (zone, band) tuple. :raises: :exc:`ValueError` if lon not in [-180..180] or if lat has no corresponding band letter. :todo: handle polar (UPS_) zones: A, B, Y, Z. """ if -180.0 > lon or lon > 180.0: raise ValueError('invalid longitude: ' + str(lon)) zone = int((lon + 180.0)//6.0) + 1 band = ' ' if 84 >= lat and lat >= 72: band = 'X' elif 72 > lat and lat >= 64: band = 'W' elif 64 > lat and lat >= 56: band = 'V' elif 56 > lat and lat >= 48: band = 'U' elif 48 > lat and lat >= 40: band = 'T' elif 40 > lat and lat >= 32: band = 'S' elif 32 > lat and lat >= 24: band = 'R' elif 24 > lat and lat >= 16: band = 'Q' elif 16 > lat and lat >= 8: band = 'P' elif 8 > lat and lat >= 0: band = 'N' elif 0 > lat and lat >= -8: band = 'M' elif -8 > lat and lat >= -16: band = 'L' elif -16 > lat and lat >= -24: band = 'K' elif -24 > lat and lat >= -32: band = 'J' elif -32 > lat and lat >= -40: band = 'H' elif -40 > lat and lat >= -48: band = 'G' elif -48 > lat and lat >= -56: band = 'F' elif -56 > lat and lat >= -64: band = 'E' elif -64 > lat and lat >= -72: band = 'D' elif -72 > lat and lat >= -80: band = 'C' else: raise ValueError('latitude out of UTM range: ' + str(lat)) return (zone, band)