From b2060a13e525404a31432c8bb7176cb6455217d9 Mon Sep 17 00:00:00 2001 From: SeaTechRC Date: Tue, 10 Sep 2024 15:49:24 +0200 Subject: [PATCH] Added function to wait for an robot pose update if ensure_gnss is true --- field_friend/automations/navigation/navigation.py | 3 +++ field_friend/localization/gnss.py | 8 ++++++++ 2 files changed, 11 insertions(+) diff --git a/field_friend/automations/navigation/navigation.py b/field_friend/automations/navigation/navigation.py index 65ac0c4a..246d585d 100644 --- a/field_friend/automations/navigation/navigation.py +++ b/field_friend/automations/navigation/navigation.py @@ -75,6 +75,9 @@ async def prepare(self) -> bool: if isinstance(self.detector, rosys.vision.DetectorSimulation) and not rosys.is_test: self.detector.simulated_objects = [] self.log.info('clearing plant provider') + + self.log.info('waiting for GNSS pose upsate') + await self.gnss.wait_for_updated_robot_pose() return True async def finish(self) -> None: diff --git a/field_friend/localization/gnss.py b/field_friend/localization/gnss.py index f27ae12d..50986469 100644 --- a/field_friend/localization/gnss.py +++ b/field_friend/localization/gnss.py @@ -1,5 +1,6 @@ from __future__ import annotations +import asyncio import contextlib import logging from abc import ABC, abstractmethod @@ -52,6 +53,7 @@ def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> N self.device: str | None = None self.antenna_offset = antenna_offset self._is_paused = False + self._pose_update = asyncio.Event() self.observed_poses: list[rosys.geometry.Pose] = [] self.last_pose_update = rosys.time() self.needed_poses: int = self.NEEDED_POSES @@ -131,12 +133,18 @@ def _on_rtk_fix(self) -> None: if not self._is_paused: self._update_robot_pose() + async def wait_for_updated_robot_pose(self) -> None: + if self.ensure_gnss: + await self._pose_update.wait() + def _update_robot_pose(self) -> None: x = np.mean([pose.point.x for pose in self.observed_poses]) y = np.mean([pose.point.y for pose in self.observed_poses]) yaw = np.mean([pose.yaw for pose in self.observed_poses]) pose = rosys.geometry.Pose(x=float(x), y=float(y), yaw=float(yaw), time=rosys.time()) self.ROBOT_POSE_LOCATED.emit(pose) + self._pose_update.set() + self._pose_update.clear() self.last_pose_update = rosys.time() self.observed_poses.clear()