Skip to content

Commit

Permalink
Merge branch 'main' into rosys_gnss
Browse files Browse the repository at this point in the history
  • Loading branch information
pascalzauberzeug committed Nov 27, 2024
2 parents e89a0b1 + 1651b81 commit 03fb774
Show file tree
Hide file tree
Showing 7 changed files with 29 additions and 8 deletions.
10 changes: 8 additions & 2 deletions field_friend/automations/field_provider.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,14 @@ def __init__(self) -> None:
self.FIELDS_CHANGED = rosys.event.Event()
"""The dict of fields has changed."""

self.FIELDS_CHANGED.register(self.refresh_fields)

self.selected_field: Field | None = None
self.FIELD_SELECTED = rosys.event.Event()
"""A field has been selected."""

self.FIELDS_CHANGED.register(self.refresh_fields)
self.FIELD_SELECTED.register(self.clear_selected_beds)

self._only_specific_beds: bool = False
self._selected_beds: list[int] = []

@property
Expand Down Expand Up @@ -59,6 +61,7 @@ def invalidate(self) -> None:
self.FIELDS_CHANGED.emit()
if self.selected_field and self.selected_field not in self.fields:
self.selected_field = None
self._only_specific_beds = False
self.selected_beds = []
self.FIELD_SELECTED.emit()

Expand Down Expand Up @@ -133,6 +136,7 @@ def update_field_parameters(self, *,
self.invalidate()

def clear_selected_beds(self) -> None:
self._only_specific_beds = False
self.selected_beds = []

def get_rows_to_work_on(self) -> list[Row]:
Expand All @@ -141,6 +145,8 @@ def get_rows_to_work_on(self) -> list[Row]:
return []
if self.selected_field.bed_count == 1:
return self.selected_field.rows
if not self._only_specific_beds:
return self.selected_field.rows
if len(self.selected_beds) == 0:
self.log.warning('No beds selected. Cannot get rows to work on.')
return []
Expand Down
7 changes: 7 additions & 0 deletions field_friend/hardware/y_axis_stepper_hardware.py
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,13 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
{name}_end_l.inverted = {str(end_stops_inverted).lower()}
{name}_end_r = {expander.name + "." if end_stops_on_expander and expander else ""}Input({end_r_pin})
{name}_end_r.inverted = {str(end_stops_inverted).lower()}
# TODO: remove when lizard issue 66 is fixed.
{name}_alarm.level = 0
{name}_alarm.active = false
{name}_end_l.level = 0
{name}_end_l.active = false
{name}_end_r.level = 0
{name}_end_r.active = false
{name} = {expander.name + "." if motor_on_expander and expander else ""}MotorAxis({name}_motor, {name + "_end_l" if reversed_direction else name + "_end_r"}, {name + "_end_r" if reversed_direction else name + "_end_l"})
''')
core_message_fields = [
Expand Down
5 changes: 2 additions & 3 deletions field_friend/hardware/z_axis_canopen_hardware.py
Original file line number Diff line number Diff line change
Expand Up @@ -33,12 +33,11 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
{name}_motor = {expander.name + "." if motor_on_expander and expander else ""}CanOpenMotor({can.name}, {can_address})
{name}_end_t = {expander.name + "." if end_stops_on_expander and expander else ""}Input({end_t_pin})
{name}_end_t.inverted = {str(end_stops_inverted).lower()}
# TODO: remove when lizard issue 66 is fixed.
{name}_end_b.level = 0
{name}_end_t.active = false
{name}_end_b = {expander.name + "." if end_stops_on_expander and expander else ""}Input({end_b_pin})
{name}_end_b.inverted = {str(end_stops_inverted).lower()}
# TODO: remove when lizard issue 66 is fixed.
{name}_end_t.level = 0
{name}_end_t.active = false
{name}_end_b.level = 0
{name}_end_b.active = false
{name} = {expander.name + "." if motor_on_expander and expander else ""}MotorAxis({name}_motor, {name + "_end_t" if reversed_direction else name + "_end_b"}, {name + "_end_b" if reversed_direction else name + "_end_t"})
Expand Down
5 changes: 5 additions & 0 deletions field_friend/hardware/z_axis_stepper_hardware.py
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,11 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
{name}_end_t.inverted = {str(end_stops_inverted).lower()}
{name}_end_b = {expander.name + "." if end_stops_on_expander and expander else ""}Input({end_b_pin})
{name}_end_b.inverted = {str(end_stops_inverted).lower()}
# TODO: remove when lizard issue 66 is fixed.
{name}_end_t.level = 0
{name}_end_t.active = false
{name}_end_b.level = 0
{name}_end_b.active = false
{name} = {expander.name + "." if motor_on_expander and expander else ""}MotorAxis({name}_motor, {name + "_end_t" if reversed_direction else name + "_end_b"}, {name + "_end_b" if reversed_direction else name + "_end_t"})
''')
core_message_fields = [
Expand Down
4 changes: 2 additions & 2 deletions field_friend/interface/components/operation.py
Original file line number Diff line number Diff line change
Expand Up @@ -172,8 +172,8 @@ def field_setting(self) -> None:
.tooltip('Delete the selected field')
if self.system.field_provider.selected_field.bed_count > 1:
with ui.row().classes('w-full'):
beds_checkbox = ui.checkbox('Select specific beds').classes(
'w-full').on_value_change(self.system.field_provider.clear_selected_beds)
beds_checkbox = ui.checkbox('Select specific beds').classes('w-full') \
.bind_value(self.system.field_provider, '_only_specific_beds')
with ui.row().bind_visibility_from(beds_checkbox, 'value').classes('w-full'):
ui.select(list(range(1, int(self.system.field_provider.selected_field.bed_count) + 1)), multiple=True, label='selected beds', clearable=True) \
.classes('grow').props('use-chips').bind_value(self.system.field_provider, 'selected_beds')
2 changes: 1 addition & 1 deletion field_friend/vision/camera_configurator.py
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ async def update_camera_config(self):
parameters_changed = True
self.log.info(f'camera rotation: {camera.rotation}; {camera.rotation_angle}')
else:
camera.rotation = 0
camera.rotation_angle = 0

elif isinstance(camera, rosys.vision.SimulatedCalibratableCamera):
if camera.resolution.width != self.config['parameters']['width'] or camera.resolution.height != self.config['parameters']['height']:
Expand Down
4 changes: 4 additions & 0 deletions tests/test_navigation.py
Original file line number Diff line number Diff line change
Expand Up @@ -411,6 +411,8 @@ async def test_complete_field_with_selected_beds(system: System, field_with_beds
assert system.gnss.last_measurement.point.distance(ROBOT_GEO_START_POSITION) < 0.01
system.field_provider.selected_beds = [1, 3]
system.field_provider.select_field(field_with_beds.id)
system.field_provider._only_specific_beds = True
system.field_provider.selected_beds = [1, 3]
system.current_navigation = system.field_navigation
system.automator.start()
await forward(until=lambda: system.automator.is_running)
Expand All @@ -427,6 +429,8 @@ async def test_complete_field_without_first_beds(system: System, field_with_beds
assert system.gnss.last_measurement.point.distance(ROBOT_GEO_START_POSITION) < 0.01
system.field_provider.selected_beds = [2, 3, 4]
system.field_provider.select_field(field_with_beds.id)
system.field_provider._only_specific_beds = True
system.field_provider.selected_beds = [2, 3, 4]
system.current_navigation = system.field_navigation
system.automator.start()
await forward(until=lambda: system.automator.is_running)
Expand Down

0 comments on commit 03fb774

Please sign in to comment.