fix: wp generation for unstuck routine

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

@@ -1,6 +1,6 @@
 import py_trees
 import rospy
-from std_msgs.msg import String, Float32
+from std_msgs.msg import String, Float32, Bool
 import numpy as np
 from . import behavior_speed as bs
 # from behavior_agent.msg import BehaviorSpeed
@@ -451,6 +451,9 @@ def setup(self, timeout):
         self.pub_unstuck_distance = rospy.Publisher("/paf/hero/"
                                                     "unstuck_distance",
                                                     Float32, queue_size=1)
+        self.pub_unstuck_flag = rospy.Publisher("/paf/hero/"
+                                                "unstuck_flag",
+                                                Bool, queue_size=1)
         self.blackboard = py_trees.blackboard.Blackboard()
 
         return True
@@ -541,11 +544,15 @@ def update(self):
         if self.stuck_duration < TRIGGER_STUCK_DURATION and \
            self.wait_stuck_duration < TRIGGER_WAIT_STUCK_DURATION:
             # rospy.logfatal("No stuck detected.")
+            self.pub_unstuck_flag.publish(False)
+            # unstuck distance -1 is set, to reset the unstuck distance
+            self.pub_unstuck_distance.publish(-1)
             return py_trees.common.Status.FAILURE
 
         # stuck detected -> unstuck routine
         if rospy.Time.now()-self.init_ros_stuck_time < UNSTUCK_DRIVE_DURATION:
             self.curr_behavior_pub.publish(bs.us_unstuck.name)
+            self.pub_unstuck_flag.publish(True)
             rospy.logfatal("Unstuck routine running.")
             return py_trees.common.Status.RUNNING
         else:
@@ -554,6 +561,9 @@ def update(self):
                 self.curr_behavior_pub.publish(bs.us_stop.name)
                 return py_trees.common.Status.RUNNING
             # vehicle has stopped:
+            unstuck_distance = get_distance(self.init_pos,
+                                            self.current_pos)
+            self.pub_unstuck_distance.publish(unstuck_distance)
 
             # check if vehicle needs to overtake:
             # save current pos to last_unstuck_positions
@@ -584,10 +594,6 @@ def update(self):
 
             # once we tried the unstuck twice, we try to overtake
             if self.current_speed.speed < 1:
-                unstuck_distance = get_distance(self.init_pos,
-                                                self.current_pos)
-
-                self.pub_unstuck_distance.publish(unstuck_distance)
                 # rospy.logwarn("Unstuck DISTANCE %s.", unstuck_distance)
 
                 # publish the over take behavior 3 times to make sure

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

@@ -53,7 +53,7 @@ def create_node(role_name):
         {'name': f"/paf/{role_name}/oncoming", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/target_velocity", 'msg': Float32,
-         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
+         'clearing-policy': py_trees.common.ClearingPolicy.NEVER}
     ]
 
     topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard")

code/planning/src/local_planner/ACC.py

@@ -7,7 +7,7 @@
 from carla_msgs.msg import CarlaSpeedometer   # , CarlaWorldInfo
 from nav_msgs.msg import Path
 # from std_msgs.msg import String
-from std_msgs.msg import Float32MultiArray, Float32
+from std_msgs.msg import Float32MultiArray, Float32, Bool
 from collision_check import CollisionCheck
 import numpy as np
 
@@ -22,6 +22,18 @@ def __init__(self):
         self.role_name = self.get_param("role_name", "hero")
         self.control_loop_rate = self.get_param("control_loop_rate", 1)
 
+        # Get Unstuck flag and distance
+        self.unstuck_flag_sub: Subscriber = self.new_subscription(
+            Bool,
+            f"/paf/{self.role_name}/unstuck_flag",
+            self.__get_unstuck_flag,
+            qos_profile=1)
+        self.unstuck_distance_sub: Subscriber = self.new_subscription(
+            Float32,
+            f"/paf/{self.role_name}/unstuck_distance",
+            self.__get_unstuck_distance,
+            qos_profile=1)
+
         # Get current speed
         self.velocity_sub: Subscriber = self.new_subscription(
             CarlaSpeedometer,
@@ -67,6 +79,10 @@ def __init__(self):
             f"/paf/{self.role_name}/speed_limit",
             qos_profile=1)
 
+        # unstuck attributes
+        self.__unstuck_flag: bool = False
+        self.__unstuck_distance: float = -1
+
         # List of all speed limits, sorted by waypoint index
         self.__speed_limits_OD: [float] = []
         # Current Trajectory
@@ -99,6 +115,22 @@ def __collision_callback(self, data: Float32):
         self.obstacle_speed = data.data[1]
         self.obstacle_distance = data.data[0]
 
+    def __get_unstuck_flag(self, data: Bool):
+        """_summary_
+
+        Args:
+            data (Bool): _description_
+        """
+        self.__unstuck_flag = data.data
+
+    def __get_unstuck_distance(self, data: Float32):
+        """_summary_
+
+        Args:
+            data (Float32): _description_
+        """
+        self.__unstuck_distance = data.data
+
     def __get_current_velocity(self, data: CarlaSpeedometer):
         """_summary_
 
@@ -148,6 +180,17 @@ def __current_position_callback(self, data: PoseStamped):
             self.speed_limit = \
                 self.__speed_limits_OD[self.__current_wp_index]
             self.speed_limit_publisher.publish(self.speed_limit)
+        # in case we used the unstuck routine to drive backwards
+        # we have to follow WPs that are already passed
+        elif self.__unstuck_flag:
+            if self.__unstuck_distance is None\
+               or self.__unstuck_distance == -1:
+                return
+            self.__current_wp_index -= int(self.__unstuck_distance)
+            self.wp_publisher.publish(self.__current_wp_index)
+            self.speed_limit = \
+                self.__speed_limits_OD[self.__current_wp_index]
+            self.speed_limit_publisher.publish(self.speed_limit)
 
     def run(self):
         """

code/planning/src/local_planner/motion_planning.py

@@ -231,7 +231,7 @@ def overtake_fallback(self, distance, pose_list, unstuck=False):
         #                                         self.current_speed)
         currentwp = self.current_wp
         normal_x_offset = 2
-        unstuck_x_offset = 4  # could need adjustment with better steering
+        unstuck_x_offset = 3.5  # could need adjustment with better steering
 
         # if overtake is called by the unstuck routine
         # -> reset the current wp to the distance driven backwards
@@ -242,7 +242,8 @@ def overtake_fallback(self, distance, pose_list, unstuck=False):
             if distance > currentwp:
                 currentwp = 0
             else:
-                currentwp = currentwp - int(distance + 1)
+                distance = distance + 2
+                # currentwp = currentwp - int(distance)
 
         # else: overtake starts from current wp
         selection = pose_list[int(currentwp):int(currentwp) +