diff --git a/rosys/driving/driver.py b/rosys/driving/driver.py index 9220110df..362984738 100644 --- a/rosys/driving/driver.py +++ b/rosys/driving/driver.py @@ -50,7 +50,11 @@ def __init__(self, wheels: Drivable, odometer: Odometer) -> None: self.odometer = odometer self.parameters = DriveParameters() self.state: Optional[DriveState] = None - self.abort_driving = False + self._abort = False + + def abort(self) -> None: + """Abort the current drive routine.""" + self._abort = True @analysis.track async def drive_square(self) -> None: @@ -61,6 +65,9 @@ async def drive_square(self) -> None: @analysis.track async def drive_arc(self) -> None: while self.odometer.prediction.x < 2: + if self._abort: + self._abort = False + raise DrivingAbortedException() await self.wheels.drive(1, np.deg2rad(25)) await rosys.sleep(0.1) await self.wheels.stop() @@ -87,6 +94,9 @@ async def drive_to(self, target: Point, backward: bool = False) -> None: @analysis.track async def drive_circle(self, target: Point, backward: bool = False) -> None: while True: + if self._abort: + self._abort = False + raise DrivingAbortedException() target_yaw = self.odometer.prediction.direction(target) if backward: target_yaw += np.pi @@ -111,8 +121,8 @@ async def drive_spline(self, spline: Spline, *, flip_hook: bool = False, throttl carrot = Carrot(spline=spline, offset=carrot_offset) while True: - if self.abort_driving: - self.abort_driving = False + if self._abort: + self._abort = False raise DrivingAbortedException() dYaw = self.parameters.hook_bending_factor * self.odometer.current_velocity.angular if self.odometer.current_velocity else 0 hook = self.odometer.prediction.transform_pose(Pose(yaw=dYaw)).transform(hook_offset)