utm.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*- */
00002 /* $Id: 1f3d19770631d66c82ea08b4324abcea523ef92b $ */
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 _UTM_H_
00039 #define _UTM_H_
00040 
00041 #include <limits>
00042 #include <ctype.h>
00043 #include <iostream>
00044 #include <iomanip>
00045 
00046 #include <tf/tf.h>
00047 
00048 #include <geodesy/wgs84.h>
00049 #include <geometry_msgs/Point.h>
00050 #include <geometry_msgs/Pose.h>
00051 
00068 namespace geodesy
00069 {
00070 
00082 class UTMPoint
00083 {
00084  public:
00085 
00087   UTMPoint():
00088     easting(0.0),
00089     northing(0.0),
00090     altitude(std::numeric_limits<double>::quiet_NaN()),
00091     zone(0),
00092     band(' ')
00093   {}
00094 
00096   UTMPoint(const UTMPoint &that):
00097     easting(that.easting),
00098     northing(that.northing),
00099     altitude(that.altitude),
00100     zone(that.zone),
00101     band(that.band)
00102   {}
00103   
00104   UTMPoint(const geographic_msgs::GeoPoint &pt);
00105 
00107   UTMPoint(double _easting, double _northing, uint8_t _zone, char _band):
00108     easting(_easting),
00109     northing(_northing),
00110     altitude(std::numeric_limits<double>::quiet_NaN()),
00111     zone(_zone),
00112     band(_band)
00113   {}
00114 
00116   UTMPoint(double _easting, double _northing, double _altitude,
00117             uint8_t _zone, char _band):
00118     easting(_easting),
00119     northing(_northing),
00120     altitude(_altitude),
00121     zone(_zone),
00122     band(_band)
00123   {}
00124 
00125   // data members
00126   double easting;           
00127   double northing;          
00128   double altitude;          
00129   uint8_t zone;             
00130   char   band;              
00131 
00132 }; // class UTMPoint
00133 
00135 class UTMPose
00136 {
00137  public:
00138 
00140   UTMPose():
00141     position(),
00142     orientation()
00143   {}
00144 
00146   UTMPose(const UTMPose &that):
00147     position(that.position),
00148     orientation(that.orientation)
00149   {}
00150   
00152   UTMPose(const geographic_msgs::GeoPose &pose):
00153     position(pose.position),
00154     orientation(pose.orientation)
00155   {}
00156 
00158   UTMPose(UTMPoint pt,
00159           const geometry_msgs::Quaternion &q):
00160     position(pt),
00161     orientation(q)
00162   {}
00163 
00165   UTMPose(const geographic_msgs::GeoPoint &pt,
00166           const geometry_msgs::Quaternion &q):
00167     position(pt),
00168     orientation(q)
00169   {}
00170 
00171   // data members
00172   UTMPoint position;
00173   geometry_msgs::Quaternion orientation;
00174 
00175 }; // class UTMPose
00176 
00177 // conversion function prototypes
00178 void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to);
00179 void fromMsg(const geographic_msgs::GeoPose &from, UTMPose &to);
00180 geographic_msgs::GeoPoint toMsg(const UTMPoint &from);
00181 geographic_msgs::GeoPose toMsg(const UTMPose &from);
00182 
00184 static inline bool is2D(const UTMPoint &pt)
00185 {
00186   // true if altitude is a NaN
00187   return (pt.altitude != pt.altitude);
00188 }
00189 
00191 static inline bool is2D(const UTMPose &pose)
00192 {
00193   // true if position has no altitude
00194   return is2D(pose.position);
00195 }
00196 
00197 bool isValid(const UTMPoint &pt);
00198 bool isValid(const UTMPose &pose);
00199 
00204 static inline void normalize(UTMPoint &pt)
00205 {
00206   geographic_msgs::GeoPoint ll(toMsg(pt));
00207   normalize(ll);
00208   fromMsg(ll, pt);
00209 }
00210 
00212 static inline std::ostream& operator<<(std::ostream& out, const UTMPoint &pt)
00213 {
00214   out << "(" << std::setprecision(10) << pt.easting << ", "
00215       << pt.northing << ", " << std::setprecision(6) << pt.altitude
00216       << " [" << (unsigned) pt.zone << pt.band << "])";
00217   return out;
00218 }
00219 
00221 static inline std::ostream& operator<<(std::ostream& out, const UTMPose &pose)
00222 {
00223   out << pose.position << ", (["
00224       << pose.orientation.x << ", "
00225       << pose.orientation.y << ", "
00226       << pose.orientation.z << "], "
00227       << pose.orientation.w << ")";
00228   return out;
00229 }
00230 
00232 static inline bool sameGridZone(const UTMPoint &pt1, const UTMPoint &pt2)
00233 {
00234   return ((pt1.zone == pt2.zone) && (pt1.band == pt2.band));
00235 }
00236 
00238 static inline bool sameGridZone(const UTMPose &pose1, const UTMPose &pose2)
00239 {
00240   return sameGridZone(pose1.position, pose2.position);
00241 }
00242 
00244 static inline geometry_msgs::Point toGeometry(const UTMPoint &from)
00245 {
00246   geometry_msgs::Point to;
00247   to.x = from.easting;
00248   to.y = from.northing;
00249   to.z = from.altitude;
00250   return to;
00251 }
00252 
00254 static inline geometry_msgs::Pose toGeometry(const UTMPose &from)
00255 {
00256   geometry_msgs::Pose to;
00257   to.position = toGeometry(from.position);
00258   to.orientation = from.orientation;
00259   return to;
00260 }
00261 
00262 }; // namespace geodesy
00263 
00264 #endif // _UTM_H_


geodesy
Author(s): Jack O'Quin
autogenerated on Thu Jun 6 2019 18:29:47