Skip to content

Commit

Permalink
Remove the need for gnss pausing (#189)
Browse files Browse the repository at this point in the history
Based on the findings in
#177, we decided to
remove the gnss pausing and automaticly update when the robot didn't
move between gnss signals.

- [x] Wait for zauberzeug/rosys#204
- [x] Wait for #188
- [x] Fix test
- [x] Test on real robot
- [x] Wait for zauberzeug/rosys#210
- [x] Wait for #202

Note on the movement check:
Before it was check whether both poses are equal, which will not work
with small movement / sensor noise and time differences between both
poses.
The tolerance values are arbitrarily picked, but should work alright.


https://github.com/zauberzeug/field_friend/blob/ef427b827e8af797f2c4213b9054ebbbae6927a8/field_friend/automations/automation_watcher.py#L133
vs


https://github.com/zauberzeug/field_friend/blob/a2378a2144d63d76e4db5c77eb276835115ea682/field_friend/localization/gnss.py#L109

---------

Co-authored-by: LukasBaecker <65940705+LukasBaecker@users.noreply.github.com>
  • Loading branch information
pascalzauberzeug and LukasBaecker authored Oct 22, 2024
1 parent 75337fa commit ecf53cc
Show file tree
Hide file tree
Showing 8 changed files with 99 additions and 78 deletions.
14 changes: 0 additions & 14 deletions field_friend/automations/automation_watcher.py
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,6 @@ def __init__(self, system: 'System') -> None:

rosys.on_repeat(self.try_resume, 0.1)
rosys.on_repeat(self.check_field_bounds, 1.0)
rosys.on_repeat(self.ensure_robot_pose_updates_when_not_in_automation, 5.0)
if self.field_friend.bumper:
self.field_friend.bumper.BUMPER_TRIGGERED.register(lambda name: self.pause(f'Bumper {name} was triggered'))
self.gnss.GNSS_CONNECTION_LOST.register(lambda: self.pause('GNSS connection lost'))
Expand Down Expand Up @@ -124,16 +123,3 @@ def check_field_bounds(self) -> None:
if self.automator.is_running:
self.stop('robot is outside of field boundaries')
self.field_watch_active = False

async def ensure_robot_pose_updates_when_not_in_automation(self) -> None:
if self.automator.is_running:
return
if self.gnss.is_paused:
self.log.warning('GNSS is paused, this should not happen outside of an automation')
self.gnss.is_paused = False
return
if self.last_robot_pose == self.odometer.prediction:
await self.gnss.update_robot_pose()
else:
self.gnss.observed_poses.clear()
self.last_robot_pose = self.odometer.prediction
27 changes: 17 additions & 10 deletions field_friend/automations/navigation/field_navigation.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,12 +25,14 @@ class State(Enum):

class FieldNavigation(FollowCropsNavigation):
TURN_STEP = np.deg2rad(25.0)
MAX_GNSS_WAITING_TIME = 15.0

def __init__(self, system: 'System', implement: Implement) -> None:
super().__init__(system, implement)
self.name = 'Field Navigation'
self.gnss = system.gnss
self.bms = system.field_friend.bms
self.automator = system.automator
self.automation_watcher = system.automation_watcher
self.field_provider = system.field_provider

Expand All @@ -42,6 +44,7 @@ def __init__(self, system: 'System', implement: Implement) -> None:
self.field: Field | None = None
self._loop: bool = False
self._turn_step = self.TURN_STEP
self._max_gnss_waiting_time = self.MAX_GNSS_WAITING_TIME

@property
def current_row(self) -> Row:
Expand Down Expand Up @@ -121,6 +124,10 @@ def update_target(self) -> None:

async def _drive(self, distance: float) -> None:
assert self.field is not None
# TODO: remove temporary fix for GNSS waiting
if self.odometer.prediction.distance(self.gnss._last_gnss_pose) > 2.0: # pylint: disable=protected-access
await self.gnss.ROBOT_POSE_LOCATED.emitted(self._max_gnss_waiting_time)

if self._state == State.APPROACHING_ROW_START:
self._state = await self._run_approaching_row_start()
elif self._state == State.FOLLOWING_ROW:
Expand All @@ -137,20 +144,20 @@ def interpolate_angles(start: float, end: float, step: float) -> Generator[float
yield angle

self.set_start_and_end_points()
await self._wait_for_gnss()
await self.gnss.ROBOT_POSE_LOCATED.emitted(self._max_gnss_waiting_time)
target_yaw = self.odometer.prediction.direction(self.start_point)
# turn towards row start
for angle in interpolate_angles(self.odometer.prediction.yaw, target_yaw, self._turn_step):
await self.turn_to_yaw(angle)
await self._wait_for_gnss()
await self.gnss.ROBOT_POSE_LOCATED.emitted(self._max_gnss_waiting_time)
# drive to row start
await self.driver.drive_to(self.start_point)
await self._wait_for_gnss()
await self.gnss.ROBOT_POSE_LOCATED.emitted(self._max_gnss_waiting_time)
# adjust heading
row_yaw = self.start_point.direction(self.end_point)
for angle in interpolate_angles(self.odometer.prediction.yaw, row_yaw, self._turn_step):
await self.turn_to_yaw(angle)
await self._wait_for_gnss()
await self.gnss.ROBOT_POSE_LOCATED.emitted(self._max_gnss_waiting_time)

if isinstance(self.detector, rosys.vision.DetectorSimulation) and not rosys.is_test:
self.create_simulation()
Expand Down Expand Up @@ -195,19 +202,14 @@ async def _run_row_completed(self) -> State:
next_state = State.FIELD_COMPLETED
return next_state

async def _wait_for_gnss(self, waiting_time: float = 1.0) -> None:
self.gnss.is_paused = False
await rosys.sleep(waiting_time)
await self.gnss.update_robot_pose()
self.gnss.is_paused = True

def backup(self) -> dict:
return super().backup() | {
'field_id': self.field.id if self.field else None,
'row_index': self.row_index,
'state': self._state.name,
'loop': self._loop,
'turn_step': self._turn_step,
'max_gnss_waiting_time': self._max_gnss_waiting_time,
}

def restore(self, data: dict[str, Any]) -> None:
Expand All @@ -218,6 +220,7 @@ def restore(self, data: dict[str, Any]) -> None:
self._state = State[data.get('state', State.APPROACHING_ROW_START.name)]
self._loop = data.get('loop', False)
self._turn_step = data.get('turn_step', self.TURN_STEP)
self._max_gnss_waiting_time = data.get('max_gnss_waiting_time', self.MAX_GNSS_WAITING_TIME)

def settings_ui(self) -> None:
with ui.row():
Expand All @@ -239,6 +242,10 @@ def developer_ui(self) -> None:
.props('dense outlined') \
.classes('w-24') \
.bind_value(self, '_turn_step', forward=np.deg2rad, backward=np.rad2deg)
ui.number('Turn Step', step=0.1, min=0.1, max=20.0, format='%.2f', on_change=self.request_backup) \
.props('dense outlined') \
.classes('w-24') \
.bind_value(self, '_max_gnss_waiting_time')

def _set_field(self, field_id: str) -> None:
field = self.field_provider.get_field(field_id)
Expand Down
14 changes: 3 additions & 11 deletions field_friend/automations/navigation/navigation.py
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,6 @@ async def start(self) -> None:
if not await self.prepare():
self.log.error('Preparation failed')
return
await self.gnss.update_robot_pose()
if self.gnss.check_distance_to_reference():
raise WorkflowException('reference to far away from robot')
self.start_position = self.odometer.prediction.point
Expand All @@ -57,15 +56,11 @@ async def start(self) -> None:
self.log.info('Navigation started')
self.is_active = True
while not self._should_finish():
await self.gnss.update_robot_pose()
self.gnss.is_paused = True
distance = await self.implement.get_stretch(self.MAX_STRETCH_DISTANCE)
if distance > self.MAX_STRETCH_DISTANCE: # we do not want to drive to long without observing
await self._drive(self.DEFAULT_DRIVE_DISTANCE)
self.gnss.is_paused = False
continue
await self._drive(distance)
self.gnss.is_paused = False
await self.implement.start_workflow()
await self.implement.stop_workflow()
except WorkflowException as e:
Expand All @@ -84,13 +79,10 @@ 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')
await self.gnss.update_robot_pose()
return True

async def finish(self) -> None:
"""Executed after the navigation is done"""
self.gnss.is_paused = False
self.is_active = False
self.log.info('Navigation finished')

@abc.abstractmethod
Expand Down Expand Up @@ -144,7 +136,7 @@ def settings_ui(self) -> None:
.classes('w-24') \
.bind_value(self, 'linear_speed_limit') \
.tooltip(f'Forward speed limit in m/s (default: {self.LINEAR_SPEED_LIMIT:.2f})')

def _update_time(self):
"""Update KPIs for time"""
if not self.is_active:
Expand All @@ -153,5 +145,5 @@ def _update_time(self):
self.start_time = rosys.time()
passed_time = rosys.time() - self.start_time
if passed_time > 1:
self.kpi_provider.increment_all_time_kpi('time',passed_time)
self.start_time = rosys.time()
self.kpi_provider.increment_all_time_kpi('time', passed_time)
self.start_time = rosys.time()
21 changes: 15 additions & 6 deletions field_friend/interface/components/status_dev.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,16 @@
from nicegui import ui

from ... import localization
from ...hardware import (Axis, AxisD1, ChainAxis, FieldFriend, FieldFriendHardware, FlashlightPWMHardware,
FlashlightPWMHardwareV2, Tornado)
from ...hardware import (
Axis,
AxisD1,
ChainAxis,
FieldFriend,
FieldFriendHardware,
FlashlightPWMHardware,
FlashlightPWMHardwareV2,
Tornado,
)

if TYPE_CHECKING:
from field_friend.system import System
Expand Down Expand Up @@ -167,14 +175,15 @@ def status_dev_page(robot: FieldFriend, system: 'System'):
with ui.row().classes('place-items-center'):
ui.markdown('**RTK-Fix:**').style('color: #EDF4FB')
rtk_fix_label = ui.label()
with ui.row().classes('place-items-center'):
ui.markdown('**GNSS paused:**').style('color: #EDF4FB')
gnss_paused_label = ui.label()

with ui.row().classes('place-items-center'):
ui.markdown('**odometry:**').style('color: #EDF4FB')
odometry_label = ui.label()

with ui.row().classes('place-items-center'):
ui.markdown('**Since last uupdate:**').style('color: #EDF4FB')
update_label = ui.label()

def update_status() -> None:
bms_flags = [
f'{robot.bms.state.short_string}',
Expand Down Expand Up @@ -291,8 +300,8 @@ def update_status() -> None:
gnss_label.text = 'No position' if system.gnss.current is None else str(system.gnss.current.location)
heading_label.text = f'{system.gnss.current.heading:.2f}° {direction_flag}' if system.gnss.current is not None and system.gnss.current.heading is not None else 'No heading'
rtk_fix_label.text = f'gps_qual: {system.gnss.current.gps_qual}, mode: {system.gnss.current.mode}' if system.gnss.current is not None else 'No fix'
gnss_paused_label.text = str(system.gnss.is_paused)
odometry_label.text = str(system.odometer.prediction)
update_label.text = f'{timedelta(seconds=rosys.time() - system.gnss._last_gnss_pose.time)}' # pylint: disable=protected-access

ui.timer(rosys.config.ui_update_interval, update_status)
return status_dev_page
52 changes: 28 additions & 24 deletions field_friend/localization/gnss.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,12 +3,14 @@
import logging
import os
from abc import ABC, abstractmethod
from collections import deque
from copy import deepcopy
from dataclasses import dataclass
from typing import Any

import numpy as np
import rosys
import rosys.helpers
from nicegui import ui
from rosys.persistence.registry import backup

Expand All @@ -30,9 +32,8 @@ class GNSSRecord:


class Gnss(rosys.persistence.PersistentModule, ABC):
NEEDED_POSES: int = 10
MIN_SECONDS_BETWEEN_UPDATES: float = 10.0
ENSURE_GNSS: bool = False
MIN_POSES: int = 3
MAX_POSES: int = 30
MAX_DISTANCE_TO_REFERENCE: float = 5000.0

def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> None:
Expand All @@ -55,18 +56,17 @@ def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> N
self.current: GNSSRecord | None = None
self.device: str | None = None
self.antenna_offset = antenna_offset
self.is_paused = False
self.observed_poses: list[rosys.geometry.Pose] = []
self.last_pose_update = rosys.time()
self.needed_poses: int = self.NEEDED_POSES
self.min_seconds_between_updates: float = self.MIN_SECONDS_BETWEEN_UPDATES
self.ensure_gnss: bool = self.ENSURE_GNSS
self.observed_poses: deque[rosys.geometry.Pose] = deque(maxlen=self.MAX_POSES)
self.needed_poses: int = self.MIN_POSES
self.max_distance_to_reference: float = self.MAX_DISTANCE_TO_REFERENCE
self.reference_alert_dialog: ui.dialog
self._last_odometer_pose = self.odometer.prediction
self._last_gnss_pose = self.odometer.prediction

self.needs_backup = False
rosys.on_repeat(self.check_gnss, 0.01)
rosys.on_repeat(self.try_connection, 3.0)
rosys.on_repeat(self.update_robot_pose, 0.5)

@abstractmethod
async def try_connection(self) -> None:
Expand All @@ -93,7 +93,7 @@ async def check_gnss(self) -> None:
try:
# TODO also do antenna_offset correction for this event
self.ROBOT_GNSS_POSITION_CHANGED.emit(self.current.location)
if not self.is_paused and ("R" in self.current.mode or self.current.mode == "SSSS"):
if "R" in self.current.mode or self.current.mode == "SSSS":
await self._on_rtk_fix()
except Exception:
self.log.exception('gnss record could not be applied')
Expand All @@ -104,6 +104,12 @@ async def _create_new_record(self) -> GNSSRecord | None:
pass

async def _on_rtk_fix(self) -> None:
distance = self._last_odometer_pose.distance(self.odometer.prediction)
yaw_difference = abs(rosys.helpers.angle(self._last_odometer_pose.yaw, self.odometer.prediction.yaw))
if abs(distance) > 0.0001 or yaw_difference > np.deg2rad(0.01): # if robot does not stand still
self.observed_poses.clear()
self._last_odometer_pose = self.odometer.prediction
return
assert self.current is not None
if localization.reference.lat == 0 and localization.reference.long == 0:
await self.update_reference()
Expand All @@ -123,34 +129,24 @@ async def _on_rtk_fix(self) -> None:
self.observed_poses.append(pose)

async def update_robot_pose(self) -> None:
assert not self.is_paused
if self.ensure_gnss:
while len(self.observed_poses) < self.needed_poses:
if rosys.time() - self.last_pose_update < self.min_seconds_between_updates:
return
await rosys.sleep(0.1)
if not self.observed_poses:
if len(self.observed_poses) < self.needed_poses:
return
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.last_pose_update = rosys.time()
self._last_gnss_pose = rosys.geometry.Pose(x=float(x), y=float(
y), yaw=float(yaw), time=self.observed_poses[-1].time)
self.ROBOT_POSE_LOCATED.emit(self._last_gnss_pose)
self.observed_poses.clear()

def backup(self) -> dict:
return {
'needed_poses': self.needed_poses,
'min_seconds_between_updates': self.min_seconds_between_updates,
'ensure_gnss': self.ensure_gnss
}

def restore(self, data: dict[str, Any]) -> None:
super().restore(data)
self.needed_poses = data.get('needed_poses', self.needed_poses)
self.min_seconds_between_updates = data.get('min_seconds_between_updates', self.min_seconds_between_updates)
self.ensure_gnss = data.get('ensure_gnss', self.ensure_gnss)

async def update_reference(self) -> None:
if self.current is None:
Expand All @@ -174,3 +170,11 @@ def check_distance_to_reference(self) -> bool:
self.reference_alert_dialog.open()
return True
return False

@ui.refreshable
def developer_ui(self) -> None:
ui.label('GNSS').classes('text-center text-bold')
ui.label(f'Reference: {localization.reference}')
ui.label(f'Observed_Poses: {len(self.observed_poses)}')
ui.label(f'Needed Poses: {self.needed_poses}')
ui.label(f'odom: {self.odometer.prediction}')
1 change: 0 additions & 1 deletion field_friend/localization/gnss_simulation.py
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@ async def _create_new_record(self) -> Optional[GNSSRecord]:
def relocate(self, position: GeoPoint) -> None:
localization.reference = position
self.wheels.pose = Pose(x=position.lat, y=position.long, yaw=0.0, time=rosys.time())
self.last_pose_update = rosys.time()

def disconnect(self):
"""Simulate serial disconnection.
Expand Down
18 changes: 18 additions & 0 deletions tests/test_gnss.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@
from rosys.testing import assert_point, forward

from field_friend import localization
from field_friend.automations.implements import Recorder
from field_friend.automations.navigation import StraightLineNavigation
from field_friend.localization import GeoPoint, GnssSimulation
from field_friend.system import System

Expand Down Expand Up @@ -78,10 +80,26 @@ async def test_device_disconnects(gnss_driving: System, gnss: GnssSimulation):


async def test_record_is_none(gnss_driving: System, gnss: GnssSimulation):
# pylint: disable=protected-access
async def empty():
return None
await forward(x=2.0)
gnss._create_new_record = empty # type: ignore
await forward(5)
# robot should have stopped driving
assert_point(gnss_driving.odometer.prediction.point, rosys.geometry.Point(x=2, y=0))


async def test_only_update_when_standing(gnss_driving: System, gnss: GnssSimulation):
# pylint: disable=protected-access
assert isinstance(gnss_driving.current_navigation, StraightLineNavigation)
gnss_driving.current_navigation.length = 10.0
assert isinstance(gnss_driving.current_navigation.implement, Recorder)
assert gnss._last_gnss_pose.x == pytest.approx(gnss._last_odometer_pose.x)
gnss_driving.automator.start()
await forward(until=lambda: gnss_driving.automator.is_running)
await forward(x=5.0)
assert gnss._last_gnss_pose.x != pytest.approx(gnss._last_odometer_pose.x)
await forward(until=lambda: gnss_driving.automator.is_stopped)
await forward(2.0)
assert gnss._last_gnss_pose.x == pytest.approx(gnss._last_odometer_pose.x)
Loading

0 comments on commit ecf53cc

Please sign in to comment.