.. _program_listing_file_include_geodesy_wgs84.h:

Program Listing for File wgs84.h
================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_geodesy_wgs84.h>` (``include/geodesy/wgs84.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

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