49 #include <geometry_msgs/Point.h> 50 #include <geometry_msgs/Pose.h> 104 UTMPoint(
const geographic_msgs::GeoPoint &pt);
107 UTMPoint(
double _easting,
double _northing, uint8_t _zone,
char _band):
110 altitude(
std::numeric_limits<double>::quiet_NaN()),
116 UTMPoint(
double _easting,
double _northing,
double _altitude,
117 uint8_t _zone,
char _band):
147 position(that.position),
148 orientation(that.orientation)
152 UTMPose(
const geographic_msgs::GeoPose &pose):
153 position(pose.position),
154 orientation(pose.orientation)
159 const geometry_msgs::Quaternion &q):
166 const geometry_msgs::Quaternion &q):
206 geographic_msgs::GeoPoint ll(
toMsg(pt));
214 out <<
"(" << std::setprecision(10) << pt.
easting <<
", " 216 <<
" [" << (unsigned) pt.
zone << pt.
band <<
"])";
246 geometry_msgs::Point to;
256 geometry_msgs::Pose to;
char band
MGRS latitude band letter.
WGS 84 geodetic system for ROS latitude and longitude messages.
UTMPose(const UTMPose &that)
UTMPoint(double _easting, double _northing, double _altitude, uint8_t _zone, char _band)
double northing
northing within grid zone [meters]
double altitude
altitude above ellipsoid [meters] or NaN
static bool sameGridZone(const UTMPoint &pt1, const UTMPoint &pt2)
geometry_msgs::Quaternion orientation
uint8_t zone
UTM longitude zone number.
static geometry_msgs::Point toGeometry(const UTMPoint &from)
UTMPose(const geographic_msgs::GeoPose &pose)
void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to)
UTMPoint(const UTMPoint &that)
UTMPoint(double _easting, double _northing, uint8_t _zone, char _band)
static bool is2D(const UTMPoint &pt)
static void normalize(UTMPoint &pt)
bool isValid(const UTMPoint &pt)
UTMPose(const geographic_msgs::GeoPoint &pt, const geometry_msgs::Quaternion &q)
double easting
easting within grid zone [meters]
static std::ostream & operator<<(std::ostream &out, const UTMPoint &pt)
geographic_msgs::GeoPoint toMsg(const UTMPoint &from)
UTMPose(UTMPoint pt, const geometry_msgs::Quaternion &q)