wgs84.h
Go to the documentation of this file.
1 /* -*- mode: C++ -*- */
2 /* $Id: 7290b1e8d933f52fa8cbf73baaf239c93a783478 $ */
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 _WGS84_H_
39 #define _WGS84_H_
40 
41 #include <limits>
42 #include <ctype.h>
43 #include <geographic_msgs/GeoPoint.h>
44 #include <geographic_msgs/GeoPose.h>
45 #include <sensor_msgs/NavSatFix.h>
46 #include <tf/tf.h>
47 
63 namespace geodesy
64 {
73  template <class From, class To>
74  void convert(const From &from, To &to);
75 
77  template <class Same>
78  void convert(const Same &from, Same &to);
79 
85  static inline void fromMsg(const geographic_msgs::GeoPoint &from,
86  geographic_msgs::GeoPoint &to)
87  {
88  convert(from, to);
89  }
90 
96  static inline void fromMsg(const geographic_msgs::GeoPose &from,
97  geographic_msgs::GeoPose &to)
98  {
99  convert(from, to);
100  }
101 
103  static inline bool is2D(const geographic_msgs::GeoPoint &pt)
104  {
105  return (pt.altitude != pt.altitude);
106  }
107 
109  static inline bool is2D(const geographic_msgs::GeoPose &pose)
110  {
111  return is2D(pose.position);
112  }
113 
115  static inline bool isValid(const geographic_msgs::GeoPoint &pt)
116  {
117  if (pt.latitude < -90.0 || pt.latitude > 90.0)
118  return false;
119 
120  if (pt.longitude < -180.0 || pt.longitude >= 180.0)
121  return false;
122 
123  return true;
124  }
125 
127  static inline bool isValid(const geographic_msgs::GeoPose &pose)
128  {
129  // check that orientation quaternion is normalized
130  double len2 = (pose.orientation.x * pose.orientation.x
131  + pose.orientation.y * pose.orientation.y
132  + pose.orientation.z * pose.orientation.z
133  + pose.orientation.w * pose.orientation.w);
134  if (fabs(len2 - 1.0) > tf::QUATERNION_TOLERANCE)
135  return false;
136 
137  return isValid(pose.position);
138  }
139 
147  static inline void normalize(geographic_msgs::GeoPoint &pt)
148  {
149  pt.longitude =
150  (fmod(fmod((pt.longitude + 180.0), 360.0) + 360.0, 360.0) - 180.0);
151  pt.latitude = std::min(std::max(pt.latitude, -90.0), 90.0);
152  }
153 
155  static inline geographic_msgs::GeoPoint toMsg(double lat, double lon)
156  {
157  geographic_msgs::GeoPoint pt;
158  pt.latitude = lat;
159  pt.longitude = lon;
160  pt.altitude = std::numeric_limits<double>::quiet_NaN();
161  return pt;
162  }
163 
165  static inline geographic_msgs::GeoPoint
166  toMsg(double lat, double lon, double alt)
167  {
168  geographic_msgs::GeoPoint pt;
169  pt.latitude = lat;
170  pt.longitude = lon;
171  pt.altitude = alt;
172  return pt;
173  }
174 
176  static inline geographic_msgs::GeoPoint
177  toMsg(const sensor_msgs::NavSatFix &fix)
178  {
179  geographic_msgs::GeoPoint pt;
180  pt.latitude = fix.latitude;
181  pt.longitude = fix.longitude;
182  pt.altitude = fix.altitude;
183  return pt;
184  }
185 
187  static inline geographic_msgs::GeoPoint
188  toMsg(const geographic_msgs::GeoPoint &from)
189  {
190  return from;
191  }
192 
196  static inline geographic_msgs::GeoPose
197  toMsg(const geographic_msgs::GeoPoint &pt,
198  const geometry_msgs::Quaternion &q)
199  {
200  geographic_msgs::GeoPose pose;
201  pose.position = pt;
202  pose.orientation = q;
203  return pose;
204  }
205 
209  static inline geographic_msgs::GeoPose
210  toMsg(const sensor_msgs::NavSatFix &fix,
211  const geometry_msgs::Quaternion &q)
212  {
213  geographic_msgs::GeoPose pose;
214  pose.position = toMsg(fix);
215  pose.orientation = q;
216  return pose;
217  }
218 
220  static inline geographic_msgs::GeoPose
221  toMsg(const geographic_msgs::GeoPose &from)
222  {
223  return from;
224  }
225 
226  template <class From, class To>
227  void convert(const From &from, To &to)
228  {
229  fromMsg(toMsg(from), to);
230  }
231 
232  template <class Same>
233  void convert(const Same &from, Same &to)
234  {
235  if (&from != &to)
236  to = from;
237  }
238 
239 }; // namespace geodesy
240 
241 #endif // _WGS84_H_
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)
static const double QUATERNION_TOLERANCE
static bool is2D(const UTMPoint &pt)
Definition: utm.h:186
static void normalize(UTMPoint &pt)
Definition: utm.h:206
bool isValid(const UTMPoint &pt)
void convert(const From &from, To &to)
Definition: wgs84.h:227
geographic_msgs::GeoPoint toMsg(const UTMPoint &from)


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