utm.h
Go to the documentation of this file.
1 /* -*- mode: C++ -*- */
2 /* $Id: 284edbd641a3576af22c2d4fbe5f27f1d17ec24d $ */
3 
4 /*********************************************************************
5 * Software License Agreement (BSD License)
6 *
7 * Copyright (C) 2011 Jack O'Quin
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the author nor other contributors may be
21 * used to endorse or promote products derived from this software
22 * without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *********************************************************************/
37 
38 #ifndef _UTM_H_
39 #define _UTM_H_
40 
41 #include <limits>
42 #include <ctype.h>
43 #include <iostream>
44 #include <iomanip>
45 
46 #include <tf/tf.h>
47 
48 #include <geodesy/wgs84.h>
49 #include <geometry_msgs/Point.h>
50 #include <geometry_msgs/Pose.h>
51 
68 namespace geodesy
69 {
70 
82 class UTMPoint
83 {
84  public:
85 
88  easting(0.0),
89  northing(0.0),
90  altitude(std::numeric_limits<double>::quiet_NaN()),
91  zone(0),
92  band(' ')
93  {}
94 
96  UTMPoint(const UTMPoint &that):
97  easting(that.easting),
98  northing(that.northing),
99  altitude(that.altitude),
100  zone(that.zone),
101  band(that.band)
102  {}
103 
104  UTMPoint(const geographic_msgs::GeoPoint &pt);
105 
107  UTMPoint(double _easting, double _northing, uint8_t _zone, char _band):
108  easting(_easting),
109  northing(_northing),
110  altitude(std::numeric_limits<double>::quiet_NaN()),
111  zone(_zone),
112  band(_band)
113  {}
114 
116  UTMPoint(double _easting, double _northing, double _altitude,
117  uint8_t _zone, char _band):
118  easting(_easting),
119  northing(_northing),
120  altitude(_altitude),
121  zone(_zone),
122  band(_band)
123  {}
124 
125  // data members
126  double easting;
127  double northing;
128  double altitude;
129  uint8_t zone;
130  char band;
131 
132 }; // class UTMPoint
133 
135 class UTMPose
136 {
137  public:
138 
141  position(),
142  orientation()
143  {}
144 
146  UTMPose(const UTMPose &that):
147  position(that.position),
148  orientation(that.orientation)
149  {}
150 
152  UTMPose(const geographic_msgs::GeoPose &pose):
153  position(pose.position),
154  orientation(pose.orientation)
155  {}
156 
159  const geometry_msgs::Quaternion &q):
160  position(pt),
161  orientation(q)
162  {}
163 
165  UTMPose(const geographic_msgs::GeoPoint &pt,
166  const geometry_msgs::Quaternion &q):
167  position(pt),
168  orientation(q)
169  {}
170 
171  // data members
173  geometry_msgs::Quaternion orientation;
174 
175 }; // class UTMPose
176 
177 // conversion function prototypes
178 void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to,
179  const bool& force_zone=false, const char& band='A', const uint8_t& zone=0 );
180 void fromMsg(const geographic_msgs::GeoPose &from, UTMPose &to,
181  const bool& force_zone=false, const char& band='A', const uint8_t& zone=0 );
182 geographic_msgs::GeoPoint toMsg(const UTMPoint &from);
183 geographic_msgs::GeoPose toMsg(const UTMPose &from);
184 
186 static inline bool is2D(const UTMPoint &pt)
187 {
188  // true if altitude is a NaN
189  return (pt.altitude != pt.altitude);
190 }
191 
193 static inline bool is2D(const UTMPose &pose)
194 {
195  // true if position has no altitude
196  return is2D(pose.position);
197 }
198 
199 bool isValid(const UTMPoint &pt);
200 bool isValid(const UTMPose &pose);
201 
206 static inline void normalize(UTMPoint &pt)
207 {
208  geographic_msgs::GeoPoint ll(toMsg(pt));
209  normalize(ll);
210  fromMsg(ll, pt);
211 }
212 
214 static inline std::ostream& operator<<(std::ostream& out, const UTMPoint &pt)
215 {
216  out << "(" << std::setprecision(10) << pt.easting << ", "
217  << pt.northing << ", " << std::setprecision(6) << pt.altitude
218  << " [" << (unsigned) pt.zone << pt.band << "])";
219  return out;
220 }
221 
223 static inline std::ostream& operator<<(std::ostream& out, const UTMPose &pose)
224 {
225  out << pose.position << ", (["
226  << pose.orientation.x << ", "
227  << pose.orientation.y << ", "
228  << pose.orientation.z << "], "
229  << pose.orientation.w << ")";
230  return out;
231 }
232 
234 static inline bool sameGridZone(const UTMPoint &pt1, const UTMPoint &pt2)
235 {
236  return ((pt1.zone == pt2.zone) && (pt1.band == pt2.band));
237 }
238 
240 static inline bool sameGridZone(const UTMPose &pose1, const UTMPose &pose2)
241 {
242  return sameGridZone(pose1.position, pose2.position);
243 }
244 
246 static inline geometry_msgs::Point toGeometry(const UTMPoint &from)
247 {
248  geometry_msgs::Point to;
249  to.x = from.easting;
250  to.y = from.northing;
251  to.z = from.altitude;
252  return to;
253 }
254 
256 static inline geometry_msgs::Pose toGeometry(const UTMPose &from)
257 {
258  geometry_msgs::Pose to;
259  to.position = toGeometry(from.position);
260  to.orientation = from.orientation;
261  return to;
262 }
263 
264 }; // namespace geodesy
265 
266 #endif // _UTM_H_
char band
MGRS latitude band letter.
Definition: utm.h:130
WGS 84 geodetic system for ROS latitude and longitude messages.
UTMPose(const UTMPose &that)
Definition: utm.h:146
Definition: utm.h:68
void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to, const bool &force_zone=false, const char &band='A', const uint8_t &zone=0)
UTMPoint(double _easting, double _northing, double _altitude, uint8_t _zone, char _band)
Definition: utm.h:116
double northing
northing within grid zone [meters]
Definition: utm.h:127
double altitude
altitude above ellipsoid [meters] or NaN
Definition: utm.h:128
static bool sameGridZone(const UTMPoint &pt1, const UTMPoint &pt2)
Definition: utm.h:234
geometry_msgs::Quaternion orientation
Definition: utm.h:173
uint8_t zone
UTM longitude zone number.
Definition: utm.h:129
static geometry_msgs::Point toGeometry(const UTMPoint &from)
Definition: utm.h:246
UTMPose(const geographic_msgs::GeoPose &pose)
Definition: utm.h:152
UTMPoint(const UTMPoint &that)
Definition: utm.h:96
UTMPoint(double _easting, double _northing, uint8_t _zone, char _band)
Definition: utm.h:107
static bool is2D(const UTMPoint &pt)
Definition: utm.h:186
static void normalize(UTMPoint &pt)
Definition: utm.h:206
bool isValid(const UTMPoint &pt)
UTMPose(const geographic_msgs::GeoPoint &pt, const geometry_msgs::Quaternion &q)
Definition: utm.h:165
double easting
easting within grid zone [meters]
Definition: utm.h:126
UTMPoint position
Definition: utm.h:172
static std::ostream & operator<<(std::ostream &out, const UTMPoint &pt)
Definition: utm.h:214
geographic_msgs::GeoPoint toMsg(const UTMPoint &from)
UTMPose(UTMPoint pt, const geometry_msgs::Quaternion &q)
Definition: utm.h:158


geodesy
Author(s): Jack O'Quin
autogenerated on Sat May 8 2021 02:29:15