git commit -m "first commit"

2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
--- a/navigations/geographic_info/geodesy/CHANGELOG.rst
+++ b/navigations/geographic_info/geodesy/CHANGELOG.rst
@@ -0,0 +1,67 @@
+Change history
+==============
+
+0.5.3 (2018-03-27)
+------------------
+
+0.5.2 (2017-04-15)
+------------------
+
+0.5.1 (2017-04-15)
+------------------
+
+0.5.0 (2017-04-07)
+------------------
+
+ * Removed Python setup install_requires option.
+
+0.4.0 (2014-09-18)
+------------------
+
+ * Released to Indigo.
+ * Remove deprecated geodesy.gen_uuid module (`#4`_).
+
+0.3.2 (2014-08-30)
+------------------
+
+ * Released to Indigo.
+ * Make C++ template declaration for WGS-84 header compatible with GCC
+   4.7 compiler (`#12`_), thanks to Mike Purvis and Dirk Thomas.
+
+0.3.1 (2013-10-03)
+------------------
+
+ * Fix ROS Hydro header install problem (`#9`_).
+
+0.3.0 (2013-08-03)
+------------------
+
+ * Released to Hydro.
+ * Convert to catkin build (`#3`_).
+ * Remove unnecessary roscpp and rospy dependencies (`#6`_).
+
+0.2.1 (2012-08-13)
+------------------
+
+ * Released to Fuerte.
+ * Released to Groovy: 2013-03-27.
+ * Use unique_identifier for UUID interfaces.
+
+0.2.0 (2012-06-02)
+------------------
+
+ * Many new mapping messages and services added.
+ * Use PROJ.4 library for Python geodesy API (C++ API not ported to
+   PROJ.4 yet).
+ * Released to Electric.
+
+0.1.0 (2012-04-10)
+------------------
+
+ * Initial release to Electric.
+
+.. _`#3`: https://github.com/ros-geographic-info/geographic_info/issues/3
+.. _`#4`: https://github.com/ros-geographic-info/geographic_info/issues/4
+.. _`#6`: https://github.com/ros-geographic-info/geographic_info/issues/6
+.. _`#9`: https://github.com/ros-geographic-info/geographic_info/issues/9
+.. _`#12`: https://github.com/ros-geographic-info/geographic_info/issues/12
--- a/navigations/geographic_info/geodesy/CMakeLists.txt
+++ b/navigations/geographic_info/geodesy/CMakeLists.txt
@@ -0,0 +1,56 @@
+# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
+cmake_minimum_required(VERSION 3.0.2)
+project(geodesy)
+find_package(catkin REQUIRED
+             COMPONENTS
+             angles
+             geographic_msgs
+             geometry_msgs
+             sensor_msgs
+             tf
+             unique_id
+             uuid_msgs)
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+catkin_python_setup()
+
+# what other packages will need to use this one
+catkin_package(CATKIN_DEPENDS
+               geographic_msgs
+               geometry_msgs
+               sensor_msgs
+               tf
+               unique_id
+               uuid_msgs
+               INCLUDE_DIRS include
+               LIBRARIES geoconv)
+
+# build C++ coordinate conversion library
+add_library(geoconv src/conv/utm_conversions.cpp)
+target_link_libraries(geoconv ${catkin_LIBRARIES})
+add_dependencies(geoconv ${catkin_EXPORTED_TARGETS})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(TARGETS geoconv
+        RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+# unit tests
+if (CATKIN_ENABLE_TESTING)
+
+  catkin_add_gtest(test_wgs84 tests/test_wgs84.cpp)
+  target_link_libraries(test_wgs84 ${catkin_LIBRARIES})
+  add_dependencies(test_wgs84 ${catkin_EXPORTED_TARGETS})
+  
+  catkin_add_gtest(test_utm tests/test_utm.cpp)
+  target_link_libraries(test_utm geoconv ${catkin_LIBRARIES})
+  add_dependencies(test_utm ${catkin_EXPORTED_TARGETS})
+  
+  catkin_add_nosetests(tests/test_bounding_box.py)
+  catkin_add_nosetests(tests/test_props.py)
+  catkin_add_nosetests(tests/test_utm.py)
+  catkin_add_nosetests(tests/test_wu_point.py)
+
+endif (CATKIN_ENABLE_TESTING)
--- a/navigations/geographic_info/geodesy/doc/CHANGELOG.rst
+++ b/navigations/geographic_info/geodesy/doc/CHANGELOG.rst
@@ -0,0 +1,67 @@
+Change history
+==============
+
+0.5.3 (2018-03-27)
+------------------
+
+0.5.2 (2017-04-15)
+------------------
+
+0.5.1 (2017-04-15)
+------------------
+
+0.5.0 (2017-04-07)
+------------------
+
+ * Removed Python setup install_requires option.
+
+0.4.0 (2014-09-18)
+------------------
+
+ * Released to Indigo.
+ * Remove deprecated geodesy.gen_uuid module (`#4`_).
+
+0.3.2 (2014-08-30)
+------------------
+
+ * Released to Indigo.
+ * Make C++ template declaration for WGS-84 header compatible with GCC
+   4.7 compiler (`#12`_), thanks to Mike Purvis and Dirk Thomas.
+
+0.3.1 (2013-10-03)
+------------------
+
+ * Fix ROS Hydro header install problem (`#9`_).
+
+0.3.0 (2013-08-03)
+------------------
+
+ * Released to Hydro.
+ * Convert to catkin build (`#3`_).
+ * Remove unnecessary roscpp and rospy dependencies (`#6`_).
+
+0.2.1 (2012-08-13)
+------------------
+
+ * Released to Fuerte.
+ * Released to Groovy: 2013-03-27.
+ * Use unique_identifier for UUID interfaces.
+
+0.2.0 (2012-06-02)
+------------------
+
+ * Many new mapping messages and services added.
+ * Use PROJ.4 library for Python geodesy API (C++ API not ported to
+   PROJ.4 yet).
+ * Released to Electric.
+
+0.1.0 (2012-04-10)
+------------------
+
+ * Initial release to Electric.
+
+.. _`#3`: https://github.com/ros-geographic-info/geographic_info/issues/3
+.. _`#4`: https://github.com/ros-geographic-info/geographic_info/issues/4
+.. _`#6`: https://github.com/ros-geographic-info/geographic_info/issues/6
+.. _`#9`: https://github.com/ros-geographic-info/geographic_info/issues/9
+.. _`#12`: https://github.com/ros-geographic-info/geographic_info/issues/12
--- a/navigations/geographic_info/geodesy/doc/conf.py
+++ b/navigations/geographic_info/geodesy/doc/conf.py
@@ -0,0 +1,251 @@
+# -*- coding: utf-8 -*-
+#
+# geodesy documentation build configuration file, created by
+# sphinx-quickstart on Wed Apr 25 16:01:20 2012.
+#
+# This file is execfile()d with the current directory set to its containing dir.
+#
+# Note that not all possible configuration values are present in this
+# autogenerated file.
+#
+# All configuration values have a default; values that are commented out
+# serve to show the default.
+
+## get catkin package information
+import os
+import catkin_pkg.package
+catkin_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+catkin_package = catkin_pkg.package.parse_package(
+    os.path.join(catkin_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME))
+
+# If extensions (or modules to document with autodoc) are in another directory,
+# add these directories to sys.path here. If the directory is relative to the
+# documentation root, use os.path.abspath to make it absolute, like shown here.
+#sys.path.insert(0, os.path.abspath('.'))
+
+# -- General configuration -----------------------------------------------------
+
+# If your documentation needs a minimal Sphinx version, state it here.
+#needs_sphinx = '1.0'
+
+# Add any Sphinx extension module names here, as strings. They can be extensions
+# coming with Sphinx (named 'sphinx.ext.*') or your custom ones.
+extensions = ['sphinx.ext.autodoc', 'sphinx.ext.intersphinx', 'sphinx.ext.todo', 'sphinx.ext.viewcode']
+
+# Add any paths that contain templates here, relative to this directory.
+templates_path = ['_templates']
+
+# The suffix of source filenames.
+source_suffix = '.rst'
+
+# The encoding of source files.
+#source_encoding = 'utf-8-sig'
+
+# The master toctree document.
+master_doc = 'index'
+
+# General information about the project.
+project = u'geodesy'
+copyright = u'2012, Jack O\'Quin'
+
+# The version info for the project you're documenting, acts as replacement for
+# |version| and |release|, also used in various other places throughout the
+# built documents.
+#
+# The short X.Y version.
+version = catkin_package.version
+# The full version, including alpha/beta/rc tags.
+release = catkin_package.version
+
+# The language for content autogenerated by Sphinx. Refer to documentation
+# for a list of supported languages.
+#language = None
+
+# There are two options for replacing |today|: either, you set today to some
+# non-false value, then it is used:
+#today = ''
+# Else, today_fmt is used as the format for a strftime call.
+#today_fmt = '%B %d, %Y'
+
+# List of patterns, relative to source directory, that match files and
+# directories to ignore when looking for source files.
+exclude_patterns = ['_build']
+
+# The reST default role (used for this markup: `text`) to use for all documents.
+#default_role = None
+
+# If true, '()' will be appended to :func: etc. cross-reference text.
+#add_function_parentheses = True
+
+# If true, the current module name will be prepended to all description
+# unit titles (such as .. function::).
+#add_module_names = True
+
+# If true, sectionauthor and moduleauthor directives will be shown in the
+# output. They are ignored by default.
+#show_authors = False
+
+# The name of the Pygments (syntax highlighting) style to use.
+pygments_style = 'sphinx'
+
+# A list of ignored prefixes for module index sorting.
+#modindex_common_prefix = []
+
+
+# -- Options for HTML output ---------------------------------------------------
+
+# The theme to use for HTML and HTML Help pages.  See the documentation for
+# a list of builtin themes.
+html_theme = 'default'
+
+# Theme options are theme-specific and customize the look and feel of a theme
+# further.  For a list of options available for each theme, see the
+# documentation.
+#html_theme_options = {}
+
+# Add any paths that contain custom themes here, relative to this directory.
+#html_theme_path = []
+
+# The name for this set of Sphinx documents.  If None, it defaults to
+# "<project> v<release> documentation".
+#html_title = None
+
+# A shorter title for the navigation bar.  Default is the same as html_title.
+#html_short_title = None
+
+# The name of an image file (relative to this directory) to place at the top
+# of the sidebar.
+#html_logo = None
+
+# The name of an image file (within the static path) to use as favicon of the
+# docs.  This file should be a Windows icon file (.ico) being 16x16 or 32x32
+# pixels large.
+#html_favicon = None
+
+# Add any paths that contain custom static files (such as style sheets) here,
+# relative to this directory. They are copied after the builtin static files,
+# so a file named "default.css" will overwrite the builtin "default.css".
+html_static_path = []
+
+# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
+# using the given strftime format.
+#html_last_updated_fmt = '%b %d, %Y'
+
+# If true, SmartyPants will be used to convert quotes and dashes to
+# typographically correct entities.
+#html_use_smartypants = True
+
+# Custom sidebar templates, maps document names to template names.
+#html_sidebars = {}
+
+# Additional templates that should be rendered to pages, maps page names to
+# template names.
+#html_additional_pages = {}
+
+# If false, no module index is generated.
+#html_domain_indices = True
+
+# If false, no index is generated.
+#html_use_index = True
+
+# If true, the index is split into individual pages for each letter.
+#html_split_index = False
+
+# If true, links to the reST sources are added to the pages.
+#html_show_sourcelink = True
+
+# If true, "Created using Sphinx" is shown in the HTML footer. Default is True.
+#html_show_sphinx = True
+
+# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True.
+#html_show_copyright = True
+
+# If true, an OpenSearch description file will be output, and all pages will
+# contain a <link> tag referring to it.  The value of this option must be the
+# base URL from which the finished HTML is served.
+#html_use_opensearch = ''
+
+# This is the file name suffix for HTML files (e.g. ".xhtml").
+#html_file_suffix = None
+
+# Output file base name for HTML help builder.
+htmlhelp_basename = 'geodesydoc'
+
+
+# -- Options for LaTeX output --------------------------------------------------
+
+latex_elements = {
+# The paper size ('letterpaper' or 'a4paper').
+#'papersize': 'letterpaper',
+
+# The font size ('10pt', '11pt' or '12pt').
+#'pointsize': '10pt',
+
+# Additional stuff for the LaTeX preamble.
+#'preamble': '',
+}
+
+# Grouping the document tree into LaTeX files. List of tuples
+# (source start file, target name, title, author, documentclass [howto/manual]).
+latex_documents = [
+  ('index', 'geodesy.tex', u'geodesy Documentation',
+   u'Jack O\'Quin', 'manual'),
+]
+
+# The name of an image file (relative to this directory) to place at the top of
+# the title page.
+#latex_logo = None
+
+# For "manual" documents, if this is true, then toplevel headings are parts,
+# not chapters.
+#latex_use_parts = False
+
+# If true, show page references after internal links.
+#latex_show_pagerefs = False
+
+# If true, show URL addresses after external links.
+#latex_show_urls = False
+
+# Documents to append as an appendix to all manuals.
+#latex_appendices = []
+
+# If false, no module index is generated.
+#latex_domain_indices = True
+
+
+# -- Options for manual page output --------------------------------------------
+
+# One entry per manual page. List of tuples
+# (source start file, name, description, authors, manual section).
+man_pages = [
+    ('index', 'geodesy', u'geodesy Documentation',
+     [u'Jack O\'Quin'], 1)
+]
+
+# If true, show URL addresses after external links.
+#man_show_urls = False
+
+
+# -- Options for Texinfo output ------------------------------------------------
+
+# Grouping the document tree into Texinfo files. List of tuples
+# (source start file, target name, title, author,
+#  dir menu entry, description, category)
+texinfo_documents = [
+  ('index', 'geodesy', u'geodesy Documentation',
+   u'Jack O\'Quin', 'geodesy', 'One line description of project.',
+   'Miscellaneous'),
+]
+
+# Documents to append as an appendix to all manuals.
+#texinfo_appendices = []
+
+# If false, no module index is generated.
+#texinfo_domain_indices = True
+
+# How to display URL addresses: 'footnote', 'no', or 'inline'.
+#texinfo_show_urls = 'footnote'
+
+
+# Example configuration for intersphinx: refer to the Python standard library.
+intersphinx_mapping = {'http://docs.python.org/': None}
--- a/navigations/geographic_info/geodesy/doc/geodesy.bounding_box.rst
+++ b/navigations/geographic_info/geodesy/doc/geodesy.bounding_box.rst
@@ -0,0 +1,5 @@
+geodesy.bounding_box
+--------------------
+
+.. automodule:: geodesy.bounding_box
+   :members:
--- a/navigations/geographic_info/geodesy/doc/geodesy.gen_uuid.rst
+++ b/navigations/geographic_info/geodesy/doc/geodesy.gen_uuid.rst
@@ -0,0 +1,21 @@
+geodesy.gen_uuid
+----------------
+
+.. module:: geodesy.gen_uuid
+
+Generate UUIDs for Geographic Information messages.
+
+.. deprecated:: 0.3.0
+
+*This module was removed with version 0.4.0 in ROS Indigo.*
+
+Instead, use the `unique_id`_ package, which has been stable and
+available since ROS Fuerte:
+
+ * ``geodesy.gen_uuid.generate(url, id)`` becomes
+   ``unique_id.fromURL(url+str(id))``
+
+ * ``geodesy.gen_uuid.makeUniqueID(url, id)`` becomes
+   ``unique_id.toMsg(unique_id.fromURL(url+str(id)))``
+
+.. _`unique_id`: http://wiki.ros.org/unique_id
--- a/navigations/geographic_info/geodesy/doc/geodesy.props.rst
+++ b/navigations/geographic_info/geodesy/doc/geodesy.props.rst
@@ -0,0 +1,5 @@
+geodesy.props
+-------------
+
+.. automodule:: geodesy.props
+   :members:
--- a/navigations/geographic_info/geodesy/doc/geodesy.utm.rst
+++ b/navigations/geographic_info/geodesy/doc/geodesy.utm.rst
@@ -0,0 +1,5 @@
+geodesy.utm
+-----------
+
+.. automodule:: geodesy.utm
+   :members:
--- a/navigations/geographic_info/geodesy/doc/geodesy.wu_point.rst
+++ b/navigations/geographic_info/geodesy/doc/geodesy.wu_point.rst
@@ -0,0 +1,5 @@
+geodesy.wu_point
+----------------
+
+.. automodule:: geodesy.wu_point
+   :members:
--- a/navigations/geographic_info/geodesy/doc/index.rst
+++ b/navigations/geographic_info/geodesy/doc/index.rst
@@ -0,0 +1,22 @@
+.. geodesy documentation master file, created by
+   sphinx-quickstart on Wed Apr 25 16:01:20 2012.
+   You can adapt this file completely to your liking, but it should at least
+   contain the root `toctree` directive.
+
+geodesy: Python APIs for Geographic coordinates
+===============================================
+
+Contents:
+
+.. toctree::
+   :maxdepth: 2
+   :glob:
+
+   geodesy.*
+   CHANGELOG
+
+Indices and tables
+==================
+
+* :ref:`genindex`
+* :ref:`modindex`
--- a/navigations/geographic_info/geodesy/include/geodesy/utm.h
+++ b/navigations/geographic_info/geodesy/include/geodesy/utm.h
@@ -0,0 +1,266 @@
+/* -*- mode: C++ -*- */
+/* $Id: 284edbd641a3576af22c2d4fbe5f27f1d17ec24d $ */
+
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+#ifndef _UTM_H_
+#define _UTM_H_
+
+#include <limits>
+#include <ctype.h>
+#include <iostream>
+#include <iomanip>
+
+#include <tf/tf.h>
+
+#include <geodesy/wgs84.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Pose.h>
+
+/** @file
+
+    @brief Universal Transverse Mercator coordinates
+
+    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.
+
+    @todo add Universal Polar Stereographic support
+
+    @author Jack O'Quin
+ */
+
+namespace geodesy
+{
+
+/** Universal Transverse Mercator (UTM) point.
+ *
+ *  The @c altitude may be specified (3D) or not (2D).  The @c
+ *  altitude of a 2D point is not a number (NaN).
+ *
+ *  Including the top-level grid zone designator (GZD) from the
+ *  Military Grid Reference System (MGRS) permits unambiguous use of
+ *  Universal Polar Stereographic (UPS) coordinates for the polar
+ *  regions not covered by UTM, making this representation more
+ *  general than pure UTM.
+ */
+class UTMPoint
+{
+ public:
+
+  /** Null constructor. Makes a 2D, invalid point object. */
+  UTMPoint():
+    easting(0.0),
+    northing(0.0),
+    altitude(std::numeric_limits<double>::quiet_NaN()),
+    zone(0),
+    band(' ')
+  {}
+
+  /** Copy constructor. */
+  UTMPoint(const UTMPoint &that):
+    easting(that.easting),
+    northing(that.northing),
+    altitude(that.altitude),
+    zone(that.zone),
+    band(that.band)
+  {}
+
+  UTMPoint(const geographic_msgs::GeoPoint &pt);
+
+  /** Create a flattened 2-D grid point. */
+  UTMPoint(double _easting, double _northing, uint8_t _zone, char _band):
+    easting(_easting),
+    northing(_northing),
+    altitude(std::numeric_limits<double>::quiet_NaN()),
+    zone(_zone),
+    band(_band)
+  {}
+
+  /** Create a 3-D grid point. */
+  UTMPoint(double _easting, double _northing, double _altitude,
+            uint8_t _zone, char _band):
+    easting(_easting),
+    northing(_northing),
+    altitude(_altitude),
+    zone(_zone),
+    band(_band)
+  {}
+
+  // data members
+  double easting;           ///< easting within grid zone [meters]
+  double northing;          ///< northing within grid zone [meters]
+  double altitude;          ///< altitude above ellipsoid [meters] or NaN
+  uint8_t zone;             ///< UTM longitude zone number
+  char   band;              ///< MGRS latitude band letter
+
+}; // class UTMPoint
+
+/** Universal Transverse Mercator (UTM) pose */
+class UTMPose
+{
+ public:
+
+  /** Null constructor. Makes a 2D, invalid pose object. */
+  UTMPose():
+    position(),
+    orientation()
+  {}
+
+  /** Copy constructor. */
+  UTMPose(const UTMPose &that):
+    position(that.position),
+    orientation(that.orientation)
+  {}
+
+  /** Create from a WGS 84 geodetic pose. */
+  UTMPose(const geographic_msgs::GeoPose &pose):
+    position(pose.position),
+    orientation(pose.orientation)
+  {}
+
+  /** Create from a UTMPoint and a quaternion. */
+  UTMPose(UTMPoint pt,
+          const geometry_msgs::Quaternion &q):
+    position(pt),
+    orientation(q)
+  {}
+
+  /** Create from a WGS 84 geodetic point and a quaternion. */
+  UTMPose(const geographic_msgs::GeoPoint &pt,
+          const geometry_msgs::Quaternion &q):
+    position(pt),
+    orientation(q)
+  {}
+
+  // data members
+  UTMPoint position;
+  geometry_msgs::Quaternion orientation;
+
+}; // class UTMPose
+
+// conversion function prototypes
+void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to,
+        const bool& force_zone=false, const char& band='A', const uint8_t& zone=0 );
+void fromMsg(const geographic_msgs::GeoPose &from, UTMPose &to,
+        const bool& force_zone=false, const char& band='A', const uint8_t& zone=0 );
+geographic_msgs::GeoPoint toMsg(const UTMPoint &from);
+geographic_msgs::GeoPose toMsg(const UTMPose &from);
+
+/** @return true if no altitude specified. */
+static inline bool is2D(const UTMPoint &pt)
+{
+  // true if altitude is a NaN
+  return (pt.altitude != pt.altitude);
+}
+
+/** @return true if no altitude specified. */
+static inline bool is2D(const UTMPose &pose)
+{
+  // true if position has no altitude
+  return is2D(pose.position);
+}
+
+bool isValid(const UTMPoint &pt);
+bool isValid(const UTMPose &pose);
+
+/** Normalize UTM point.
+ *
+ *  Ensures the point is within its canonical grid zone.
+ */
+static inline void normalize(UTMPoint &pt)
+{
+  geographic_msgs::GeoPoint ll(toMsg(pt));
+  normalize(ll);
+  fromMsg(ll, pt);
+}
+
+/** Output stream operator for UTM point. */
+static inline std::ostream& operator<<(std::ostream& out, const UTMPoint &pt)
+{
+  out << "(" << std::setprecision(10) << pt.easting << ", "
+      << pt.northing << ", " << std::setprecision(6) << pt.altitude
+      << " [" << (unsigned) pt.zone << pt.band << "])";
+  return out;
+}
+
+/** Output stream operator for UTM pose. */
+static inline std::ostream& operator<<(std::ostream& out, const UTMPose &pose)
+{
+  out << pose.position << ", (["
+      << pose.orientation.x << ", "
+      << pose.orientation.y << ", "
+      << pose.orientation.z << "], "
+      << pose.orientation.w << ")";
+  return out;
+}
+
+/** @return true if two points have the same Grid Zone Designator */
+static inline bool sameGridZone(const UTMPoint &pt1, const UTMPoint &pt2)
+{
+  return ((pt1.zone == pt2.zone) && (pt1.band == pt2.band));
+}
+
+/** @return true if two poses have the same Grid Zone Designator */
+static inline bool sameGridZone(const UTMPose &pose1, const UTMPose &pose2)
+{
+  return sameGridZone(pose1.position, pose2.position);
+}
+
+/** @return a geometry Point corresponding to a UTM point. */
+static inline geometry_msgs::Point toGeometry(const UTMPoint &from)
+{
+  geometry_msgs::Point to;
+  to.x = from.easting;
+  to.y = from.northing;
+  to.z = from.altitude;
+  return to;
+}
+
+/** @return a geometry Pose corresponding to a UTM pose. */
+static inline geometry_msgs::Pose toGeometry(const UTMPose &from)
+{
+  geometry_msgs::Pose to;
+  to.position = toGeometry(from.position);
+  to.orientation = from.orientation;
+  return to;
+}
+
+}; // namespace geodesy
+
+#endif // _UTM_H_
--- a/navigations/geographic_info/geodesy/include/geodesy/wgs84.h
+++ b/navigations/geographic_info/geodesy/include/geodesy/wgs84.h
@@ -0,0 +1,241 @@
+/* -*- mode: C++ -*- */
+/* $Id: 7290b1e8d933f52fa8cbf73baaf239c93a783478 $ */
+
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+#ifndef _WGS84_H_
+#define _WGS84_H_
+
+#include <limits>
+#include <ctype.h>
+#include <geographic_msgs/GeoPoint.h>
+#include <geographic_msgs/GeoPose.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <tf/tf.h>
+
+/** @file
+
+    @brief WGS 84 geodetic system for ROS latitude and longitude messages
+
+    Standard ROS latitude and longitude coordinates are defined in
+    terms of the World Geodetic System (WGS 84) ellipsoid used by most
+    navigation satellite receivers.
+
+    Many other geodetic coordinate systems can be defined.  They
+    should always be converted to WGS 84 when publishing ROS messages
+    to avoid confusion among subscribers.
+    
+    @author Jack O'Quin
+ */
+
+namespace geodesy
+{
+  /** Convert any coordinate to any other via intermediate WGS 84
+   *  representation.
+   *
+   *  @author Tully Foote
+   *
+   *  @note Every coordinate type @b must implement fromMsg() and
+   *        toMsg() functions for both points and poses.
+   */
+  template <class From, class To>
+  void convert(const From &from, To &to);
+
+  /** Convert any coordinate to itself. */
+  template <class Same>
+  void convert(const Same &from, Same &to);
+
+  /** Convert one WGS 84 geodetic point to another.
+   *
+   *  @param from WGS 84 point message.
+   *  @param to another point.
+   */
+  static inline void fromMsg(const geographic_msgs::GeoPoint &from,
+                             geographic_msgs::GeoPoint &to)
+  {
+    convert(from, to);
+  }
+
+  /** Convert one WGS 84 geodetic pose to another.
+   *
+   *  @param from WGS 84 pose message.
+   *  @param to another pose.
+   */
+  static inline void fromMsg(const geographic_msgs::GeoPose &from,
+                             geographic_msgs::GeoPose &to)
+  {
+    convert(from, to);
+  }
+
+  /** @return true if no altitude specified. */
+  static inline bool is2D(const geographic_msgs::GeoPoint &pt)
+  {
+    return (pt.altitude != pt.altitude);
+  }
+
+  /** @return true if pose has no altitude. */
+  static inline bool is2D(const geographic_msgs::GeoPose &pose)
+  {
+    return is2D(pose.position);
+  }
+
+  /** @return true if point is valid. */
+  static inline bool isValid(const geographic_msgs::GeoPoint &pt)
+  {
+    if (pt.latitude < -90.0 || pt.latitude > 90.0)
+      return false;
+
+    if (pt.longitude < -180.0 || pt.longitude >= 180.0)
+      return false;
+
+    return true;
+  }
+
+  /** @return true if pose is valid. */
+  static inline bool isValid(const geographic_msgs::GeoPose &pose)
+  {
+    // check that orientation quaternion is normalized
+    double len2 = (pose.orientation.x * pose.orientation.x
+                   + pose.orientation.y * pose.orientation.y
+                   + pose.orientation.z * pose.orientation.z
+                   + pose.orientation.w * pose.orientation.w);
+    if (fabs(len2 - 1.0) > tf::QUATERNION_TOLERANCE)
+      return false;
+
+    return isValid(pose.position);
+  }
+
+  /** Normalize a WGS 84 geodetic point.
+   *
+   *  @param pt point to normalize.
+   *
+   *  Normalizes the longitude to [-180, 180).
+   *  Clamps latitude to [-90, 90].
+   */
+  static inline void normalize(geographic_msgs::GeoPoint &pt)
+  {
+    pt.longitude =
+      (fmod(fmod((pt.longitude + 180.0), 360.0) + 360.0, 360.0) - 180.0);
+    pt.latitude = std::min(std::max(pt.latitude, -90.0), 90.0);
+  }
+
+  /** @return a 2D WGS 84 geodetic point message. */
+  static inline geographic_msgs::GeoPoint toMsg(double lat, double lon)
+  {
+    geographic_msgs::GeoPoint pt;
+    pt.latitude = lat;
+    pt.longitude = lon;
+    pt.altitude = std::numeric_limits<double>::quiet_NaN();
+    return pt;
+  }
+
+  /** @return a 3D WGS 84 geodetic point message. */
+  static inline geographic_msgs::GeoPoint
+    toMsg(double lat, double lon, double alt)
+  {
+    geographic_msgs::GeoPoint pt;
+    pt.latitude = lat;
+    pt.longitude = lon;
+    pt.altitude = alt;
+    return pt;
+  }
+
+  /** @return a WGS 84 geodetic point message from a NavSatFix. */
+  static inline geographic_msgs::GeoPoint
+    toMsg(const sensor_msgs::NavSatFix &fix)
+  {
+    geographic_msgs::GeoPoint pt;
+    pt.latitude = fix.latitude;
+    pt.longitude = fix.longitude;
+    pt.altitude = fix.altitude;
+    return pt;
+  }
+
+  /** @return a WGS 84 geodetic point message from another. */
+  static inline geographic_msgs::GeoPoint
+    toMsg(const geographic_msgs::GeoPoint &from)
+  {
+    return from;
+  }
+
+  /** @return a WGS 84 geodetic pose message from a point and a
+   *          quaternion.
+   */
+  static inline geographic_msgs::GeoPose
+    toMsg(const geographic_msgs::GeoPoint &pt,
+          const geometry_msgs::Quaternion &q)
+  {
+    geographic_msgs::GeoPose pose;
+    pose.position = pt;
+    pose.orientation = q;
+    return pose;
+  }
+
+  /** @return a WGS 84 geodetic pose message from a NavSatFix and a
+   *          quaternion.
+   */
+  static inline geographic_msgs::GeoPose
+    toMsg(const sensor_msgs::NavSatFix &fix,
+          const geometry_msgs::Quaternion &q)
+  {
+    geographic_msgs::GeoPose pose;
+    pose.position = toMsg(fix);
+    pose.orientation = q;
+    return pose;
+  }
+
+  /** @return a WGS 84 geodetic pose message from another. */
+  static inline geographic_msgs::GeoPose
+    toMsg(const geographic_msgs::GeoPose &from)
+  {
+    return from;
+  }
+
+  template <class From, class To>
+  void convert(const From &from, To &to)
+  {
+    fromMsg(toMsg(from), to);
+  }
+
+  template <class Same>
+  void convert(const Same &from, Same &to)
+  {
+    if (&from != &to)
+      to = from;
+  }
+
+}; // namespace geodesy
+
+#endif // _WGS84_H_
--- a/navigations/geographic_info/geodesy/mainpage.dox
+++ b/navigations/geographic_info/geodesy/mainpage.dox
@@ -0,0 +1,9 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+This package provides C++ APIs for manipulating geodetic coordinates.
+
+It has no nodes, utilities, or scripts.
+
+*/
--- a/navigations/geographic_info/geodesy/package.xml
+++ b/navigations/geographic_info/geodesy/package.xml
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>geodesy</name>
+  <version>0.5.6</version>
+  <description>
+    Python and C++ interfaces for manipulating geodetic coordinates.
+  </description>
+  <maintainer email="jack.oquin@gmail.com">Jack O'Quin</maintainer>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <author>Jack O'Quin</author>
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/geodesy</url>
+  <url type="bugtracker">https://github.com/ros-geographic-info/geographic_info/issues</url>
+  <url type="repository">https://github.com/ros-geographic-info/geographic_info.git</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>angles</build_depend>
+  <build_depend>geographic_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>unique_id</build_depend>
+  <build_depend>uuid_msgs</build_depend>
+
+  <exec_depend>geographic_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>unique_id</exec_depend>
+  <exec_depend>uuid_msgs</exec_depend>
+
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-pyproj</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-pyproj</exec_depend>
+
+  <buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
+  <buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_depend>
+
+  <!-- these dependencies are only for testing -->
+  <test_depend>rosunit</test_depend>
+
+  <!-- documentation dependencies -->
+  <build_depend condition="$ROS_PYTHON_VERSION == 2">python-catkin-pkg-modules</build_depend>
+  <build_depend condition="$ROS_PYTHON_VERSION == 3">python3-catkin-pkg-modules</build_depend>
+
+  <export>
+    <rosdoc config="rosdoc.yaml"/>
+  </export>
+
+</package>
--- a/navigations/geographic_info/geodesy/rosdoc.yaml
+++ b/navigations/geographic_info/geodesy/rosdoc.yaml
@@ -0,0 +1,9 @@
+ - builder: doxygen
+   name: C++ API
+   output_dir: c++
+   file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
+ - builder: sphinx
+   name: Python API
+   sphinx_root_dir: doc
+   output_dir: python
+
--- a/navigations/geographic_info/geodesy/setup.py
+++ b/navigations/geographic_info/geodesy/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from catkin_pkg.python_setup import generate_distutils_setup
+from setuptools import setup
+
+d = generate_distutils_setup(
+    packages=['geodesy'],
+    package_dir={'': 'src'},
+    )
+
+setup(**d)
--- a/navigations/geographic_info/geodesy/src/conv/utm_conversions.cpp
+++ b/navigations/geographic_info/geodesy/src/conv/utm_conversions.cpp
@@ -0,0 +1,339 @@
+/* $Id: 67e70c2a6633d089c804653bac18a8b325f3082b $ */
+
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (C) 2011 Chuck Gantz, 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 <exception>
+#include <ros/ros.h>
+#include <angles/angles.h>
+#include <geodesy/utm.h>
+
+/**  @file
+
+     @brief Universal Transverse Mercator transforms.
+
+     Functions to convert WGS 84 (ellipsoidal) latitude and longitude
+     to and from (Euclidean) UTM coordinates.
+
+     @todo handle Universal Polar Stereographic (UPS) zones
+
+     @author Chuck Gantz, Globalstar, Inc.
+     @author Jack O'Quin, for ROS interface
+*/
+
+namespace geodesy
+{
+
+// WGS84 Parameters
+#define WGS84_A		6378137.0		// major axis
+#define WGS84_B		6356752.31424518	// minor axis
+#define WGS84_F		0.0033528107		// ellipsoid flattening
+#define WGS84_E		0.0818191908		// first eccentricity
+#define WGS84_EP	0.0820944379		// second eccentricity
+
+// UTM Parameters
+#define UTM_K0		0.9996			// scale factor
+#define UTM_FE		500000.0		// false easting
+#define UTM_FN_N	0.0           // false northing, northern hemisphere
+#define UTM_FN_S	10000000.0    // false northing, southern hemisphere
+#define UTM_E2		(WGS84_E*WGS84_E)	// e^2
+#define UTM_E4		(UTM_E2*UTM_E2)		// e^4
+#define UTM_E6		(UTM_E4*UTM_E2)		// e^6
+#define UTM_EP2		(UTM_E2/(1-UTM_E2))	// e'^2
+
+
+/**
+ * Determine the correct UTM band letter for the given latitude.
+ * (Does not currently handle polar (UPS) zones: A, B, Y, Z).
+ *
+ * @return ' ' if latitude is outside the UTM limits of +84 to -80
+ */
+static char UTMBand(double Lat, double Lon)
+{
+  char LetterDesignator;
+
+  if     ((84 >= Lat) && (Lat >= 72))  LetterDesignator = 'X';
+  else if ((72 > Lat) && (Lat >= 64))  LetterDesignator = 'W';
+  else if ((64 > Lat) && (Lat >= 56))  LetterDesignator = 'V';
+  else if ((56 > Lat) && (Lat >= 48))  LetterDesignator = 'U';
+  else if ((48 > Lat) && (Lat >= 40))  LetterDesignator = 'T';
+  else if ((40 > Lat) && (Lat >= 32))  LetterDesignator = 'S';
+  else if ((32 > Lat) && (Lat >= 24))  LetterDesignator = 'R';
+  else if ((24 > Lat) && (Lat >= 16))  LetterDesignator = 'Q';
+  else if ((16 > Lat) && (Lat >= 8))   LetterDesignator = 'P';
+  else if (( 8 > Lat) && (Lat >= 0))   LetterDesignator = 'N';
+  else if (( 0 > Lat) && (Lat >= -8))  LetterDesignator = 'M';
+  else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
+  else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
+  else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
+  else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
+  else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
+  else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
+  else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
+  else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
+  else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
+  // '_' is an error flag, the Latitude is outside the UTM limits
+  else LetterDesignator = ' ';
+
+  return LetterDesignator;
+}
+
+/** Convert UTM point to WGS 84 geodetic point.
+ *
+ *  Equations from USGS Bulletin 1532
+ *
+ *  @param from UTM point.
+ *  @return WGS 84 point message.
+ */
+geographic_msgs::GeoPoint toMsg(const UTMPoint &from)
+{
+  //remove 500,000 meter offset for longitude
+  double x = from.easting - 500000.0;
+  double y = from.northing;
+
+  double k0 = UTM_K0;
+  double a = WGS84_A;
+  double eccSquared = UTM_E2;
+  double eccPrimeSquared;
+  double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
+  double N1, T1, C1, R1, D, M;
+  double LongOrigin;
+  double mu, phi1Rad;
+
+  if ((from.band - 'N') < 0)
+    {
+      //point is in southern hemisphere
+      //remove 10,000,000 meter offset used for southern hemisphere
+      y -= 10000000.0;
+    }
+
+  //+3 puts origin in middle of zone
+  LongOrigin = (from.zone - 1)*6 - 180 + 3;
+  eccPrimeSquared = (eccSquared)/(1-eccSquared);
+
+  M = y / k0;
+  mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64
+             -5*eccSquared*eccSquared*eccSquared/256));
+
+  phi1Rad = mu + ((3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
+                  + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
+                  + (151*e1*e1*e1/96)*sin(6*mu));
+
+  N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
+  T1 = tan(phi1Rad)*tan(phi1Rad);
+  C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
+  R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
+  D = x/(N1*k0);
+
+  // function result
+  geographic_msgs::GeoPoint to;
+  to.altitude = from.altitude;
+  to.latitude =
+    phi1Rad - ((N1*tan(phi1Rad)/R1)
+               *(D*D/2
+                 -(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
+                 +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared
+                   -3*C1*C1)*D*D*D*D*D*D/720));
+  to.latitude = angles::to_degrees(to.latitude);
+  to.longitude = ((D-(1+2*T1+C1)*D*D*D/6
+                   +(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
+                   *D*D*D*D*D/120)
+                  / cos(phi1Rad));
+  to.longitude = LongOrigin + angles::to_degrees(to.longitude);
+
+  // Normalize latitude and longitude to valid ranges.
+  normalize(to);
+  return to;
+}
+
+/** Convert WGS 84 geodetic point to UTM point.
+ *
+ *  Equations from USGS Bulletin 1532
+ *
+ *  @param from WGS 84 point message.
+ *  @param to UTM point.
+ */
+void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to,
+        const bool& force_zone, const char& band, const uint8_t& zone)
+{
+  double Lat = from.latitude;
+  double Long = from.longitude;
+
+  double a = WGS84_A;
+  double eccSquared = UTM_E2;
+  double k0 = UTM_K0;
+
+  double LongOrigin;
+  double eccPrimeSquared;
+  double N, T, C, A, M;
+
+  // Make sure the longitude is between -180.00 .. 179.9
+  // (JOQ: this is broken for Long < -180, do a real normalize)
+  double LongTemp = (Long+180)-int((Long+180)/360)*360-180;
+  double LatRad = angles::from_degrees(Lat);
+  double LongRad = angles::from_degrees(LongTemp);
+  double LongOriginRad;
+
+  to.altitude = from.altitude;
+  if (!force_zone)
+    to.zone = int((LongTemp + 180)/6) + 1;
+  else
+    to.zone = zone;
+
+  if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
+    to.zone = 32;
+
+  // Special zones for Svalbard
+  if( Lat >= 72.0 && Lat < 84.0 )
+    {
+      if(      LongTemp >= 0.0  && LongTemp <  9.0 ) to.zone = 31;
+      else if( LongTemp >= 9.0  && LongTemp < 21.0 ) to.zone = 33;
+      else if( LongTemp >= 21.0 && LongTemp < 33.0 ) to.zone = 35;
+      else if( LongTemp >= 33.0 && LongTemp < 42.0 ) to.zone = 37;
+    }
+  // +3 puts origin in middle of zone
+  LongOrigin = (to.zone - 1)*6 - 180 + 3;
+  LongOriginRad = angles::from_degrees(LongOrigin);
+
+  // compute the UTM band from the latitude
+  if (!force_zone)
+    to.band = UTMBand(Lat, LongTemp);
+  else
+    to.band = band;
+
+
+
+#if 0
+  if (to.band == ' ')
+    throw std::range_error;
+#endif
+
+  eccPrimeSquared = (eccSquared)/(1-eccSquared);
+
+  N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
+  T = tan(LatRad)*tan(LatRad);
+  C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
+  A = cos(LatRad)*(LongRad-LongOriginRad);
+
+  M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64
+          - 5*eccSquared*eccSquared*eccSquared/256) * LatRad
+         - (3*eccSquared/8 + 3*eccSquared*eccSquared/32
+            + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
+         + (15*eccSquared*eccSquared/256
+            + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
+         - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
+
+  to.easting = (double)
+    (k0*N*(A+(1-T+C)*A*A*A/6
+           + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
+     + 500000.0);
+
+  to.northing = (double)
+    (k0*(M+N*tan(LatRad)
+         *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
+           + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
+
+  if(Lat < 0)
+    {
+      //10000000 meter offset for southern hemisphere
+      to.northing += 10000000.0;
+    }
+}
+
+/** @return true if point is valid. */
+bool isValid(const UTMPoint &pt)
+{
+  if (pt.zone < 1 || pt.zone > 60)
+    return false;
+
+  if (!isupper(pt.band) || pt.band == 'I' || pt.band == 'O')
+    return false;
+
+  // The Universal Polar Stereographic bands are not currently
+  // supported.  When they are: A, B, Y and Z will be valid (and the
+  // zone number will be ignored).
+  if (pt.band < 'C' || pt.band > 'X')
+    return false;
+
+  return true;
+}
+
+/** Create UTM point from WGS 84 geodetic point. */
+UTMPoint::UTMPoint(const geographic_msgs::GeoPoint &pt)
+{
+  fromMsg(pt, *this);
+}
+
+/** Convert UTM pose to WGS 84 geodetic pose.
+ *
+ *  @param from UTM pose.
+ *  @return WGS 84 pose message.
+ */
+geographic_msgs::GeoPose toMsg(const UTMPose &from)
+{
+  geographic_msgs::GeoPose to;
+  to.position = toMsg(from.position);
+  to.orientation = from.orientation;
+  return to;
+}
+
+/** Convert WGS 84 geodetic pose to UTM pose.
+ *
+ *  @param from WGS 84 pose message.
+ *  @param to UTM pose.
+ *
+ *  @todo define the orientation transformation properly
+ */
+void fromMsg(const geographic_msgs::GeoPose &from, UTMPose &to,
+        const bool& force_zone, const char& band, const uint8_t& zone)
+{
+  fromMsg(from.position, to.position, force_zone, band, zone);
+  to.orientation = from.orientation;
+}
+
+/** @return true if pose is valid. */
+bool isValid(const UTMPose &pose)
+{
+  if (!isValid(pose.position))
+    return false;
+
+  // check that orientation quaternion is normalized
+  double len2 = (pose.orientation.x * pose.orientation.x
+                 + pose.orientation.y * pose.orientation.y
+                 + pose.orientation.z * pose.orientation.z
+                 + pose.orientation.w * pose.orientation.w);
+  return fabs(len2 - 1.0) <= tf::QUATERNION_TOLERANCE;
+}
+
+} // end namespace geodesy
--- a/navigations/geographic_info/geodesy/src/geodesy/init.py
+++ b/navigations/geographic_info/geodesy/src/geodesy/init.py
--- a/navigations/geographic_info/geodesy/src/geodesy/bounding_box.py
+++ b/navigations/geographic_info/geodesy/src/geodesy/bounding_box.py
@@ -0,0 +1,126 @@
+# 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:: bounding_box
+
+Bounding box functions for geographic maps.
+
+.. _`geographic_msgs/BoundingBox`: http://ros.org/doc/api/geographic_msgs/html/msg/BoundingBox.html
+
+"""
+
+from geographic_msgs.msg import BoundingBox
+
+def getLatLong(bbox):
+    """
+    Get the tuple of minimum and maximum latitudes and longitudes.
+
+    :param bbox: `geographic_msgs/BoundingBox`_.
+    :returns: (min_lat, min_lon, max_lat, max_lon)
+    """
+    return (bbox.min_pt.latitude, bbox.min_pt.longitude,
+            bbox.max_pt.latitude, bbox.max_pt.longitude)
+
+def is2D(bbox):
+    """
+    Two-dimensional bounding box predicate.
+
+    :param bbox: `geographic_msgs/BoundingBox`_.
+    :returns: True if *bbox* matches any altitude.
+    """
+    return bbox.min_pt.altitude != bbox.min_pt.altitude
+
+def isGlobal(bbox):
+    """
+    Global bounding box predicate.
+
+    :param bbox: `geographic_msgs/BoundingBox`_.
+    :returns: True if *bbox* matches any global coordinate.
+    """
+    return bbox.min_pt.latitude != bbox.min_pt.latitude
+
+def makeBounds2D(min_lat, min_lon, max_lat, max_lon):
+    """
+    Create a 2D geographic bounding box (ignoring altitudes).
+
+    :param min_lat: Minimum latitude.
+    :param min_lon: Minimum longitude.
+    :param max_lat: Maximum latitude.
+    :param max_lon: Maximum longitude.
+    :returns: `geographic_msgs/BoundingBox`_ object.
+    """
+    bbox = BoundingBox()
+    bbox.min_pt.latitude =  min_lat
+    bbox.min_pt.longitude = min_lon
+    bbox.min_pt.altitude =  float('nan')
+    bbox.max_pt.latitude =  max_lat
+    bbox.max_pt.longitude = max_lon
+    bbox.max_pt.altitude =  float('nan')
+    return bbox
+
+def makeBounds3D(min_lat, min_lon, min_alt,
+                 max_lat, max_lon, max_alt):
+    """
+    Create a 3D geographic bounding box (including altitudes).
+
+    :param min_lat: Minimum latitude.
+    :param min_lon: Minimum longitude.
+    :param min_alt: Minimum altitude.
+    :param max_lat: Maximum latitude.
+    :param max_lon: Maximum longitude.
+    :param max_alt: Maximum altitude.
+    :returns: `geographic_msgs/BoundingBox`_ object.
+    """
+    bbox = BoundingBox()
+    bbox.min_pt.latitude =  min_lat
+    bbox.min_pt.longitude = min_lon
+    bbox.min_pt.altitude =  min_alt
+    bbox.max_pt.latitude =  max_lat
+    bbox.max_pt.longitude = max_lon
+    bbox.max_pt.altitude =  max_alt
+    return bbox
+
+def makeGlobal():
+    """
+    Create a global bounding box, which matches any valid coordinate.
+
+    :returns: `geographic_msgs/BoundingBox`_ object.
+    """
+    bbox = BoundingBox()
+    bbox.min_pt.latitude = float('nan')
+    bbox.min_pt.longitude = float('nan')
+    bbox.min_pt.altitude = float('nan')
+    bbox.max_pt.latitude = float('nan')
+    bbox.max_pt.longitude = float('nan')
+    bbox.max_pt.altitude = float('nan')
+    return bbox
--- a/navigations/geographic_info/geodesy/src/geodesy/props.py
+++ b/navigations/geographic_info/geodesy/src/geodesy/props.py
@@ -0,0 +1,87 @@
+# 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:: props
+
+`geographic_msgs/KeyValue`_ property interface for Geographic Information messages.
+
+.. _`geographic_msgs/KeyValue`: http://ros.org/doc/api/geographic_msgs/html/msg/KeyValue.html
+
+"""
+
+from geographic_msgs.msg import KeyValue
+
+def get(msg, key):
+    """ Get property value.
+
+    :param msg: Message containing properties.
+    :param key: Property key to match.
+
+    :returns: Corresponding value, if defined; None otherwise.
+              Beware: the value may be '', which evaluates False as a
+              predicate, use ``is not None`` to test for presence.
+    """
+    for prop in msg.props:
+        if prop.key == key:
+            return prop.value
+    return None
+
+def match(msg, key_set):
+    """ Match message properties.
+
+    :param msg:     Message containing properties.
+    :param key_set: Set of property keys to match.
+    :returns: (key, value) of first property matched; None otherwise.
+    :raises: :exc:`ValueError` if key_set is not a set
+    """
+    if type(key_set) is not set:
+        raise ValueError('property matching requires a set of keys')
+    for prop in msg.props:
+        if prop.key in key_set:
+            return (prop.key, prop.value)
+    return None
+
+def put(msg, key, val=''):
+    """ Add KeyValue to message properties.
+
+    :param msg:   Message to update.
+    :param key:   Property key name.
+    :param value: Corresponding value string (default '').
+    """
+    for prop in msg.props:
+        if prop.key == key:
+            # key already present, update value
+            prop.value = str(val)
+            return
+    # key missing, append a new KeyValue pair
+    msg.props.append(KeyValue(key=key, value=str(val)))
--- a/navigations/geographic_info/geodesy/src/geodesy/utm.py
+++ b/navigations/geographic_info/geodesy/src/geodesy/utm.py
@@ -0,0 +1,191 @@
+# 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
+
+from geographic_msgs.msg import GeoPoint
+from geometry_msgs.msg   import Point
+
+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)
+
+    def gridZone(self):
+        """:returns: (zone, band) tuple. """
+        return (self.zone, self.band)
+
+    def is2D(self):
+        """:returns: True if altitude is not defined."""
+        return self.altitude != self.altitude
+
+    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
+
+    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
+
+    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 != ' ')
+
+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)
+
+def fromMsg(msg):
+    """
+    :param msg: `geographic_msgs/GeoPoint`_ message.
+    :returns: :class:`UTMPoint` object.
+    """
+    return fromLatLong(msg.latitude, msg.longitude, msg.altitude)
+
+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)
--- a/navigations/geographic_info/geodesy/src/geodesy/wu_point.py
+++ b/navigations/geographic_info/geodesy/src/geodesy/wu_point.py
@@ -0,0 +1,272 @@
+# 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:: wu_point
+
+Convenience classes for manipulating way points and their associated
+Universal Transverse Mercator (UTM_) coordinates.
+
+.. _UTM: http://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system
+.. _UUID: http://en.wikipedia.org/wiki/Uuid
+
+.. _`geographic_msgs/GeographicMap`: http://ros.org/doc/api/geographic_msgs/html/msg/GeographicMap.html
+.. _`geographic_msgs/GeoPoint`: http://ros.org/doc/api/geographic_msgs/html/msg/GeoPoint.html
+.. _`geographic_msgs/RouteNetwork`: http://ros.org/doc/api/geographic_msgs/html/msg/RouteNetwork.html
+.. _`geographic_msgs/WayPoint`: http://ros.org/doc/api/geographic_msgs/html/msg/WayPoint.html
+.. _`geometry_msgs/Point`: http://ros.org/doc/api/geometry_msgs/html/msg/Point.html
+
+"""
+
+import math
+import geodesy.utm
+
+from geographic_msgs.msg import WayPoint
+from geometry_msgs.msg import Point
+
+class WuPoint():
+    """
+    :class:`WuPoint` represents a map way point with associated UTM_
+    information.
+
+    :param waypt: `geographic_msgs/WayPoint`_ message.
+    :param utm: Corresponding :class:`geodesy.utm.UTMPoint` object. If None
+                provided, the *utm* object will be created.
+
+    .. describe:: str(wu_point)
+
+       :returns: String representation of :class:`WuPoint` object.
+    """
+
+    def __init__(self, waypt, utm=None):
+        """Constructor.
+
+        Collects relevant information from the way point message, and
+        creates the corresponding  :class:`geodesy.utm.UTMPoint`.
+        """
+        self.way_pt = waypt
+
+        if utm:
+            self.utm = utm
+        else:
+            # convert latitude and longitude to UTM (ignoring altitude)
+            self.utm = geodesy.utm.fromMsg(waypt.position)
+
+    def __str__(self):
+        """:returns: String representation of :class:`WuPoint` """
+        # uses python3-compatible str.format() method:
+        return str(self.way_pt) + '\n' + str(self.utm)
+
+    def is2D(self):
+        """:returns: True if no altitude provided. """
+        geo_point = self.way_pt.position
+        return geo_point.altitude != geo_point.altitude
+
+    def position(self):
+        """:returns: Corresponding `geographic_msgs/GeoPoint`_ message."""
+        return self.way_pt.position
+
+    def toPoint(self):
+        """:returns: Corresponding `geometry_msgs/Point`_ message."""
+        return self.utm.toPoint()
+
+    def toPointXY(self):
+        """:returns: `geometry_msgs/Point`_ with X and Y coordinates, and Z coordinate of zero. """
+        return Point(x = self.utm.easting, y = self.utm.northing)
+
+    def toWayPoint(self):
+        """:returns: Corresponding `geographic_msgs/WayPoint`_ message. """
+        return self.way_pt
+
+    def uuid(self):
+        """:returns: UUID_ of way point. """
+        return self.way_pt.id.uuid
+
+class WuPointSet():
+    """
+    :class:`WuPointSet` is a container for the way points in a
+    `geographic_msgs/GeographicMap`_ or
+    `geographic_msgs/RouteNetwork`_ message.  UTM_ coordinates are
+    available for each way point, but they are evaluated lazily, only
+    when needed.
+
+    :param points: array of `geographic_msgs/WayPoint`_ messages
+
+    :class:`WuPointSet` supports these standard container operations:
+
+    .. describe:: len(wu_set)
+
+       :returns: The number of points in the set.
+
+    .. describe:: wu_set[uuid]
+
+       :returns: The point with key *uuid*.  Raises a :exc:`KeyError`
+                 if *uuid* is not in the set.
+
+    .. describe:: uuid in wu_set
+
+       :returns: ``True`` if *wu_set* has a key *uuid*, else ``False``.
+
+    .. describe:: uuid not in wu_set
+
+       Equivalent to ``not uuid in wu_set``.
+
+    .. describe:: iter(wu_set)
+
+       :returns: An iterator over the points in the set.
+
+    These methods are also provided:
+
+    """
+
+    def __init__(self, points):
+        """Constructor.
+
+        Collects relevant way point information from the way point
+        array, and provides convenient access to the data.
+        """
+        self.points = points
+
+        # Initialize way point information.
+        self.way_point_ids = {}         # points symbol table
+        self.n_points = len(self.points)
+        for wid in range(self.n_points):
+            self.way_point_ids[self.points[wid].id.uuid] = wid
+        self.way_point_ids = dict(self.way_point_ids)
+        # Create empty list of UTM points, corresponding to map points.
+        # They will be evaluated lazily, when first needed.
+        self.utm_points = [None for wid in range(self.n_points)]
+
+    def __contains__(self, item):
+        """ Point set membership. """
+        return item in self.way_point_ids
+
+    def __getitem__(self, key):
+        """
+        :param key: UUID_ of desired point.
+        :returns: Named :class:`WuPoint`.
+        :raises: :exc:`KeyError` if no such point
+        """
+        index = self.way_point_ids[key]
+        return self._get_point_with_utm(index)
+
+    def __iter__(self):
+        """ Points iterator. """
+        self.iter_index = 0
+        return self
+
+    def __len__(self):
+        """Point set length."""
+        return self.n_points
+
+    def _get_point_with_utm(self, index):
+        """Get way point with UTM coordinates.
+
+        Creates the corresponding :class:`UTMPoint`, if necessary.
+
+        :param index: Index of point in self.
+        :returns: Corresponding :class:`WuPoint` object.
+        """
+        way_pt = self.points[index]
+        utm_pt = list(self.utm_points)[index]
+        if utm_pt is not None:
+            utm_pt = geodesy.utm.fromMsg(way_pt.position)
+            self.utm_points[index] = utm_pt
+        return WuPoint(way_pt, utm=utm_pt)
+
+    def distance2D(self, idx1, idx2):
+        """ Compute 2D Euclidean distance between points.
+
+        :param idx1: Index of first point.
+        :param idx2: Index of second point.
+
+        :returns: Distance in meters within the UTM XY
+                  plane. Altitudes are ignored.
+        """
+        p1 = self._get_point_with_utm(idx1)
+        p2 = self._get_point_with_utm(idx2)
+        dx = p2.utm.easting - p1.utm.easting
+        dy = p2.utm.northing - p1.utm.northing
+        return math.sqrt(dx*dx + dy*dy)
+
+    def distance3D(self, idx1, idx2):
+        """ Compute 3D Euclidean distance between points.
+
+        :param idx1: Index of first point.
+        :param idx2: Index of second point.
+
+        :returns: Distance in meters between two UTM points, including
+                  altitudes.
+        """
+        p1 = self._get_point_with_utm(idx1)
+        p2 = self._get_point_with_utm(idx2)
+        dx = p2.utm.easting - p1.utm.easting
+        dy = p2.utm.northing - p1.utm.northing
+        dz = p2.utm.altitude - p1.utm.altitude
+        return math.sqrt(dx*dx + dy*dy + dz*dz)
+
+    def get(self, key, default=None):
+        """ Get point, if defined.
+
+        :param key: UUID_ of desired point.
+        :param default: value to return if no such point.
+        :returns: Named :class:`WuPoint`, if successful; otherwise default.
+        """
+        index = self.way_point_ids.get(key)
+        if index is not None:
+            return self._get_point_with_utm(index)
+        else:
+            return default
+
+    def index(self, key, default=None):
+        """ Get index of point, if defined.
+
+        :param key: UUID_ of desired point.
+        :param default: value to return if no such point.
+        :returns: Index of point, if successful; otherwise default.
+                  Beware: the index may be 0, which evaluates False as
+                  a predicate, use ``is not None`` to test for
+                  presence.
+        """
+        return self.way_point_ids.get(key, default)
+
+    def __next__(self):
+        """ Next iteration point.
+
+        :returns: Next :class:`WuPoint`.
+        :raises: :exc:`StopIteration` when finished.
+        """
+        i = self.iter_index
+        if i >= self.n_points:
+            raise StopIteration
+        self.iter_index = i + 1
+        return self._get_point_with_utm(i)
--- a/navigations/geographic_info/geodesy/tests/test_bounding_box.py
+++ b/navigations/geographic_info/geodesy/tests/test_bounding_box.py
@@ -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) 
--- a/navigations/geographic_info/geodesy/tests/test_props.py
+++ b/navigations/geographic_info/geodesy/tests/test_props.py
@@ -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) 
--- a/navigations/geographic_info/geodesy/tests/test_utm.cpp
+++ b/navigations/geographic_info/geodesy/tests/test_utm.cpp
@@ -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();
+}
--- a/navigations/geographic_info/geodesy/tests/test_utm.py
+++ b/navigations/geographic_info/geodesy/tests/test_utm.py
@@ -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) 
--- a/navigations/geographic_info/geodesy/tests/test_wgs84.cpp
+++ b/navigations/geographic_info/geodesy/tests/test_wgs84.cpp
@@ -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();
+}
--- a/navigations/geographic_info/geodesy/tests/test_wu_point.py
+++ b/navigations/geographic_info/geodesy/tests/test_wu_point.py
@@ -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)