Fixing geoconv class so it uses the right values from the parameter server

src/pyutil/geoconv.py

@@ -47,6 +47,8 @@ class GeoConv:
         lat = rospy.get_param("/world_origin_lat", None)
         lon = rospy.get_param("/world_origin_lon", None)
         elev = rospy.get_param("/world_origin_elevation", None)
+        default_geoid_height_egm96 = rospy.get_param("default_geoid_height_egm96", 0.0)
+        self.geoid_height_egm96 = rospy.get_param("geoid_height_egm96", default_geoid_height_egm96)
         if lat != None and lon != None and elev != None:
 #            print "HAVE WORLD ORIGIN:", lat, lon, elev
             self.world_origin_lat = lat
@@ -89,7 +91,7 @@ class GeoConv:
         res = p
         if p.header.frame_id == "/world" or p.header.frame_id == "world":
             res.point.x, res.point.y = self.to_utm_from_world(p.point.x, p.point.y)
-            res.point.z = self.world_origin_elevation + p.point.z
+            res.point.z = self.world_origin_elevation - self.geoid_height_egm96 + p.point.z
             res.header.frame_id = self.utm_frame_id()
         if p.header.frame_id == "wgs84":
             res.point.x, res.point.y = self.to_utm_from_lon_lat(p.point.x, p.point.y)
@@ -100,7 +102,7 @@ class GeoConv:
         res = p
         if p.header.frame_id == self.utm_frame_id():
             res.point.x, res.point.y = self.to_world_from_utm(p.point.x, p.point.y)
-            res.point.z = p.point.z - self.world_origin_elevation
+            res.point.z = p.point.z + self.geoid_height_egm96 - self.world_origin_elevation
             res.header.frame_id = "world"
         if p.header.frame_id == "wgs84":
             return self.point_to_world(self.point_to_utm(p))