fix: Overtaking adjusted

code/planning/launch/planning.launch

@@ -5,7 +5,7 @@
     </node>
     <node pkg="planning" type="ACC.py" name="ACC" output="screen">
         <param name="role_name" value="hero" />
-        <param name="control_loop_rate" value="0.1" />
+        <param name="control_loop_rate" value="0.3" />
     </node>
     <!-- <node pkg="local_planner" type="local_planner/dev_collision_publisher.py" name="DevCollisionCheck" output="screen">
         <param name="role_name" value="hero" />

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

@@ -332,7 +332,7 @@ def update(self):
                 rospy.loginfo("Overtake: Slowing down")
                 return py_trees.common.Status.RUNNING
             else:
-                rospy.loginfo("Overtake: Abort ")
+                rospy.loginfo("OvertakeEnter: Abort ")
                 return py_trees.common.Status.FAILURE
         else:
             rospy.loginfo("Overtake: Bigger Failure")

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

@@ -3,6 +3,8 @@
 
 import py_trees
 import numpy as np
+from scipy.spatial.transform import Rotation
+import rospy
 
 
 """
@@ -216,6 +218,8 @@ def initialise(self):
         What to do here?
             Any initialisation you need before putting your behaviour to work.
         """
+        # Counter for detecting overtake situation
+        self.counter_overtake = 0
         return True
 
     def update(self):
@@ -235,17 +239,40 @@ def update(self):
         """
 
         obstacle_msg = self.blackboard.get("/paf/hero/collision")
-        if obstacle_msg is None:
+        current_position = self.blackboard.get("/paf/hero/current_pos")
+        current_heading = self.blackboard.get("/paf/hero/current_heading").data
+
+        if obstacle_msg is None or \
+                current_position is None or \
+                current_heading is None:
             return py_trees.common.Status.FAILURE
+        current_position = [current_position.pose.position.x,
+                            current_position.pose.position.y,
+                            current_position.pose.position.z]
 
         obstacle_distance = obstacle_msg.data[0]
         obstacle_speed = obstacle_msg.data[1]
 
         if obstacle_distance == np.Inf:
             return py_trees.common.Status.FAILURE
+        # calculate approx collision position in global coords
+        rotation_matrix = Rotation.from_euler('z', current_heading)
+        # Apply current heading to absolute distance vector
+        # and add to current position
+        pos_moved_in_x_direction = current_position + rotation_matrix.apply(
+                np.array([obstacle_distance, 0, 0]))
+
+        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")
+            return py_trees.common.Status.FAILURE
 
         if obstacle_speed < 2 and obstacle_distance < 30:
-            return py_trees.common.Status.SUCCESS
+            self.counter_overtake += 1
+            rospy.loginfo("Overtake counter: " + str(self.counter_overtake))
+            if self.counter_overtake > 3:
+                return py_trees.common.Status.SUCCESS
+            return py_trees.common.Status.RUNNING
         else:
             return py_trees.common.Status.FAILURE
 

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

@@ -48,6 +48,8 @@ def create_node(role_name):
          'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE},
         {'name': f"/paf/{role_name}/current_pos", 'msg': PoseStamped,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
+        {'name': f"/paf/{role_name}/current_heading", 'msg': Float32,
+         'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
         {'name': f"/paf/{role_name}/overtake_success", 'msg': Float32,
          'clearing-policy': py_trees.common.ClearingPolicy.NEVER},
     ]

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.01
+                    lerp_factor = 0.1
                     safe_speed = (1 - lerp_factor) * self.__current_velocity +\
                         lerp_factor * safe_speed
                     if safe_speed < 1.0:
@@ -185,18 +185,17 @@ def loop(timer_event=None):
                     # If safety distance is reached just hold current speed
                     # if self.obstacle_speed < 1.0:
                     #     self.obstacle_speed = 0
-                    self.logerr("ACC: my speed: " +
-                                str(self.__current_velocity))
-                    self.velocity_pub.publish(self.obstacle_speed)
+                    # self.logerr("ACC: my speed: " +
+                    #             str(self.__current_velocity))
+                    self.velocity_pub.publish(self.__current_velocity)
 
             elif self.speed_limit is not None:
                 # If we have no obstacle, we want to drive with the current
                 # speed limit
-                self.logerr("ACC: Speed limit: " + str(self.speed_limit))
+                # self.logerr("ACC: Speed limit: " + str(self.speed_limit))
                 self.velocity_pub.publish(self.speed_limit)
             else:
-                self.logerr("ACC: default Speed limit")
-                self.velocity_pub.publish(5.0)
+                self.velocity_pub.publish(0)
 
         self.new_timer(self.control_loop_rate, loop)
         self.spin()

code/planning/src/local_planner/collision_check.py

@@ -128,6 +128,8 @@ def calculate_obstacle_speed(self):
             return
 
         speed = self.__current_velocity + relative_speed
+        if speed < 0:
+            speed = 0
         # Publish speed to ACC for permanent distance check
         self.speed_publisher.publish(Float32(data=speed))
         # Check for crash

code/planning/src/local_planner/motion_planning.py

@@ -269,12 +269,12 @@ def change_trajectory(self, distance_obj):
                                        30):]
             # self.trajectory = path
         else:
-            self.logerr("Overtake failed")
+            self.loginfo("Overtake failed")
             self.overtake_fallback(distance_obj, pose_list)
             self.overtake_success_pub.publish(1)
 
     def overtake_fallback(self, distance, pose_list):
-        self.logerr("Overtake Fallback!")
+        self.loginfo("Overtake Fallback!")
         # obstacle_position = approx_obstacle_pos(distance,
         #                                         self.current_heading,
         #                                         self.current_pos,
@@ -322,7 +322,7 @@ def __set_trajectory(self, data: Path):
             data (Path): Trajectory waypoints
         """
         self.trajectory = data
-        self.logerr("Trajectory received")
+        self.loginfo("Trajectory received")
         self.__corners = self.__calc_corner_points()
 
     def __calc_corner_points(self):
@@ -391,7 +391,7 @@ def map_corner(dist):
         distance_corner = 0
         for i in range(len(corner) - 1):
             distance_corner += euclid_dist(corner[i], corner[i + 1])
-        # self.logerr(distance_corner)
+        # self.loginfo(distance_corner)
 
         if self.__in_corner:
             distance_end = euclid_dist(pos, corner[0])
@@ -434,9 +434,9 @@ def __check_emergency(self, data: Bool):
         Args:
             data (Bool): True if emergency stop detected by collision check
         """
-        # self.logerr("Emergency stop detected")
+        # self.loginfo("Emergency stop detected")
         if not self.__curr_behavior == bs.parking.name:
-            # self.logerr("Emergency stop detected and executed")
+            # self.loginfo("Emergency stop detected and executed")
             self.emergency_pub.publish(data)
 
     def update_target_speed(self, acc_speed, behavior):

code/planning/src/local_planner/utils.py

@@ -193,7 +193,7 @@ def filter_vision_objects(float_array):
     all_cars = float_array[np.where(float_array[:, 0] == 2)]
 
     # Get cars that are on our lane
-    cars_in_front = all_cars[np.where(np.abs(all_cars[:, 2]) < 1.5)]
+    cars_in_front = all_cars[np.where(np.abs(all_cars[:, 2]) < 0.3)]
     if cars_in_front.size == 0:
         # no car in front
         return None