Merge pull request #131 from una-auxme/78-feature-filtering-of-sensor…

…data

78 feature filtering of sensordata

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)" />
@@ -56,4 +78,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

@@ -175,10 +175,14 @@ def update_gps_data(self, data: NavSatFix):
         """
         # Make sure position is only published when reference values have been
         # read from the Map
-        if CoordinateTransformer.ref_set:
+        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)

code/perception/src/coordinate_transformation.py

@@ -15,15 +15,19 @@
 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):
@@ -58,7 +62,9 @@ def geodetic_to_enu(lat, lon, alt):
 
     # Is not necessary in new version
     # y *= -1
-    return x, y, alt + 331.00000
+    # 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):