Program Listing for File wgs84.h

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

/* -*- mode: C++ -*- */
/* $Id: ef6b9a0f8fae991dd258c38b0e49c4429c3b6d73 $ */

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

#include <limits>
#include <ctype.h>
#include <cmath>
#include "geographic_msgs/msg/geo_point.hpp"
#include "geographic_msgs/msg/geo_pose.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"

#define TF_QUATERNION_TOLERANCE 0.1

namespace geodesy
{
  template < class From, class To >
  void convert(const From & from, To & to);

  template < class Same >
  void convert(const Same & from, Same & to);

  static inline void fromMsg(
    const geographic_msgs::msg::GeoPoint & from,
    geographic_msgs::msg::GeoPoint & to)
  {
    convert(from, to);
  }

  static inline void fromMsg(
    const geographic_msgs::msg::GeoPose & from,
    geographic_msgs::msg::GeoPose & to)
  {
    convert(from, to);
  }

  static inline bool is2D(const geographic_msgs::msg::GeoPoint & pt)
  {
    return  pt.altitude != pt.altitude;
  }

  static inline bool is2D(const geographic_msgs::msg::GeoPose & pose)
  {
    return is2D(pose.position);
  }

  static inline bool isValid(const geographic_msgs::msg::GeoPoint & pt)
  {
    if (pt.latitude < -90.0 || pt.latitude > 90.0) {
      return false;
    }

    if (pt.longitude < -180.0 || pt.longitude >= 180.0) {
      return false;
    }

    if (std::isnan(pt.latitude) || std::isnan(pt.longitude)) {
      return false;
    }

    return true;
  }

  static inline bool isValid(const geographic_msgs::msg::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);
  }

  static inline void normalize(geographic_msgs::msg::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);
  }

  static inline geographic_msgs::msg::GeoPoint toMsg(double lat, double lon)
  {
    geographic_msgs::msg::GeoPoint pt;
    pt.latitude = lat;
    pt.longitude = lon;
    pt.altitude = std::numeric_limits < double > ::quiet_NaN();
    return pt;
  }

  static inline geographic_msgs::msg::GeoPoint
  toMsg(double lat, double lon, double alt)
  {
    geographic_msgs::msg::GeoPoint pt;
    pt.latitude = lat;
    pt.longitude = lon;
    pt.altitude = alt;
    return pt;
  }

  static inline geographic_msgs::msg::GeoPoint
  toMsg(const sensor_msgs::msg::NavSatFix & fix)
  {
    geographic_msgs::msg::GeoPoint pt;
    pt.latitude = fix.latitude;
    pt.longitude = fix.longitude;
    pt.altitude = fix.altitude;
    return pt;
  }

  static inline geographic_msgs::msg::GeoPoint
  toMsg(const geographic_msgs::msg::GeoPoint & from)
  {
    return from;
  }

  static inline geographic_msgs::msg::GeoPose
  toMsg(
    const geographic_msgs::msg::GeoPoint & pt,
    const geometry_msgs::msg::Quaternion & q)
  {
    geographic_msgs::msg::GeoPose pose;
    pose.position = pt;
    pose.orientation = q;
    return pose;
  }

  static inline geographic_msgs::msg::GeoPose
  toMsg(
    const sensor_msgs::msg::NavSatFix & fix,
    const geometry_msgs::msg::Quaternion & q)
  {
    geographic_msgs::msg::GeoPose pose;
    pose.position = toMsg(fix);
    pose.orientation = q;
    return pose;
  }

  static inline geographic_msgs::msg::GeoPose
  toMsg(const geographic_msgs::msg::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  // GEODESY__WGS84_H_