wgs84.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*- */
00002 /* $Id: 7290b1e8d933f52fa8cbf73baaf239c93a783478 $ */
00003 
00004 /*********************************************************************
00005 * Software License Agreement (BSD License)
00006 *
00007 *  Copyright (C) 2011 Jack O'Quin
00008 *  All rights reserved.
00009 *
00010 *  Redistribution and use in source and binary forms, with or without
00011 *  modification, are permitted provided that the following conditions
00012 *  are met:
00013 *
00014 *   * Redistributions of source code must retain the above copyright
00015 *     notice, this list of conditions and the following disclaimer.
00016 *   * Redistributions in binary form must reproduce the above
00017 *     copyright notice, this list of conditions and the following
00018 *     disclaimer in the documentation and/or other materials provided
00019 *     with the distribution.
00020 *   * Neither the name of the author nor other contributors may be
00021 *     used to endorse or promote products derived from this software
00022 *     without specific prior written permission.
00023 *
00024 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035 *  POSSIBILITY OF SUCH DAMAGE.
00036 *********************************************************************/
00037 
00038 #ifndef _WGS84_H_
00039 #define _WGS84_H_
00040 
00041 #include <limits>
00042 #include <ctype.h>
00043 #include <geographic_msgs/GeoPoint.h>
00044 #include <geographic_msgs/GeoPose.h>
00045 #include <sensor_msgs/NavSatFix.h>
00046 #include <tf/tf.h>
00047 
00063 namespace geodesy
00064 {
00073   template <class From, class To>
00074   void convert(const From &from, To &to);
00075 
00077   template <class Same>
00078   void convert(const Same &from, Same &to);
00079 
00085   static inline void fromMsg(const geographic_msgs::GeoPoint &from,
00086                              geographic_msgs::GeoPoint &to)
00087   {
00088     convert(from, to);
00089   }
00090 
00096   static inline void fromMsg(const geographic_msgs::GeoPose &from,
00097                              geographic_msgs::GeoPose &to)
00098   {
00099     convert(from, to);
00100   }
00101 
00103   static inline bool is2D(const geographic_msgs::GeoPoint &pt)
00104   {
00105     return (pt.altitude != pt.altitude);
00106   }
00107 
00109   static inline bool is2D(const geographic_msgs::GeoPose &pose)
00110   {
00111     return is2D(pose.position);
00112   }
00113 
00115   static inline bool isValid(const geographic_msgs::GeoPoint &pt)
00116   {
00117     if (pt.latitude < -90.0 || pt.latitude > 90.0)
00118       return false;
00119 
00120     if (pt.longitude < -180.0 || pt.longitude >= 180.0)
00121       return false;
00122 
00123     return true;
00124   }
00125 
00127   static inline bool isValid(const geographic_msgs::GeoPose &pose)
00128   {
00129     // check that orientation quaternion is normalized
00130     double len2 = (pose.orientation.x * pose.orientation.x
00131                    + pose.orientation.y * pose.orientation.y
00132                    + pose.orientation.z * pose.orientation.z
00133                    + pose.orientation.w * pose.orientation.w);
00134     if (fabs(len2 - 1.0) > tf::QUATERNION_TOLERANCE)
00135       return false;
00136 
00137     return isValid(pose.position);
00138   }
00139 
00147   static inline void normalize(geographic_msgs::GeoPoint &pt)
00148   {
00149     pt.longitude =
00150       (fmod(fmod((pt.longitude + 180.0), 360.0) + 360.0, 360.0) - 180.0);
00151     pt.latitude = std::min(std::max(pt.latitude, -90.0), 90.0);
00152   }
00153 
00155   static inline geographic_msgs::GeoPoint toMsg(double lat, double lon)
00156   {
00157     geographic_msgs::GeoPoint pt;
00158     pt.latitude = lat;
00159     pt.longitude = lon;
00160     pt.altitude = std::numeric_limits<double>::quiet_NaN();
00161     return pt;
00162   }
00163 
00165   static inline geographic_msgs::GeoPoint
00166     toMsg(double lat, double lon, double alt)
00167   {
00168     geographic_msgs::GeoPoint pt;
00169     pt.latitude = lat;
00170     pt.longitude = lon;
00171     pt.altitude = alt;
00172     return pt;
00173   }
00174 
00176   static inline geographic_msgs::GeoPoint
00177     toMsg(const sensor_msgs::NavSatFix &fix)
00178   {
00179     geographic_msgs::GeoPoint pt;
00180     pt.latitude = fix.latitude;
00181     pt.longitude = fix.longitude;
00182     pt.altitude = fix.altitude;
00183     return pt;
00184   }
00185 
00187   static inline geographic_msgs::GeoPoint
00188     toMsg(const geographic_msgs::GeoPoint &from)
00189   {
00190     return from;
00191   }
00192 
00196   static inline geographic_msgs::GeoPose
00197     toMsg(const geographic_msgs::GeoPoint &pt,
00198           const geometry_msgs::Quaternion &q)
00199   {
00200     geographic_msgs::GeoPose pose;
00201     pose.position = pt;
00202     pose.orientation = q;
00203     return pose;
00204   }
00205 
00209   static inline geographic_msgs::GeoPose
00210     toMsg(const sensor_msgs::NavSatFix &fix,
00211           const geometry_msgs::Quaternion &q)
00212   {
00213     geographic_msgs::GeoPose pose;
00214     pose.position = toMsg(fix);
00215     pose.orientation = q;
00216     return pose;
00217   }
00218 
00220   static inline geographic_msgs::GeoPose
00221     toMsg(const geographic_msgs::GeoPose &from)
00222   {
00223     return from;
00224   }
00225 
00226   template <class From, class To>
00227   void convert(const From &from, To &to)
00228   {
00229     fromMsg(toMsg(from), to);
00230   }
00231 
00232   template <class Same>
00233   void convert(const Same &from, Same &to)
00234   {
00235     if (&from != &to)
00236       to = from;
00237   }
00238 
00239 }; // namespace geodesy
00240 
00241 #endif // _WGS84_H_


geodesy
Author(s): Jack O'Quin
autogenerated on Sun Apr 16 2017 02:52:12