From 3194c467c4afc895b364010e99bd8f2a8cd9e040 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 30 Nov 2023 18:43:00 +0100 Subject: [PATCH 1/3] fix: Fixed calculation from geodetic to xyz --- .../perception/src/Position_Publisher_Node.py | 86 +++++++++++++------ .../src/coordinate_transformation.py | 57 ++++++------ .../global_planner/src/global_planner.py | 34 ++++++++ 3 files changed, 125 insertions(+), 52 deletions(-) diff --git a/code/perception/src/Position_Publisher_Node.py b/code/perception/src/Position_Publisher_Node.py index 424a1527..3e3b2ecf 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,36 @@ 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 + """ + self.logerr("GEOREGASDFJKAOFAOGAD: MapUpdate called") + root = eTree.fromstring(opendrive.data) + header = root.find("header") + self.logerr(f"header tags: {header.tag}") + geoRefText = header.find("geoReference").text + self.logerr(f"total header: {geoRefText}") + + 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 +176,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..b8c86f85 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 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..6b2c026a 100755 --- a/code/planning/global_planner/src/global_planner.py +++ b/code/planning/global_planner/src/global_planner.py @@ -10,6 +10,7 @@ from nav_msgs.msg import Path from std_msgs.msg import String from std_msgs.msg import Float32MultiArray +import carla from preplanning_trajectory import OpenDriveConverter @@ -36,6 +37,21 @@ class PrePlanner(CompatibleNode): def __init__(self): super(PrePlanner, self).__init__('DevGlobalRoute') + # get real data + self.host = rospy.get_param('~host', 'carla-simulator') + self.port = rospy.get_param('~port', 2000) + timeout = rospy.get_param('~timeout', 100.0) + + # Connect to the CARLA server + self.client = carla.Client(self.host, self.port) + self.client.set_timeout(timeout) + + # Get the world + self.world = self.client.get_world() + self.vehicle = None + self.ideal_x = None + self.ideal_y = None + self.path_backup = Path() self.odc = None @@ -79,6 +95,10 @@ def __init__(self): qos_profile=1) self.loginfo('PrePlanner-Node started') + def get_ideal_position(self, data: PoseStamped): + self.ideal_x = data.poses.position.x + self.ideal_y = data.poses.position.y + def global_route_callback(self, data: CarlaRoute) -> None: """ when the global route gets updated a new trajectory is calculated with @@ -107,6 +127,8 @@ def global_route_callback(self, data: CarlaRoute) -> None: y_start = self.agent_pos.y # -5433.2 x_target = data.poses[0].position.x y_target = data.poses[0].position.y + self.logerr(f"first waypoint: {x_target}, {y_target}") + self.logerr(f"current pos: {x_start}, {y_start}") if abs(x_start - x_target) > self.distance_spawn_to_first_wp or \ abs(y_start - y_target) > self.distance_spawn_to_first_wp: self.logwarn("PrePlanner: current agent-pose doesnt match the " @@ -187,6 +209,18 @@ 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.logerr(f"Trajectory {self.path_backup}") + self.logerr(f"Position: {x_start}. {y_start}. {self.agent_pos.z}") + + # Get the ego vehicle + if self.vehicle is None: + for actor in self.world.get_actors(): + self.logdebug(f"role: {actor.attributes.get('role_name')}") + if actor.attributes.get('role_name') == "hero": + self.vehicle = actor + break + pos = self.vehicle.get_location() + self.logerr(f"""Real position: {pos.x},{pos.y}""") self.loginfo("PrePlanner: published trajectory") From 9980bab75792ec12beb71623f901bdddcfbcdf69 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 30 Nov 2023 18:48:27 +0100 Subject: [PATCH 2/3] fix: removed logging commands from debugging --- .../perception/src/Position_Publisher_Node.py | 3 -- .../global_planner/src/global_planner.py | 35 ------------------- 2 files changed, 38 deletions(-) diff --git a/code/perception/src/Position_Publisher_Node.py b/code/perception/src/Position_Publisher_Node.py index 3e3b2ecf..87f03ab9 100755 --- a/code/perception/src/Position_Publisher_Node.py +++ b/code/perception/src/Position_Publisher_Node.py @@ -91,12 +91,9 @@ def get_geoRef(self, opendrive: String): Args: opendrive (String): OpenDrive Map from carla """ - self.logerr("GEOREGASDFJKAOFAOGAD: MapUpdate called") root = eTree.fromstring(opendrive.data) header = root.find("header") - self.logerr(f"header tags: {header.tag}") geoRefText = header.find("geoReference").text - self.logerr(f"total header: {geoRefText}") latString = "+lat_0=" lonString = "+lon_0=" diff --git a/code/planning/global_planner/src/global_planner.py b/code/planning/global_planner/src/global_planner.py index 6b2c026a..fdc542d6 100755 --- a/code/planning/global_planner/src/global_planner.py +++ b/code/planning/global_planner/src/global_planner.py @@ -10,7 +10,6 @@ from nav_msgs.msg import Path from std_msgs.msg import String from std_msgs.msg import Float32MultiArray -import carla from preplanning_trajectory import OpenDriveConverter @@ -37,21 +36,6 @@ class PrePlanner(CompatibleNode): def __init__(self): super(PrePlanner, self).__init__('DevGlobalRoute') - # get real data - self.host = rospy.get_param('~host', 'carla-simulator') - self.port = rospy.get_param('~port', 2000) - timeout = rospy.get_param('~timeout', 100.0) - - # Connect to the CARLA server - self.client = carla.Client(self.host, self.port) - self.client.set_timeout(timeout) - - # Get the world - self.world = self.client.get_world() - self.vehicle = None - self.ideal_x = None - self.ideal_y = None - self.path_backup = Path() self.odc = None @@ -95,10 +79,6 @@ def __init__(self): qos_profile=1) self.loginfo('PrePlanner-Node started') - def get_ideal_position(self, data: PoseStamped): - self.ideal_x = data.poses.position.x - self.ideal_y = data.poses.position.y - def global_route_callback(self, data: CarlaRoute) -> None: """ when the global route gets updated a new trajectory is calculated with @@ -127,8 +107,6 @@ def global_route_callback(self, data: CarlaRoute) -> None: y_start = self.agent_pos.y # -5433.2 x_target = data.poses[0].position.x y_target = data.poses[0].position.y - self.logerr(f"first waypoint: {x_target}, {y_target}") - self.logerr(f"current pos: {x_start}, {y_start}") if abs(x_start - x_target) > self.distance_spawn_to_first_wp or \ abs(y_start - y_target) > self.distance_spawn_to_first_wp: self.logwarn("PrePlanner: current agent-pose doesnt match the " @@ -209,19 +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.logerr(f"Trajectory {self.path_backup}") - self.logerr(f"Position: {x_start}. {y_start}. {self.agent_pos.z}") - - # Get the ego vehicle - if self.vehicle is None: - for actor in self.world.get_actors(): - self.logdebug(f"role: {actor.attributes.get('role_name')}") - if actor.attributes.get('role_name') == "hero": - self.vehicle = actor - break - pos = self.vehicle.get_location() - self.logerr(f"""Real position: {pos.x},{pos.y}""") - self.loginfo("PrePlanner: published trajectory") # def world_info_callback(self, data: CarlaWorldInfo) -> None: From d689182fba1bc67cff78b2af2fcb09ce5c9a52e1 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 1 Dec 2023 10:16:34 +0100 Subject: [PATCH 3/3] doc: comment added --- code/perception/src/coordinate_transformation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/perception/src/coordinate_transformation.py b/code/perception/src/coordinate_transformation.py index b8c86f85..f65dc4cb 100755 --- a/code/perception/src/coordinate_transformation.py +++ b/code/perception/src/coordinate_transformation.py @@ -11,7 +11,7 @@ from tf.transformations import euler_from_quaternion -a = 6378137 # EARTH_RADIUS_EQUA in Pylot +a = 6378137 # EARTH_RADIUS_EQUA in Pylot, used in geodetic_to_enu b = 6356752.3142 f = (a - b) / a e_sq = f * (2 - f)