fix: add time.sleep to the overtake handler

configs/config.defaults.yaml

@@ -108,6 +108,10 @@ traffic_light:
 
 overtake:
   consecutive_frames: 3
+  force_return:
+    enabled: true
+    angle: 1.0  # percentage
+    duration: 0.5  # seconds
   min_distance: 6
   min_angle: 230
   max_angle: 280

src/driving/modes/autonomous.py

@@ -110,11 +110,15 @@ def __init_object_detection(self, calibration: CalibrationData) -> None:
         object_controller.add_handler(SpeedLimitHandler(object_controller))
         object_controller.add_handler(TrafficLightHandler(object_controller))
 
+        # Initialize Lidar-specific handlers.
         lidar = Lidar.safe_init()
         if lidar is not None:
             object_controller.add_handler(OvertakeHandler(object_controller, lidar))
             object_controller.add_handler(ParkingHandler(object_controller, lidar))
 
+            lidar.start()
+
+        # Initialize the object detector.
         self.detector = ObjectDetector.from_model(
             config["object_detection"]["model_path"], object_controller, config["camera_ids"]["center"]
         )

src/object_recognition/handlers/overtake_handler.py

@@ -1,4 +1,5 @@
 import logging
+import time
 
 from threading import Thread
 from ultralytics.engine.results import Boxes
@@ -68,6 +69,7 @@ def __return_lane(self) -> None:
         while True:
             current_lane = self.controller.get_current_lane()
             if current_lane == 0:
+                time.sleep(0.1)
                 continue
 
             if current_lane not in self.__frames_seen:
@@ -80,17 +82,33 @@ def __return_lane(self) -> None:
                 config["overtake"]["range_threshold"]
             )
 
+            # Wait until we have seen the car for the first time
             if not is_side_free:
                 self.__frames_seen[current_lane] += 1
 
+            # If we have previously seen the car, we can start checking if we have passed it
             if self.__frames_seen[current_lane] >= config["overtake"]["consecutive_frames"]:
                 if is_side_free:
                     self.__frames_lost[current_lane] += 1
                 else:
                     self.__frames_lost[current_lane] -= 1
 
+            # If the side is free for a certain amount of frames, return to the previous lane
             if self.__frames_lost[current_lane] >= config["overtake"]["consecutive_frames"]:
                 logging.info("The right side is free. Returning to the previous lane.")
+
                 self.controller.set_lane(current_lane - 1)
                 del self.__frames_lost[current_lane]
                 del self.__frames_seen[current_lane]
+
+                if config["overtake"]["force_return"]["enabled"]:
+                    # Force the go-kart to return to the previous lane
+                    self.controller.set_steering(config["overtake"]["force_return"]["angle"])
+
+                    # Wait for the specified duration
+                    time.sleep(config["overtake"]["force_return"]["duration"])
+
+                    # Reset the steering angle to 0.0
+                    self.controller.set_steering(0.0)
+
+            time.sleep(0.1)

src/object_recognition/object_controller.py

@@ -151,3 +151,10 @@ def set_state(self, state: SpeedControllerState) -> None:
         :param state: The new state.
         """
         self.speed_controller.state = state
+
+    def set_steering(self, angle: float) -> None:
+        """Set the steering of the go-kart.
+
+        :param angle: The angle to set.
+        """
+        self.speed_controller.can_controller.set_steering(angle)