Fix UTMPose to GeoPose conversion

Fixes issue #36

Author: Roboterbastler

geodesy/src/conv/utm_conversions.cpp

@@ -113,8 +113,8 @@ static char UTMBand(double Lat, double Lon)
  *
  *  Equations from USGS Bulletin 1532
  *
- *  @param from WGS 84 point message.
- *  @return UTM point.
+ *  @param from UTM point.
+ *  @return WGS 84 point message.
  */
 geographic_msgs::GeoPoint toMsg(const UTMPoint &from)
 {
@@ -285,6 +285,19 @@ UTMPoint::UTMPoint(const geographic_msgs::GeoPoint &pt)
   fromMsg(pt, *this);
 }
 
+/** Convert UTM pose to WGS 84 geodetic pose.
+ *
+ *  @param from UTM pose.
+ *  @return WGS 84 pose message.
+ */
+geographic_msgs::GeoPose toMsg(const UTMPose &from)
+{
+  geographic_msgs::GeoPose to;
+  to.position = toMsg(from.position);
+  to.orientation = from.orientation;
+  return to;
+}
+
 /** Convert WGS 84 geodetic pose to UTM pose.
  *
  *  @param from WGS 84 pose message.

geodesy/tests/test_utm.cpp

@@ -36,6 +36,7 @@
 
 #include <sstream>
 #include <gtest/gtest.h>
+#include <angles/angles.h>
 #include "geodesy/utm.h"
 
 
@@ -299,6 +300,34 @@ TEST(UTMPose, quaternionValidation)
   EXPECT_FALSE(geodesy::isValid(pose4));
 }
 
+// Test UTM pose conversion
+TEST(UTMConvert, fromUtmPoseToLatLongAndBack)
+{
+  double e = 500000.0;                  // central meridian of each zone
+  double n = 1000.0;
+  double alt = 100.0;
+  char b = 'N';
+
+  // try every possible zone of longitude
+  for (uint8_t z = 1; z <= 60; ++z)
+  {
+    for (unsigned int heading = 0; heading < 360; heading++)
+    {
+      geodesy::UTMPose ps1(geodesy::UTMPoint(e, n, alt, z, b),
+                           tf::createQuaternionMsgFromYaw(angles::from_degrees(heading)));
+      geographic_msgs::GeoPose ll;
+      convert(ps1, ll);
+      geodesy::UTMPose ps2;
+      convert(ll, ps2);
+
+      EXPECT_TRUE(geodesy::isValid(ps1));
+      EXPECT_TRUE(geodesy::isValid(ps2));
+      check_utm_near(ps1.position, ps2.position, 0.000001);
+      EXPECT_DOUBLE_EQ(tf::getYaw(ps1.orientation), tf::getYaw(ps2.orientation));
+    }
+  }
+}
+
 // Test conversion from UTM to WGS 84 and back
 TEST(UTMConvert, fromUtmToLatLongAndBack)
 {