diff --git a/code/perception/src/Position_Publisher_Node.py b/code/perception/src/Position_Publisher_Node.py index 424a1527..87f03ab9 100755 --- a/code/perception/src/Position_Publisher_Node.py +++ b/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): """ diff --git a/code/perception/src/coordinate_transformation.py b/code/perception/src/coordinate_transformation.py index 5c7d4c75..f65dc4cb 100755 --- a/code/perception/src/coordinate_transformation.py +++ b/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): diff --git a/code/planning/global_planner/src/global_planner.py b/code/planning/global_planner/src/global_planner.py index d5926dc5..fdc542d6 100755 --- a/code/planning/global_planner/src/global_planner.py +++ b/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: