Merge pull request #227 from una-auxme/226-bug-unstuck-behavior-even-…

…when-not-fully-stuck

fix: seems to fix the issue

code/perception/launch/perception.launch

@@ -3,8 +3,8 @@
   <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">
+  <!-- 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> -->

code/perception/src/00_Experiments/kalman_datasets/viz.py

@@ -309,7 +309,7 @@ def get_x_or_y_from_csv_file(file_name, type='x'):
         file_path = folder_path_y + file_name
 
     # Read the CSV file into a DataFrame
-    df = pd.read_csv(file_path)
+    df = pd.read_csv(file_path, skiprows=8)
 
     # rename the columns
     df.columns = ['time', 'ideal_x', 'kalman_x', 'running_avg_x',
@@ -870,11 +870,11 @@ def calculate_RMSPE(ideal, estimated):
 # # plot_RMSPE_notched_box("data_26.csv")
 # plot_CEP("data_25.csv")
 # plot_CEP("data_26.csv")
-data = "data_26.csv"
+data = "data_50.csv"
 plot_CEP(data)
 plot_x_or_y_notched_box(data, type='x', error_type='MSE')
 plot_x_or_y_notched_box(data, type='x', error_type='MAE')
-plot_x_or_y_notched_box(data, type='y', error_type='MSE')
-plot_x_or_y_notched_box(data, type='y', error_type='MAE')
+# plot_x_or_y_notched_box(data, type='y', error_type='MSE')
+# plot_x_or_y_notched_box(data, type='y', error_type='MAE')
 
 plt.show()

code/perception/src/kalman_filter.py

@@ -14,15 +14,22 @@
 from coordinate_transformation import quat_to_heading
 from xml.etree import ElementTree as eTree
 
-GPS_RUNNING_AVG_ARGS: int = 10
+GPS_RUNNING_AVG_ARGS = 10
 
 '''
-This class implements a Kalman filter for a 3D object tracked in 3D space.
+For more information see the documentation in:
+../../doc/06_perception/08_kalman_filter.md
+
+This class implements a Kalman filter for a 3D object tracked in 2D space.
 It implements the data of the IMU and the GPS Sensors.
 The IMU Sensor provides the acceleration
 and the GPS Sensor provides the position.
 The Carla Speedometer provides the current Speed in the headed direction.
 
+The Z Coordinate (Latitude) is calculated by a simple Running Average of
+the last 10 GPS-z Measurements and is not in any way related
+to the Kalman Filter.
+
 The testing Noise for the GPS Sensor is defined as:
     "noise_alt_stddev": 0.000005,
     "noise_lat_stddev": 0.000005,
@@ -56,7 +63,7 @@
 The process covariance matrix Q is defined as:
     self.Q = np.diag([0.0001, 0.0001, 0.00001, 0.00001, 0.000001, 0.00001])
 The measurement covariance matrix R is defined as:
-    self.R = np.diag([0.0005, 0.0005, 0, 0, 0, 0])
+    self.R = np.diag([0.0007, 0.0007, 0, 0, 0, 0])
 '''
 
 
@@ -148,7 +155,7 @@ def __init__(self):
         # The process covariance matrix Q is defined as:
         self.Q = np.diag([0.0001, 0.0001, 0.00001, 0.00001, 0.000001, 0.00001])
         # The measurement covariance matrix R is defined as:
-        self.R = np.diag([0.0005, 0.0005, 0, 0, 0, 0])
+        self.R = np.diag([0.0007, 0.0007, 0, 0, 0, 0])
 
         # self.x_old_est = np.copy(self.x0)  # old state vector
         # self.P_old_est = np.copy(self.P0)  # old state covariance matrix
@@ -286,6 +293,7 @@ def publish_kalman_location(self):
         """
         Publish the kalman location
         """
+
         # Initialize the kalman-position
         kalman_position = PoseStamped()
 
@@ -300,6 +308,7 @@ def publish_kalman_location(self):
         kalman_position.pose.position.y = self.x_est[1, 0]
 
         kalman_position.pose.position.z = self.latitude
+
         kalman_position.pose.orientation.x = 0
         kalman_position.pose.orientation.y = 0
         kalman_position.pose.orientation.z = 1

code/planning/src/behavior_agent/behaviours/maneuvers.py

@@ -394,8 +394,9 @@ def pos_to_np_array(pos):
         return None
 
 
-TRIGGER_STUCK_DURATION = rospy.Duration(5)  # default 5 (s)
-TRIGGER_WAIT_STUCK_DURATION = rospy.Duration(20)  # default 20 (s)
+TRIGGER_STUCK_SPEED = 0.1  # default 0.1 (m/s)
+TRIGGER_STUCK_DURATION = rospy.Duration(20)  # default 8 (s)
+TRIGGER_WAIT_STUCK_DURATION = rospy.Duration(50)  # default 25 (s)
 UNSTUCK_DRIVE_DURATION = rospy.Duration(1.2)  # default 1.2 (s)
 UNSTUCK_CLEAR_DISTANCE = 1.5  # default 1.5 (m)
 
@@ -412,6 +413,23 @@ def reset_stuck_values(self):
         self.stuck_timer = rospy.Time.now()
         self.wait_stuck_timer = rospy.Time.now()
 
+    def print_warnings(self):
+        # update last log values
+        self.last_stuck_duration_log = self.stuck_duration
+        self.last_wait_stuck_duration_log = self.wait_stuck_duration
+
+        stuck_duration_diff = (self.stuck_duration -
+                               self.last_stuck_duration_log)
+        wait_stuck_duration_diff = (self.wait_stuck_duration -
+                                    self.last_wait_stuck_duration_log)
+
+        if self.stuck_duration.secs > TRIGGER_STUCK_DURATION.secs/2 \
+           and stuck_duration_diff.secs >= 1:
+            rospy.logwarn(f"Stuck for {self.stuck_duration.secs} s")
+        if self.wait_stuck_duration.secs > TRIGGER_WAIT_STUCK_DURATION.secs/2\
+           and wait_stuck_duration_diff.secs >= 1:
+            rospy.logwarn(f"Wait stuck for {self.wait_stuck_duration.secs} s")
+
     def __init__(self, name):
         """
         Minimal one-time initialisation. A good rule of thumb is to only
@@ -433,6 +451,9 @@ def __init__(self, name):
         dummy_pos = np.array([0, 0])
         self.last_unstuck_positions = np.array([dummy_pos, dummy_pos])
 
+        self.last_wait_stuck_duration_log = rospy.Duration(0)
+        self.last_stuck_duration_log = rospy.Duration(0)
+
     def setup(self, timeout):
         """
         Delayed one-time initialisation that would otherwise interfere with
@@ -484,19 +505,24 @@ def initialise(self):
             self.reset_stuck_values()
             return True
 
-        # check if vehicle is not stuck, v > 1
-        if self.current_speed.speed >= 1.0:
+        # check if vehicle is NOT stuck, v > 0.1
+        if self.current_speed.speed >= TRIGGER_STUCK_SPEED:
             # reset wait stuck timer
             self.wait_stuck_timer = rospy.Time.now()
-            # check if vehicle is not stuck, v > 1 when told to v > 0
-            if target_speed.data >= 1.0:
+
+            # check if vehicle is NOT stuck, v >= 0.1 when should be v > 0.1
+            if target_speed.data >= TRIGGER_STUCK_SPEED:
                 # reset stuck timer
                 self.stuck_timer = rospy.Time.now()
 
         # update the stuck durations
         self.stuck_duration = rospy.Time.now() - self.stuck_timer
         self.wait_stuck_duration = rospy.Time.now() - self.wait_stuck_timer
 
+        # print warnings to indicate potential stuck
+        self.print_warnings()
+
+        # print fatal error if stuck for too long
         if self.stuck_duration >= TRIGGER_STUCK_DURATION:
             rospy.logfatal(f"""Should be Driving but Stuck in one place
                            for more than {TRIGGER_STUCK_DURATION.secs}\n

code/planning/src/local_planner/motion_planning.py

@@ -239,9 +239,14 @@ def overtake_fallback(self, distance, pose_list, unstuck=False):
         #                                         self.current_speed)
         currentwp = self.current_wp
         normal_x_offset = 2
-        unstuck_x_offset = 3.5  # could need adjustment with better steering
-        selection = pose_list[int(currentwp):int(currentwp) +
-                              int(distance) + NUM_WAYPOINTS]
+        unstuck_x_offset = 3  # could need adjustment with better steering
+        if unstuck:
+            selection = pose_list[int(currentwp)-2:int(currentwp) +
+                                  int(distance)+2 + NUM_WAYPOINTS]
+        else:
+            selection = pose_list[int(currentwp) + int(distance/2):
+                                  int(currentwp) +
+                                  int(distance) + NUM_WAYPOINTS]
         waypoints = self.convert_pose_to_array(selection)
 
         if unstuck is True:
@@ -275,8 +280,13 @@ def overtake_fallback(self, distance, pose_list, unstuck=False):
         path = Path()
         path.header.stamp = rospy.Time.now()
         path.header.frame_id = "global"
-        path.poses = pose_list[:int(currentwp)] + \
-            result + pose_list[int(currentwp + distance + NUM_WAYPOINTS):]
+        if unstuck:
+            path.poses = pose_list[:int(currentwp)-2] + \
+                result + pose_list[int(currentwp) +
+                                   int(distance) + 2 + NUM_WAYPOINTS:]
+        else:
+            path.poses = pose_list[:int(currentwp) + int(distance/2)] + \
+                result + pose_list[int(currentwp + distance + NUM_WAYPOINTS):]
 
         self.trajectory = path
 

code/planning/src/local_planner/utils.py

@@ -78,11 +78,11 @@ def calculate_rule_of_thumb(emergency, speed):
     Returns:
         float: distance calculated with rule of thumb
     """
-    reaction_distance = speed / 2
+    reaction_distance = speed
     braking_distance = (speed * 0.36)**2
     if emergency:
         # Emergency brake is really effective in Carla
-        return reaction_distance + braking_distance
+        return reaction_distance + braking_distance / 2
     else:
         return reaction_distance + braking_distance