Merge remote-tracking branch 'origin/main' into 101-bug-segmentation-…

…mask

code/acting/launch/acting.launch

@@ -28,12 +28,12 @@
         <param name="control_loop_rate" value="$(arg control_loop_rate)" />
         <param name="role_name" value="$(arg role_name)" />
     </node>
-    <!--
+
     <node pkg="acting" type="DummyTrajectoryPublisher.py" name="DummyTrajectoryPublisher" output="screen">
         <param name="control_loop_rate" value="1" />
         <param name="role_name" value="$(arg role_name)" />
     </node>
-    -->
+
 
     <node pkg="acting" type="MainFramePublisher.py" name="MainFramePublisher" output="screen">
         <param name="control_loop_rate" value="0.05" />
@@ -45,13 +45,16 @@
         <param name="role_name" value="$(arg role_name)" />
     </node-->
 
-   <node pkg="acting" type="acc_distance_publisher_dummy.py" name="AccDistancePublisherDummy" output="screen">
+   <!--node pkg="acting" type="acc_distance_publisher_dummy.py" name="AccDistancePublisherDummy" output="screen">
         <param name="role_name" value="$(arg role_name)" />
-        <param name="enabled" value="False" /> <!-- set to True to publish dummy velocities for testing-->
-   </node>
+        <param name="enabled" value="True" /> <!set to True to publish dummy velocities for testing>
+   </node-->
 
-  <node pkg="acting" type="acc.py" name="Acc" output="screen">
+    <node pkg="acting" type="acc.py" name="Acc" output="screen">
         <param name="control_loop_rate" value="$(arg control_loop_rate)" />
         <param name="role_name" value="$(arg role_name)" />
-  </node>
+    </node>
+
+    <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot" args="/carla/hero/Speed /paf/hero/stanley_debug/cross_err" -->
+
 </launch>

code/agent/config/dev_objects.json

@@ -179,6 +179,55 @@
         {
           "type": "actor.pseudo.control",
           "id": "control"
+        },
+        {
+          "type": "sensor.other.gnss",
+          "id": "Ideal_GPS",
+          "spawn_point": {
+            "x": 0.0,
+            "y": 0.0,
+            "z": 1.60
+          },
+          "noise_alt_stddev": 0.0,
+          "noise_lat_stddev": 0.0,
+          "noise_lon_stddev": 0.0,
+          "noise_alt_bias": 0.0,
+          "noise_lat_bias": 0.0,
+          "noise_lon_bias": 0.0
+        },
+        {
+          "type": "sensor.other.imu",
+          "id": "Ideal_IMU",
+          "spawn_point": {
+            "x": 0.7,
+            "y": 0.4,
+            "z": 1.60,
+            "roll": 0.0,
+            "pitch": 0.0,
+            "yaw": 0.0
+          },
+          "noise_accel_stddev_x": 0.0,
+          "noise_accel_stddev_y": 0.0,
+          "noise_accel_stddev_z": 0.0,
+          "noise_gyro_stddev_x": 0.0,
+          "noise_gyro_stddev_y": 0.0,
+          "noise_gyro_stddev_z": 0.0
+        },
+        {
+          "type": "sensor.other.radar",
+          "id": "Ideal_RADAR",
+          "spawn_point": {
+            "x": 0.7,
+            "y": 0.4,
+            "z": 1.60,
+            "roll": 0.0,
+            "pitch": 0.0,
+            "yaw": 45.0
+          },
+          "horizontal_fov": 30.0,
+          "vertical_fov": 30.0,
+          "points_per_second": 1500,
+          "range": 100.0
         }
       ]
     }

code/perception/launch/perception.launch

@@ -2,11 +2,33 @@
 <launch>
   <arg name="role_name" default="hero" />
   <arg name="control_loop_rate" default="0.1" />
+
+  <!--just for testing purposes -> Uncomment to use and see plots
+  <node pkg="perception" type="sensor_filter_debug.py" name="sensor_filter_debug" output="screen">
+    <param name="control_loop_rate" value="0.001" />
+    <param name="role_name" value="$(arg role_name)" />
+  </node>
+  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="rqt_plot" args="/paf/hero/location_error/data[0] /paf/hero/location_error/data[1]" />
+  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="x_plot" args="/paf/hero/ideal_x/data[0] /paf/hero/ideal_x/data[1]" />
+  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="y_plot" args="/paf/hero/ideal_y/data[0] /paf/hero/ideal_y/data[1]" /-->
+  <!--node pkg="rqt_plot" type="rqt_plot" output="screen" name="error_plot" args="/paf/hero/ideal_x/data[2]" />
+  -->
+
+  <!-- does not publish data yet. Uncomment to use and see plots
+  <node pkg="perception" type="kalman_filter.py" name="kalman_filter_node" output="screen">
+    <param name="control_loop_rate" value="0.1" />
+    <param name="role_name" value="$(arg role_name)" />
+  </node>
+  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="x_plot" args="/paf/hero/current_pos/data[0] /paf/hero/kalman_pos/data[0]" />
+  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="y_plot" args="/paf/hero/current_pos/data[1] /paf/hero/kalman_pos/data[1]" />
+  <node pkg="rqt_plot" type="rqt_plot" output="screen" name="heading_plot" args="/paf/hero/current_heading/data /paf/hero/kalman_heading/data" />
+  -->
 
   <node pkg="perception" type="Position_Publisher_Node.py" name="Position_Publisher_Node" output="screen">
     <param name="control_loop_rate" value="0.1" />
     <param name="role_name" value="$(arg role_name)" />
   </node>
+
 
   <node pkg="perception" type="vision_node.py" name="VisionNode" output="screen">
     <param name="role_name" value="$(arg role_name)" />
@@ -71,4 +93,5 @@
     <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered_rear_left"/>
     <param name="range_topic" value="/carla/hero/LIDAR_range_rear_left"/>
   </node>
+
 </launch>

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,38 @@ 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 is False:
+            self.transformer = CoordinateTransformer()
+            CoordinateTransformer.ref_set = True
+        if CoordinateTransformer.ref_set is True:
+            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,54 +8,63 @@
 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)
+alt_offset = 331.00000
+STD_LAT = 0.0
+STD_LON = 0.0
+STD_H = 0.0
 
 
 class CoordinateTransformer:
     """Object that is used to transform Coordinates between
     xyz and gnss reference frame"""
 
-    la_ref: float
-    ln_ref: float
-    h_ref: float
+    la_ref = STD_LAT
+    ln_ref = STD_LON
+    h_ref = STD_H
     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
+    # alt_offset is needed to keep the hight of the map in mind
+    # right now we don't really use the altitude anyways
+    return x, y, alt + alt_offset
 
 
 def geodetic_to_ecef(lat, lon, h):