Program Listing for File wgs84.h

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

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

/*********************************************************************
* 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/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;

    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 // _WGS84_H_