diff --git a/field_friend/automations/navigation/field_navigation.py b/field_friend/automations/navigation/field_navigation.py index 2b3a4f29..335eae8a 100644 --- a/field_friend/automations/navigation/field_navigation.py +++ b/field_friend/automations/navigation/field_navigation.py @@ -137,7 +137,12 @@ async def _run_approaching_row_start(self) -> State: # TODO only drive to row if we are not on any rows and near the row start self.set_start_and_end_points() await self._wait_for_gnss() - await self._drive_to_row() + row_yaw = self.start_point.direction(self.end_point) + safe_start_point = self.start_point.polar(-self.clear_row_distance, self.start_point.direction(self.end_point)) + await self.driver.drive_to(safe_start_point, stop_at_end=False) + await self.driver.drive_to(self.start_point) + await self._wait_for_gnss() + await self.driver.drive_circle(self.start_point.polar(1.0, row_yaw), angle_threshold=np.deg2rad(1.0)) await self._wait_for_gnss() if isinstance(self.detector, rosys.vision.DetectorSimulation) and not rosys.is_test: self.create_simulation() @@ -156,7 +161,7 @@ async def _run_following_row(self, distance: float) -> State: async def _run_clear_row(self) -> State: target = self.odometer.prediction.transform_pose(rosys.geometry.Pose(x=self.clear_row_distance, y=0)) - await self._drive_towards_target(0.02, target) + await self.driver.drive_to(target) await self._wait_for_gnss() return State.ROW_COMPLETED @@ -182,20 +187,6 @@ async def _wait_for_gnss(self, waiting_time: float = 1.0) -> None: await self.gnss.update_robot_pose() self.gnss.is_paused = True - async def _drive_to_row(self) -> None: - # self.log.info(f'Driving to "{self.current_row.name}"...') - assert self.field - assert self.start_point - assert self.end_point - direction = self.start_point.direction(self.end_point) - row_start_pose = Pose(x=self.start_point.x, y=self.start_point.y, yaw=direction) - target_pose = row_start_pose.transform_pose(Pose(x=-self.clear_row_distance)) - # self.log.info(f'Driving to {end_pose} from {self.odometer.prediction}...') - spline = Spline.from_poses(self.odometer.prediction, target_pose) - # self.log.info(f'Driving spline {spline}...') - await self.driver.drive_spline(spline) - # self.log.info(f'Arrived at row {self.current_row.name} starting at {target}') - def backup(self) -> dict: return super().backup() | { 'field_id': self.field.id if self.field else None,