Merge pull request #120 from una-auxme/119-bug-incorrect-position-cal…

…culation

119 bug incorrect position calculation

code/perception/src/Position_Publisher_Node.py

@@ -10,10 +10,10 @@
 from geometry_msgs.msg import PoseStamped
 from sensor_msgs.msg import NavSatFix, Imu
 from nav_msgs.msg import Odometry
-from std_msgs.msg import Float32
-from coordinate_transformation import CoordinateTransformer, GeoRef
+from std_msgs.msg import Float32, String
+from coordinate_transformation import CoordinateTransformer
 from tf.transformations import euler_from_quaternion
-
+from xml.etree import ElementTree as eTree
 GPS_RUNNING_AVG_ARGS: int = 10
 
 
@@ -37,9 +37,15 @@ def __init__(self):
         self.control_loop_rate = self.get_param("control_loop_rate", "0.05")
 
         # todo: automatically detect town
-        self.transformer = CoordinateTransformer(GeoRef.TOWN12)
+        self.transformer = None
 
         # Subscriber
+        self.map_sub = self.new_subscription(
+            String,
+            "/carla/" + self.role_name + "/OpenDRIVE",
+            self.get_geoRef,
+            qos_profile=1)
+
         self.imu_subscriber = self.new_subscription(
             Imu,
             "/carla/" + self.role_name + "/IMU",
@@ -79,6 +85,33 @@ def __init__(self):
             f"/paf/{self.role_name}/current_heading",
             qos_profile=1)
 
+    def get_geoRef(self, opendrive: String):
+        """_summary_
+        Reads the reference values for lat and lon from the carla OpenDriveMap
+        Args:
+            opendrive (String): OpenDrive Map from carla
+        """
+        root = eTree.fromstring(opendrive.data)
+        header = root.find("header")
+        geoRefText = header.find("geoReference").text
+
+        latString = "+lat_0="
+        lonString = "+lon_0="
+
+        indexLat = geoRefText.find(latString)
+        indexLon = geoRefText.find(lonString)
+
+        indexLatEnd = geoRefText.find(" ", indexLat)
+        indexLonEnd = geoRefText.find(" ", indexLon)
+
+        latValue = float(geoRefText[indexLat + len(latString):indexLatEnd])
+        lonValue = float(geoRefText[indexLon + len(lonString):indexLonEnd])
+
+        CoordinateTransformer.la_ref = latValue
+        CoordinateTransformer.ln_ref = lonValue
+        CoordinateTransformer.ref_set = True
+        self.transformer = CoordinateTransformer()
+
     def update_imu_data(self, data: Imu):
         """
         This method is called when new IMU data is received.
@@ -140,34 +173,34 @@ def update_gps_data(self, data: NavSatFix):
         :param data: GNSS measurement
         :return:
         """
-        lat = data.latitude
-        lon = data.longitude
-        alt = data.altitude
-        x, y, z = self.transformer.gnss_to_xyz(lat, lon, alt)
-        # find reason for discrepancy
-        x *= 0.998
-        y *= 1.003
+        # Make sure position is only published when reference values have been
+        # read from the Map
+        if CoordinateTransformer.ref_set:
+            lat = data.latitude
+            lon = data.longitude
+            alt = data.altitude
+            x, y, z = self.transformer.gnss_to_xyz(lat, lon, alt)
 
-        self.avg_xyz = np.roll(self.avg_xyz, -1, axis=0)
-        self.avg_xyz[-1] = np.matrix([x, y, z])
+            self.avg_xyz = np.roll(self.avg_xyz, -1, axis=0)
+            self.avg_xyz[-1] = np.matrix([x, y, z])
 
-        avg_x, avg_y, avg_z = np.mean(self.avg_xyz, axis=0)
+            avg_x, avg_y, avg_z = np.mean(self.avg_xyz, axis=0)
 
-        cur_pos = PoseStamped()
+            cur_pos = PoseStamped()
 
-        cur_pos.header.stamp = data.header.stamp
-        cur_pos.header.frame_id = "global"
+            cur_pos.header.stamp = data.header.stamp
+            cur_pos.header.frame_id = "global"
 
-        cur_pos.pose.position.x = avg_x
-        cur_pos.pose.position.y = avg_y
-        cur_pos.pose.position.z = avg_z
+            cur_pos.pose.position.x = avg_x
+            cur_pos.pose.position.y = avg_y
+            cur_pos.pose.position.z = avg_z
 
-        cur_pos.pose.orientation.x = 0
-        cur_pos.pose.orientation.y = 0
-        cur_pos.pose.orientation.z = 1
-        cur_pos.pose.orientation.w = 0
+            cur_pos.pose.orientation.x = 0
+            cur_pos.pose.orientation.y = 0
+            cur_pos.pose.orientation.z = 1
+            cur_pos.pose.orientation.w = 0
 
-        self.cur_pos_publisher.publish(cur_pos)
+            self.cur_pos_publisher.publish(cur_pos)
 
     def run(self):
         """

code/perception/src/coordinate_transformation.py

@@ -8,27 +8,10 @@
 http://dirsig.cis.rit.edu/docs/new/coordinates.html
 """
 import math
-from enum import Enum
 from tf.transformations import euler_from_quaternion
 
 
-# Class to choose a map with a predefined reference point
-class GeoRef(Enum):
-    TOWN01 = 0, 0, 0
-    TOWN02 = 0, 0, 0
-    TOWN03 = 0, 0, 0
-    TOWN04 = 0, 0, 0
-    TOWN05 = 0, 0, 0
-    TOWN06 = 0, 0, 0  # lat =, lon =, alt = #Town06/HD not found
-    TOWN07 = 0, 0, 0  # lat =, lon =, alt = #Town07/HD not found
-    TOWN08 = 0, 0, 0  # lat =, lon =, alt = #Town08/HD not found
-    TOWN09 = 0, 0, 0  # lat =, lon =, alt = #Town09/HD not found
-    TOWN10 = 0, 0, 0  # Town10HD
-    TOWN11 = 0, 0, 0  # lat =, lon =, alt = #Town11/HD not found
-    TOWN12 = 0, 0, 0  # 35.25000, -101.87500, 331.00000
-
-
-a = 6378137
+a = 6378137  # EARTH_RADIUS_EQUA in Pylot, used in geodetic_to_enu
 b = 6356752.3142
 f = (a - b) / a
 e_sq = f * (2 - f)
@@ -43,19 +26,39 @@ class CoordinateTransformer:
     h_ref: float
     ref_set = False
 
-    def __init__(self, gps_ref: GeoRef):
-        self.la_ref = gps_ref.value[0]
-        self.ln_ref = gps_ref.value[1]
-        self.h_ref = gps_ref.value[2]
+    def __init__(self):
+        pass
 
     def gnss_to_xyz(self, lat, lon, h):
-        return geodetic_to_enu(lat, lon, h,
-                               self.la_ref, self.ln_ref, self.h_ref)
+        return geodetic_to_enu(lat, lon, h)
+
+
+def geodetic_to_enu(lat, lon, alt):
+    """
+    Method from pylot project to calculate coordinates
+    https://github.com/erdos-project/pylot/blob/master/pylot/utils.py#L470
+
+    Args:
+        lat (float): latitude
+        lon (float): longitude
+        alt (float: altitude
+
+    Returns:
+        x, y, z: coordinates
+    """
+
+    scale = math.cos(CoordinateTransformer.la_ref * math.pi / 180.0)
+    basex = scale * math.pi * a / 180.0 * CoordinateTransformer.ln_ref
+    basey = scale * a * math.log(
+        math.tan((90.0 + CoordinateTransformer.la_ref) * math.pi / 360.0))
 
+    x = scale * math.pi * a / 180.0 * lon - basex
+    y = scale * a * math.log(
+        math.tan((90.0 + lat) * math.pi / 360.0)) - basey
 
-def geodetic_to_enu(lat, lon, h, lat_ref, lon_ref, h_ref):
-    x, y, z = geodetic_to_ecef(lat, lon, h)
-    return ecef_to_enu(x, y, z, lat_ref, lon_ref, h_ref)
+    # Is not necessary in new version
+    # y *= -1
+    return x, y, alt + 331.00000
 
 
 def geodetic_to_ecef(lat, lon, h):

code/planning/global_planner/src/global_planner.py

@@ -187,7 +187,6 @@ def global_route_callback(self, data: CarlaRoute) -> None:
         self.path_backup.header.frame_id = "global"
         self.path_backup.poses = stamped_poses
         self.path_pub.publish(self.path_backup)
-
         self.loginfo("PrePlanner: published trajectory")
 
 #    def world_info_callback(self, data: CarlaWorldInfo) -> None: