utm.h

Go to the documentation of this file.

/* -*- mode: C++ -*- */
/* $Id: 8fdd0bc075d283abbd67dd0b4e584e47467fb244 $ */

/*********************************************************************
* 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 <ros/ros.h>
#include <tf/tf.h>

#include <geodesy/wgs84.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>

namespace geodesy
{

class UTMPoint
{
 public:

  UTMPoint():
    easting(0.0),
    northing(0.0),
    altitude(std::numeric_limits<double>::quiet_NaN()),
    zone(0),
    band(' ')
  {}

  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);

  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)
  {}

  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;           
  double northing;          
  double altitude;          
  uint8_t zone;             
  char   band;              

}; // class UTMPoint

class UTMPose
{
 public:

  UTMPose():
    position(),
    orientation()
  {}

  UTMPose(const UTMPose &that):
    position(that.position),
    orientation(that.orientation)
  {}
  
  UTMPose(const geographic_msgs::GeoPose &pose):
    position(pose.position),
    orientation(pose.orientation)
  {}

  UTMPose(UTMPoint pt,
          const geometry_msgs::Quaternion &q):
    position(pt),
    orientation(q)
  {}

  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);
void fromMsg(const geographic_msgs::GeoPose &from, UTMPose &to);
geographic_msgs::GeoPoint toMsg(const UTMPoint &from);
geographic_msgs::GeoPose toMsg(const UTMPose &from);

static inline bool is2D(const UTMPoint &pt)
{
  // true if altitude is a NaN
  return (pt.altitude != pt.altitude);
}

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);

static inline void normalize(UTMPoint &pt)
{
  geographic_msgs::GeoPoint ll(toMsg(pt));
  normalize(ll);
  fromMsg(ll, pt);
}

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;
}

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;
}

static inline bool sameGridZone(const UTMPoint &pt1, const UTMPoint &pt2)
{
  return ((pt1.zone == pt2.zone) && (pt1.band == pt2.band));
}

static inline bool sameGridZone(const UTMPose &pose1, const UTMPose &pose2)
{
  return sameGridZone(pose1.position, pose2.position);
}

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;
}

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_