fix: update for overtake

code/perception/src/vision_node.py

@@ -368,6 +368,7 @@ def predict_ultralytics(self, image):
 
                     else:
                         obj_dist1 = (np.inf, np.inf, np.inf)
+                        obj_dist3 = (np.inf, np.inf, np.inf)
                         abs_distance = np.inf
                         """distance_output.append(float(cls))
                         distance_output.append(float(abs_distance))

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

@@ -15,6 +15,10 @@ def convert_to_ms(speed):
     return speed / 3.6
 
 
+# Varaible to determine if overtake is currently exec
+OVERTAKE_EXECUTING = False
+
+
 class Approach(py_trees.behaviour.Behaviour):
     """
     This behaviour is executed when the ego vehicle is in close proximity of
@@ -62,13 +66,7 @@ def initialise(self):
         rospy.loginfo("Approaching Overtake")
         self.ot_distance = 30
 
-        self.ot_option = 1  # self.blackboard.get("paf/hero/...")
-        if self.ot_option == 0:
-            self.clear_distance = 15
-            self.side = "LIDAR_range_rear_left"
-        elif self.ot_option == 1:
-            self.clear_distance = 30
-            self.side = "LIDAR_range"
+        self.clear_distance = 30
 
     def update(self):
         """
@@ -86,27 +84,28 @@ def update(self):
                  object or entered the process.
                  py_trees.common.Status.FAILURE,
         """
+        global OVERTAKE_EXECUTING
         # Update distance to collision object
         _dis = self.blackboard.get("/paf/hero/collision")
         if _dis is not None:
             self.ot_distance = _dis.data[0]
             rospy.loginfo(f"Overtake distance: {self.ot_distance}")
+            OVERTAKE_EXECUTING = self.ot_distance
 
         if np.isinf(self.ot_distance):
-            rospy.loginfo("Abort Overtake")
+            rospy.loginfo("OvertakeApproach: Abort")
             return py_trees.common.Status.FAILURE
 
         # slow down before overtake if blocked
-        if self.ot_distance < 15.0:
-            distance_lidar = self.blackboard. \
-                    get(f"/carla/hero/{self.side}")
-            if distance_lidar is not None:
-                distance_lidar = distance_lidar.data
+        if self.ot_distance < 20.0:
+            data = self.blackboard.get("/paf/hero/oncoming")
+            if data is not None:
+                distance_oncoming = data.data
             else:
-                distance_lidar = None
+                distance_oncoming = 35
 
-            if distance_lidar is not None and \
-                    distance_lidar > self.clear_distance:
+            if distance_oncoming is not None and \
+                    distance_oncoming > self.clear_distance:
                 rospy.loginfo("Overtake is free not slowing down!")
                 self.curr_behavior_pub.publish(bs.ot_app_free.name)
                 return py_trees.common.Status.SUCCESS
@@ -122,17 +121,17 @@ def update(self):
             rospy.logwarn("no speedometer connected")
             return py_trees.common.Status.RUNNING
 
-        if self.ot_distance > 15.0:
+        if self.ot_distance > 20.0:
             # too far
             rospy.loginfo("still approaching")
             return py_trees.common.Status.RUNNING
         elif speed < convert_to_ms(2.0) and \
-                self.ot_distance < 8.0:
+                self.ot_distance < 6.0:
             # stopped
             rospy.loginfo("stopped")
             return py_trees.common.Status.SUCCESS
         else:
-            rospy.logerr("Overtake Approach: Default Case")
+            # still approaching
             return py_trees.common.Status.RUNNING
 
     def terminate(self, new_status):
@@ -214,34 +213,29 @@ def update(self):
         """
 
         # Update distance to collison and distance for clear
-        self.ot_option = 1  # self.blackboard.get("paf/hero/...")
-        if self.ot_option == 0:
-            distance_lidar = self.blackboard. \
-                get("/carla/hero/LIDAR_range_rear_left")
-            clear_distance = 15
-        elif self.ot_option == 1:
-            distance_lidar = self.blackboard. \
-                get("/carla/hero/LIDAR_range")
-            clear_distance = 30
-        else:
-            distance_lidar = None
-
+        clear_distance = 30
         obstacle_msg = self.blackboard.get("/paf/hero/collision")
         if obstacle_msg is None:
             rospy.logerr("No OBSTACLE")
             return py_trees.common.Status.FAILURE
 
-        if distance_lidar is not None:
-            collision_distance = distance_lidar.data
-            if collision_distance > clear_distance:
+        data = self.blackboard.get("/paf/hero/oncoming")
+        if data is not None:
+            distance_oncoming = data.data
+        else:
+            distance_oncoming = 35
+
+        if distance_oncoming is not None:
+            if distance_oncoming > clear_distance:
                 rospy.loginfo("Overtake is free!")
                 self.curr_behavior_pub.publish(bs.ot_wait_free.name)
                 return py_trees.common.Status.SUCCESS
             else:
-                rospy.loginfo("Overtake still blocked")
+                rospy.loginfo(f"Overtake still blocked: {distance_oncoming}")
                 self.curr_behavior_pub.publish(bs.ot_wait_stopped.name)
-                return py_trees.commom.Status.RUNNING
+                return py_trees.common.Status.RUNNING
         elif obstacle_msg.data[0] == np.inf:
+            rospy.loginf("No OBSTACLE")
             return py_trees.common.Status.FAILURE
         else:
             rospy.loginfo("No Lidar Distance")
@@ -332,11 +326,11 @@ def update(self):
                 rospy.loginfo("Overtake: Slowing down")
                 return py_trees.common.Status.RUNNING
             else:
-                rospy.loginfo("OvertakeEnter: Abort ")
+                rospy.loginfo("OvertakeEnter: Abort")
                 return py_trees.common.Status.FAILURE
         else:
-            rospy.loginfo("Overtake: Bigger Failure")
-            return py_trees.common.Status.FAILURE
+            rospy.loginfo("Overtake: Waiting for status update")
+            return py_trees.common.Status.RUNNING
         # Currently not in use
         # Can be used to check if we can go back to the original lane
 
@@ -396,11 +390,11 @@ def initialise(self):
             Any initialisation you need before putting your behaviour to work.
         This prints a state status message and publishes the behavior
         """
-        rospy.loginfo("Leave Overtake")
         self.curr_behavior_pub.publish(bs.ot_leave.name)
         data = self.blackboard.get("/paf/hero/current_pos")
         self.first_pos = np.array([data.pose.position.x,
                                    data.pose.position.y])
+        rospy.loginfo(f"Leave Overtake: {self.first_pos}")
         return True
 
     def update(self):
@@ -414,11 +408,13 @@ def update(self):
         Abort this subtree
         :return: py_trees.common.Status.FAILURE, to exit this subtree
         """
+        global OVERTAKE_EXECUTING
         data = self.blackboard.get("/paf/hero/current_pos")
         self.current_pos = np.array([data.pose.position.x,
                                     data.pose.position.y])
         distance = np.linalg.norm(self.first_pos - self.current_pos)
-        if distance > 10:
+        if distance > OVERTAKE_EXECUTING:
+            rospy.loginfo(f"Left Overtake: {self.current_pos}")
             return py_trees.common.Status.FAILURE
         else:
             return py_trees.common.Status.RUNNING

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

@@ -5,8 +5,6 @@
 import numpy as np
 from scipy.spatial.transform import Rotation
 import rospy
-
-
 """
 Source: https://github.com/ll7/psaf2
 """
@@ -264,7 +262,7 @@ def update(self):
 
         if np.linalg.norm(pos_moved_in_x_direction - current_position) < 1:
             # current collision is not near trajectory lane
-            self.logerr("Obstacle is not near trajectory lane")
+            rospy.logerr("Obstacle is not near trajectory lane")
             return py_trees.common.Status.FAILURE
 
         if obstacle_speed < 2 and obstacle_distance < 30:

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

@@ -38,8 +38,6 @@ def create_node(role_name):
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/max_velocity", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
-        {'name': f"/carla/{role_name}/LIDAR_range", 'msg': Float32,
-         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/speed_limit", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/lane_change_distance", 'msg': LaneChange,
@@ -52,6 +50,8 @@ def create_node(role_name):
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/overtake_success", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
+        {'name': f"/paf/{role_name}/oncoming", 'msg': Float32,
+         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
     ]
 
     topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard")

code/planning/src/local_planner/ACC.py

@@ -173,7 +173,7 @@ def loop(timer_event=None):
                     # https://encyclopediaofmath.org/index.php?title=Linear_interpolation
                     safe_speed = self.obstacle_speed * \
                         (self.obstacle_distance / safety_distance)
-                    lerp_factor = 0.1
+                    lerp_factor = 0.2
                     safe_speed = (1 - lerp_factor) * self.__current_velocity +\
                         lerp_factor * safe_speed
                     if safe_speed < 1.0:
@@ -187,7 +187,11 @@ def loop(timer_event=None):
                     #     self.obstacle_speed = 0
                     # self.logerr("ACC: my speed: " +
                     #             str(self.__current_velocity))
-                    self.velocity_pub.publish(self.__current_velocity)
+                    if self.__current_velocity < 1.0:
+                        safe_speed = 0
+                    else:
+                        safe_speed = self.__current_velocity
+                    self.velocity_pub.publish(safe_speed)
 
             elif self.speed_limit is not None:
                 # If we have no obstacle, we want to drive with the current

code/planning/src/local_planner/collision_check.py

@@ -38,7 +38,7 @@ def __init__(self):
         self.lidar_dist = self.new_subscription(
             Float32MultiArray,
             f"/paf/{self.role_name}/Center/object_distance",
-            self.__set_distance,
+            self.__set_all_distances,
             qos_profile=1)
         # Publisher for emergency stop
         self.emergency_pub = self.new_publisher(
@@ -50,6 +50,10 @@ def __init__(self):
             Float32MultiArray,
             f"/paf/{self.role_name}/collision",
             qos_profile=1)
+        self.oncoming_pub = self.new_publisher(
+            Float32,
+            f"/paf/{self.role_name}/oncoming",
+            qos_profile=1)
         # Approx speed publisher for ACC
         self.speed_publisher = self.new_publisher(
             Float32,
@@ -59,8 +63,14 @@ def __init__(self):
         self.__current_velocity: float = None
         self.__object_first_position: tuple = None
         self.__object_last_position: tuple = None
+        self.__last_position_oncoming: tuple = None
+        self.__first_position_oncoming: tuple = None
         self.logdebug("CollisionCheck started")
 
+    def __set_all_distances(self, data: Float32MultiArray):
+        self.__set_distance(data)
+        self.__set_distance_oncoming(data)
+
     def update_distance(self, reset):
         """Updates the distance to the obstacle in front
         """
@@ -83,26 +93,54 @@ def __set_distance(self, data: Float32MultiArray):
         Args:
             data (Float32): Message from lidar with distance
         """
-        nearest_object = filter_vision_objects(data.data)
+        nearest_object = filter_vision_objects(data.data, False)
         if nearest_object is None and \
                 self.__object_last_position is not None and \
                 rospy.get_rostime() - self.__object_last_position[0] > \
-                rospy.Duration(2):
+                rospy.Duration(3):
             self.update_distance(True)
             return
         elif nearest_object is None:
             return
-        # self.logerr("Obstacle detected: " + str(nearest_object))
-        # if np.isinf(data.data) and \
-        #         self.__object_last_position is not None and \
-        #         rospy.get_rostime() - self.__object_last_position[0] < \
-        #         rospy.Duration(1):
-        #     return
-        # Set distance - 2 to clear distance from car
         self.__object_last_position = (rospy.get_rostime(), nearest_object[1])
         self.update_distance(False)
         self.calculate_obstacle_speed()
 
+    def __set_distance_oncoming(self, data: Float32MultiArray):
+        """Saves last distance from  LIDAR
+
+        Args:
+            data (Float32): Message from lidar with distance
+        """
+        nearest_object = filter_vision_objects(data.data, True)
+        if (nearest_object is None and
+                self.__last_position_oncoming is not None and
+                rospy.get_rostime() - self.__last_position_oncoming[0] >
+                rospy.Duration(3)):
+            self.update_distance_oncoming(True)
+            return
+        elif nearest_object is None:
+            return
+
+        self.__last_position_oncoming =  \
+            (rospy.get_rostime(), nearest_object[1])
+        self.update_distance_oncoming(False)
+        self.oncoming_pub.publish(Float32(data=nearest_object[1]))
+
+    def update_distance_oncoming(self, reset):
+        """Updates the distance to the obstacle in front
+        """
+        if reset:
+            # Reset all values if we do not have car in front
+            self.__last_position_oncoming = None
+            self.__first_position_oncoming = None
+            self.oncoming_pub.publish(Float32(data=np.inf))
+            return
+        if self.__first_position_oncoming is None:
+            self.__first_position_oncoming = self.__last_position_oncoming
+            self.__last_position_oncoming = None
+            return
+
     def calculate_obstacle_speed(self):
         """Caluclate the speed of the obstacle in front of the ego vehicle
             based on the distance between to timestamps
@@ -183,10 +221,11 @@ def calculate_rule_of_thumb(emergency, speed):
         Returns:
             float: distance calculated with rule of thumb
         """
-        reaction_distance = speed * 0.36
+        reaction_distance = speed * 0.5
         braking_distance = (speed * 0.36)**2
         if emergency:
-            return reaction_distance + braking_distance / 2
+            # Emergency brake is really effective in Carla
+            return reaction_distance + braking_distance / 4
         else:
             return reaction_distance + braking_distance
 
@@ -208,7 +247,7 @@ def check_crash(self, obstacle):
         if collision_time > 0:
             if distance < emergency_distance2:
                 # Initiate emergency brake
-                self.logerr("Emergency Brake")
+                self.logerr(f"Emergency Brake: {distance}")
                 self.emergency_pub.publish(True)
             # When no emergency brake is needed publish collision object
             data = Float32MultiArray(data=[distance, obstacle_speed])