Program Listing for File utm.h

↰ Return to documentation for file (include/geodesy/utm.h)

/* -*- mode: C++ -*- */
/* $Id: 27bd084517a5467049c7b3ed57b3d41e60b3ce53 $ */

/*********************************************************************
* 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 GEODESY__UTM_H_
#define GEODESY__UTM_H_

#include <limits>
#include <ctype.h>
#include <iostream>
#include <iomanip>

#include <geodesy/wgs84.h>
#include "geographic_msgs/msg/geo_point.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geographic_msgs/msg/geo_pose.hpp"
#include "geometry_msgs/msg/pose.hpp"

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::msg::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::msg::GeoPose & pose)
    : position(pose.position),
    orientation(pose.orientation)
{
  }

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

  UTMPose(const geographic_msgs::msg::GeoPoint & pt,
        const geometry_msgs::msg::Quaternion & q)
    : position(pt),
    orientation(q)
{
  }

// data members
  UTMPoint position;
  geometry_msgs::msg::Quaternion orientation;
};  // class UTMPose

// conversion function prototypes
void fromMsg(
  const geographic_msgs::msg::GeoPoint & from, UTMPoint & to,
  const bool & force_zone = false, const char & band = 'A', const uint8_t & zone = 0);
void fromMsg(
  const geographic_msgs::msg::GeoPose & from, UTMPose & to,
  const bool & force_zone = false, const char & band = 'A', const uint8_t & zone = 0);
geographic_msgs::msg::GeoPoint toMsg(const UTMPoint & from);
geographic_msgs::msg::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::msg::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::msg::Point toGeometry(const UTMPoint & from)
{
  geometry_msgs::msg::Point to;
  to.x = from.easting;
  to.y = from.northing;
  to.z = from.altitude;
  return to;
}

static inline geometry_msgs::msg::Pose toGeometry(const UTMPose & from)
{
  geometry_msgs::msg::Pose to;
  to.position = toGeometry(from.position);
  to.orientation = from.orientation;
  return to;
}

}  // namespace geodesy

#endif  // GEODESY__UTM_H_