From 6571cf1796f478d318e624b3a0fcfacb9fcc2066 Mon Sep 17 00:00:00 2001 From: HeadTriXz Date: Mon, 3 Jun 2024 19:23:42 +0200 Subject: [PATCH] fix: add time.sleep to the overtake handler --- configs/config.defaults.yaml | 4 ++++ src/driving/modes/autonomous.py | 4 ++++ .../handlers/overtake_handler.py | 18 ++++++++++++++++++ src/object_recognition/object_controller.py | 7 +++++++ 4 files changed, 33 insertions(+) diff --git a/configs/config.defaults.yaml b/configs/config.defaults.yaml index 04603dc..2a5a9ab 100644 --- a/configs/config.defaults.yaml +++ b/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 diff --git a/src/driving/modes/autonomous.py b/src/driving/modes/autonomous.py index f58d41c..41e5b2d 100644 --- a/src/driving/modes/autonomous.py +++ b/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"] ) diff --git a/src/object_recognition/handlers/overtake_handler.py b/src/object_recognition/handlers/overtake_handler.py index c0bcded..39881dd 100644 --- a/src/object_recognition/handlers/overtake_handler.py +++ b/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) diff --git a/src/object_recognition/object_controller.py b/src/object_recognition/object_controller.py index c0cc446..f5f471f 100644 --- a/src/object_recognition/object_controller.py +++ b/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)