fix: last fixes before merge

build/docker-compose.yml

@@ -21,7 +21,7 @@ services:
 
       # based on https://github.com/ll7/paf21-1/blob/master/scenarios/docker-carla-sim-compose.yml
   carla-simulator:
-    command: /bin/bash CarlaUE4.sh -quality-level=Medium -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini"
+    command: /bin/bash CarlaUE4.sh -quality-level=High -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini"
     image: ghcr.io/una-auxme/paf23:leaderboard-2.0
     init: true
     deploy:

code/acting/src/acting/vehicle_controller.py

@@ -143,6 +143,7 @@ def loop(timer_event=None) -> None:
             if self.__velocity > 5:
                 steer = self._s_steer
             else:
+                # while doing the unstuck routine we don't want to steer
                 if self.__curr_behavior == "us_unstuck" or \
                    self.__curr_behavior == "us_stop":
                     steer = 0

code/acting/src/acting/velocity_controller.py

@@ -110,27 +110,16 @@ def loop(timer_event=None):
             if self.__target_velocity < 0:
                 # self.logerr("VelocityController doesn't support backward "
                 #             "driving yet.")
-                # TODO ROBERT implement backward driving
                 if self.__target_velocity == -3:
                     #  -3 is the signal for reverse driving
-                    # brake = 0
+
                     reverse = True
-                    # throttle = 1
-                    # TODO code wiederholung -> cleaner!
-                    # v = self.__target_velocity
-                    # pid_t.setpoint = v
-                    # throttle = pid_t(self.__current_velocity)
-                    # if throttle < 0:
-                    #     brake = abs(throttle)
-                    #     throttle = 0
-                    # else:
-                    #     brake = 0
                     throttle = 1
                     brake = 0
                     rospy.loginfo("VelocityController: reverse driving")
 
                 else:
-                    #  other negative values lead to braking
+                    #  other negative values only lead to braking
                     reverse = False
                     brake = 1
                     throttle = 0

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

@@ -394,14 +394,13 @@ def pos_to_np_array(pos):
         return None
 
 
-TRIGGER_STUCK_COUNT = 30  # default 40 (about 5 s)
-TRIGGER_WAIT_STUCK_COUNT = 100  # default 100 (about 20 s)
+TRIGGER_STUCK_COUNT = 30  # default 30 (about 5 s -> behavior tree 5 Hz)
+TRIGGER_WAIT_STUCK_COUNT = 100  # default 100 (about 20 s-> behavior tree 5 Hz)
 UNSTUCK_DRIVE_DURATION = 1.2  # default 1.2 (s)
 
 
 class UnstuckRoutine(py_trees.behaviour.Behaviour):
 
-    # TODO: Implement this behavior ROBERT
     """
     This behavior is triggered when the vehicle is stuck and needs to be
     unstuck. The behavior will then try to reverse and steer to the left or
@@ -441,11 +440,6 @@ def setup(self, timeout):
         self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
                                                  "curr_behavior",
                                                  String, queue_size=1)
-        # self.stuck_count_pub = rospy.Publisher("/paf/hero/stuck_count",
-        #                                        Int32, queue_size=1)
-        # self.wait_stuck_count_pub = rospy.Publisher("/paf/hero/"
-        #                                             "wait_stuck_count",
-        #                                             Int32, queue_size=1)
         self.pub_unstuck_distance = rospy.Publisher("/paf/hero/"
                                                     "unstuck_distance",
                                                     Float32, queue_size=1)
@@ -467,19 +461,9 @@ def initialise(self):
 
         self.current_speed = self.blackboard.get("/carla/hero/Speed")
         target_speed = self.blackboard.get("/paf/hero/target_velocity")
-        # self.stuck_count = self.blackboard.get("/paf/hero/stuck_count")
-        # self.wait_stuck_count = self.blackboard.get("/paf/hero/"
-        #                                             "wait_stuck_count")
 
         # check for None values and initialize if necessary
         if self.current_speed is None or target_speed is None:
-            # self.stuck_count_pub.publish(0)
-            # self.stuck_count = self.blackboard.get("/paf/hero/stuck_count")
-            # rospy.loginfo("stuck_count initialized to %s", 0)
-            # self.wait_stuck_count_pub.publish(0)
-            # self.wait_stuck_count = self.blackboard.get("/paf/hero/"
-            #                                             "wait_stuck_count")
-            # rospy.loginfo("wait_stuck_count initialized to %s", 0)
             if self.current_speed is None:
                 rospy.logdebug("current_speed is None")
             elif target_speed is None:
@@ -488,32 +472,28 @@ def initialise(self):
 
         # check if vehicle is stuck, v = 0 when told to v > 0
         if self.current_speed.speed < 1 and target_speed.data >= 1:
-            rospy.loginfo("stuck_count increased by 1, now: %s",
-                          self.stuck_count + 1)
-            # self.stuck_count_pub.publish(self.stuck_count.data + 1)
+            # rospy.loginfo("stuck_count increased by 1, now: %s",
+            #               self.stuck_count + 1)
             self.stuck_count += 1
         else:
-            # self.stuck_count_pub.publish(0)
             self.stuck_count = 0
         # check if vehicle is stuck in wait loop
         # for example when waiting for a traffic light seems to take too long
         # -> something might have went wrong
         if self.current_speed.speed < 1.0:
-            rospy.loginfo("wait_stuck_count increased by 1, now: %s",
-                          self.wait_stuck_count + 1)
-            # self.wait_stuck_count_pub.publish(self.wait_stuck_count.data + 1)
+            # rospy.loginfo("wait_stuck_count increased by 1, now: %s",
+            #               self.wait_stuck_count + 1)
             self.wait_stuck_count += 1
         else:
-            # self.wait_stuck_count_pub.publish(0)
             self.wait_stuck_count = 0
 
         # inform about which kind of stuck situation is detected
         if self.stuck_count >= TRIGGER_STUCK_COUNT:
-            rospy.logwarn("Stuck detected -> starting unstuck routine")
+            rospy.logfatal("Stuck detected -> starting unstuck routine")
             self.init_pos = pos_to_np_array(
                 self.blackboard.get("/paf/hero/current_pos"))
         elif self.wait_stuck_count >= TRIGGER_WAIT_STUCK_COUNT:
-            rospy.logwarn("Wait stuck detected -> starting unstuck routine")
+            rospy.logfatal("Wait stuck detected -> starting unstuck routine")
             self.init_pos = pos_to_np_array(
                 self.blackboard.get("/paf/hero/current_pos"))
 
@@ -591,14 +571,13 @@ def update(self):
             if self.current_speed.speed < 1:
                 unstuck_distance = get_distance(self.init_pos,
                                                 self.current_pos)
-                # if distance_to_waypoint < 0.5:
 
                 self.pub_unstuck_distance.publish(unstuck_distance)
-                rospy.logwarn("Unstuck DISTANCE %s.", unstuck_distance)
-                self.curr_behavior_pub.publish(bs.us_overtake.name)
+                # rospy.logwarn("Unstuck DISTANCE %s.", unstuck_distance)
 
-                # self.stuck_count_pub.publish(0)
-                # self.wait_stuck_count_pub.publish(0)
+                # publish the over take behavior 3 times to make sure
+                # it is detected
+                self.curr_behavior_pub.publish(bs.us_overtake.name)
                 if self.unstuck_overtake_count > 3:
                     self.stuck_count = 0
                     self.wait_stuck_count = 0

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

@@ -4,7 +4,7 @@
 import py_trees
 import py_trees_ros
 
-from std_msgs.msg import Float32, Bool, Float32MultiArray, Int32
+from std_msgs.msg import Float32, Bool, Float32MultiArray
 from carla_msgs.msg import CarlaSpeedometer
 from geometry_msgs.msg import PoseStamped
 
@@ -52,12 +52,8 @@ def create_node(role_name):
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/oncoming", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
-        {'name': f"/paf/{role_name}/stuck_count", 'msg': Int32,
-         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/target_velocity", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
-        {'name': f"/paf/{role_name}/wait_stuck_count", 'msg': Int32,
-         'clearing-policy': py_trees.common.ClearingPolicy.NEVER}
     ]
 
     topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard")

code/planning/src/local_planner/motion_planning.py

@@ -463,14 +463,17 @@ def __get_speed_unstuck(self, behavior: str) -> float:
                and self.current_pos is not None:
                 distance = np.linalg.norm(
                     self.init_overtake_pos[:2] - self.current_pos[:2])
-                self.logfatal(f"Unstuck Distance in mp: {distance}")
+                # self.logfatal(f"Unstuck Distance in mp: {distance}")
+                # clear distance to last unstuck -> avoid spamming overtake
                 if distance > UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE:
                     self.unstuck_overtake_flag = False
                     self.logfatal("Unstuck Overtake Flag Cleared")
 
             # to avoid spamming the overtake_fallback
             if self.unstuck_overtake_flag is False:
-                # create overtake trajectory 6 meteres before the obstacle
+                # create overtake trajectory starting 6 meteres before
+                # the obstacle
+                # 6 worked well in tests, but can be adjusted
                 self.overtake_fallback(self.unstuck_distance + 6, pose_list)
                 self.logfatal("Overtake fallback while unstuck!")
                 self.unstuck_overtake_flag = True