From 25e6caace690d607a1ea44eb72ab521e8bc35da8 Mon Sep 17 00:00:00 2001 From: Christoph Biering <1353438+biering@users.noreply.github.com> Date: Thu, 6 Feb 2025 12:58:41 +0000 Subject: [PATCH] feat(RPS-1206): implemented I/O access, read & write (#41) Co-authored-by: cbiering --- examples/01_basic.py | 2 +- examples/02_plan_and_execute.py | 2 +- examples/03_move_and_set_ios.py | 8 +- examples/04_move_multiple_robots.py | 4 +- .../05_selection_motion_group_activation.py | 4 +- nova/core/controller.py | 60 ++++++-- nova/core/io.py | 128 ++++++++++++++++++ nova/core/nova.py | 4 + nova/core/robot_cell.py | 51 ++++++- tests/core/test_io.py | 51 +++++++ 10 files changed, 291 insertions(+), 23 deletions(-) create mode 100644 nova/core/io.py create mode 100644 tests/core/test_io.py diff --git a/examples/01_basic.py b/examples/01_basic.py index 6fb6d97..99cdd09 100644 --- a/examples/01_basic.py +++ b/examples/01_basic.py @@ -41,7 +41,7 @@ async def main(): tcp_pose = await motion_group.tcp_pose(tcp) print(tcp_pose) - await cell.delete_robot_controller(controller.name) + await cell.delete_robot_controller(controller.controller_id) if __name__ == "__main__": diff --git a/examples/02_plan_and_execute.py b/examples/02_plan_and_execute.py index 16afff0..384b7e4 100644 --- a/examples/02_plan_and_execute.py +++ b/examples/02_plan_and_execute.py @@ -54,7 +54,7 @@ async def main(): joint_trajectory = await motion_group.plan(actions, tcp) await motion_group.execute(joint_trajectory, tcp, actions=actions) - await cell.delete_robot_controller(controller.name) + await cell.delete_robot_controller(controller.controller_id) if __name__ == "__main__": diff --git a/examples/03_move_and_set_ios.py b/examples/03_move_and_set_ios.py index 3fb6cb6..7b3c22d 100644 --- a/examples/03_move_and_set_ios.py +++ b/examples/03_move_and_set_ios.py @@ -47,7 +47,13 @@ def on_movement(motion_state): await motion_group.plan_and_execute(actions, tcp, on_movement=on_movement) - await cell.delete_robot_controller(controller.name) + io_value = await controller.read("tool_out[0]") + print(io_value) + await controller.write("tool_out[0]", True) + written_io_value = await controller.read("tool_out[0]") + print(written_io_value) + + await cell.delete_robot_controller(controller.controller_id) if __name__ == "__main__": diff --git a/examples/04_move_multiple_robots.py b/examples/04_move_multiple_robots.py index 93bf58b..142d72f 100644 --- a/examples/04_move_multiple_robots.py +++ b/examples/04_move_multiple_robots.py @@ -42,8 +42,8 @@ async def main(): models.Manufacturer.UNIVERSALROBOTS, ) await asyncio.gather(move_robot(ur5), move_robot(ur10)) - await cell.delete_robot_controller(ur5.name) - await cell.delete_robot_controller(ur10.name) + await cell.delete_robot_controller(ur5.controller_id) + await cell.delete_robot_controller(ur10.controller_id) if __name__ == "__main__": diff --git a/examples/05_selection_motion_group_activation.py b/examples/05_selection_motion_group_activation.py index 90de916..7d71fe4 100644 --- a/examples/05_selection_motion_group_activation.py +++ b/examples/05_selection_motion_group_activation.py @@ -71,8 +71,8 @@ async def main(): async with mg_0, mg_1: await asyncio.gather(move_robot(mg_0, tcp), move_robot(mg_1, tcp)) - await cell.delete_robot_controller(ur5.name) - await cell.delete_robot_controller(ur10.name) + await cell.delete_robot_controller(ur5.controller_id) + await cell.delete_robot_controller(ur10.controller_id) if __name__ == "__main__": diff --git a/nova/core/controller.py b/nova/core/controller.py index 388cc8d..cf96f8f 100644 --- a/nova/core/controller.py +++ b/nova/core/controller.py @@ -1,13 +1,30 @@ -from typing import Sized, final +from typing import Sized, Literal from loguru import logger from nova.core.motion_group import MotionGroup from nova.api import models from nova.gateway import ApiGateway +from nova.core.robot_cell import ( + AbstractController, + ConfigurablePeriphery, + Device, + IODevice, + AbstractRobot, + ValueType, +) +from nova.core.io import IOAccess -class Controller(Sized): +# TODO: Device is not associated to IODevice so it is pretty confusing and we should change it +class Controller(Sized, AbstractController, ConfigurablePeriphery, Device, IODevice): + class Configuration(ConfigurablePeriphery.Configuration): + type: Literal["controller"] = "controller" + identifier: str = "controller" + controller_id: str + # TODO: needs to be removed + plan: bool = False + def __init__( self, *, api_gateway: ApiGateway, cell: str, controller_instance: models.ControllerInstance ): @@ -15,22 +32,30 @@ def __init__( self._controller_api = api_gateway.controller_api self._motion_group_api = api_gateway.motion_group_api self._cell = cell - self._name = controller_instance.controller + self._controller_id = controller_instance.controller self._activated_motion_group_ids: list[str] = [] + self._io_access = IOAccess( + api_gateway=api_gateway, cell=cell, controller_id=controller_instance.controller + ) + + configuration = self.Configuration( + identifier=controller_instance.controller, + controller_id=controller_instance.controller, + plan=False, + ) + super().__init__(configuration=configuration) @property - def name(self) -> str: - return self._name + def controller_id(self) -> str: + return self._controller_id - @final - async def __aenter__(self): + async def open(self): motion_group_ids = await self.activated_motion_group_ids() self._activated_motion_group_ids = motion_group_ids logger.info(f"Found motion group {motion_group_ids}") return self - @final - async def __aexit__(self, exc_type, exc_val, exc_tb): + async def close(self): # RPS-1174: when a motion group is deactivated, RAE closes all open connections # this behaviour is not desired in some cases, # so for now we will not deactivate for the user @@ -39,11 +64,12 @@ async def __aexit__(self, exc_type, exc_val, exc_tb): def __len__(self) -> int: return len(self._activated_motion_group_ids) + # TODO: should accept the exact motion group id as str def motion_group(self, motion_group_id: int = 0) -> MotionGroup: return MotionGroup( api_gateway=self._api_gateway, cell=self._cell, - motion_group_id=f"{motion_group_id}@{self._name}", + motion_group_id=f"{motion_group_id}@{self._controller_id}", ) def __getitem__(self, motion_group_id: int) -> MotionGroup: @@ -52,7 +78,7 @@ def __getitem__(self, motion_group_id: int) -> MotionGroup: async def activated_motion_group_ids(self) -> list[str]: activate_all_motion_groups_response = ( await self._motion_group_api.activate_all_motion_groups( - cell=self._cell, controller=self._name + cell=self._cell, controller=self._controller_id ) ) motion_groups = activate_all_motion_groups_response.instances @@ -61,3 +87,15 @@ async def activated_motion_group_ids(self) -> list[str]: async def activated_motion_groups(self) -> list[MotionGroup]: motion_group_ids = await self.activated_motion_group_ids() return [self.motion_group(int(mg.split("@")[0])) for mg in motion_group_ids] + + def get_robots(self) -> dict[str, AbstractRobot]: + return { + motion_group_id: self.motion_group(int(motion_group_id.split("@")[0])) + for motion_group_id in self._activated_motion_group_ids + } + + async def read(self, key: str) -> ValueType: + return await self._io_access.read(key) + + async def write(self, key: str, value: ValueType) -> None: + return await self._io_access.write(key, value) diff --git a/nova/core/io.py b/nova/core/io.py new file mode 100644 index 0000000..f6f4259 --- /dev/null +++ b/nova/core/io.py @@ -0,0 +1,128 @@ +from __future__ import annotations + +import asyncio +from nova.api import models + +from nova.core.robot_cell import Device, ValueType +from nova.gateway import ApiGateway +from enum import Enum + + +class IOType(Enum): + IO_TYPE_INPUT = "IO_TYPE_INPUT" + IO_TYPE_OUTPUT = "IO_TYPE_OUTPUT" + + +class IOValueType(Enum): + IO_VALUE_ANALOG_INTEGER = "IO_VALUE_ANALOG_INTEGER" + IO_VALUE_ANALOG_FLOATING = "IO_VALUE_ANALOG_FLOATING" + IO_VALUE_DIGITAL = "IO_VALUE_DIGITAL" + + +class ComparisonType(Enum): + COMPARISON_TYPE_EQUAL = "COMPARISON_TYPE_EQUAL" + COMPARISON_TYPE_GREATER = "COMPARISON_TYPE_GREATER" + COMPARISON_TYPE_LESS = "COMPARISON_TYPE_LESS" + + +class IOAccess(Device): + """Provides access to input and outputs via a dictionary-like style + + TODO: + - Add listener to value changes + - Handle integer masks + - Read and check types based on the description + """ + + io_descriptions_cache: dict[str, dict[str, models.IODescription]] = {} + + def __init__(self, api_gateway: ApiGateway, cell: str, controller_id: str): + super().__init__() + self._api_gateway = api_gateway + self._controller_ios_api = api_gateway.controller_ios_api + self._cell = cell + self._controller_id = controller_id + self._io_operation_in_progress = asyncio.Lock() + + async def get_io_descriptions(self) -> dict[str, models.IODescription]: + cache = self.__class__.io_descriptions_cache + if self._controller_id not in cache: + # empty list fetches all + response = await self._controller_ios_api.list_io_descriptions( + cell=self._cell, controller=self._controller_id, ios=None + ) + cache[self._controller_id] = { + description.id: description for description in response.io_descriptions + } + return cache[self._controller_id] + + @staticmethod + def filter_io_descriptions( + io_descriptions: dict[str, models.IODescription], + filter_value_type: IOValueType | None = None, + filter_type: IOType | None = None, + ) -> list[str]: + return [ + io.id + for io in io_descriptions.values() + if filter_value_type is None + or (IOValueType(io.value_type) == filter_value_type and IOType(io.type) == filter_type) + ] + + async def read(self, key: str) -> bool | int | float: + """Reads a value from a given IO""" + async with self._io_operation_in_progress: + values = await self._controller_ios_api.list_io_values( + cell=self._cell, controller=self._controller_id, ios=[key] + ) + io_value: models.IOValue = values.io_values[0] + + if io_value.boolean_value is not None: + return io_value.boolean_value + if io_value.integer_value is not None: + return int(io_value.integer_value) + if io_value.floating_value is not None: + return float(io_value.floating_value) + raise ValueError(f"IO value for {key} is of an unexpected type.") + + async def write(self, key: str, value: ValueType): + """Set a value asynchronously (So a direct read after setting might return still the old value)""" + io_descriptions = await self.get_io_descriptions() + io_description = io_descriptions[key] + io_value_type = IOValueType(io_description.value_type) + if isinstance(value, bool): + if io_value_type is not IOValueType.IO_VALUE_DIGITAL: + raise ValueError( + f"Boolean value can only be set at an IO_VALUE_DIGITAL IO and not to {io_value_type}" + ) + io_value = models.IOValue(io=key, boolean_value=value) + elif isinstance(value, int): + if io_value_type is not IOValueType.IO_VALUE_ANALOG_INTEGER: + raise ValueError( + f"Integer value can only be set at an IO_VALUE_ANALOG_INTEGER IO and not to {io_value_type}" + ) + io_value = models.IOValue(io=key, integer_value=str(value)) # TODO: handle mask + elif isinstance(value, float): + if io_value_type is not IOValueType.IO_VALUE_ANALOG_FLOATING: + raise ValueError( + f"Float value can only be set at an IO_VALUE_ANALOG_FLOATING IO and not to {io_value_type}" + ) + io_value = models.IOValue(io=key, floating_value=value) + else: + raise ValueError(f"Unexpected type {type(value)}") + + async with self._io_operation_in_progress: + await self._controller_ios_api.set_output_values( + cell=self._cell, controller=self._controller_id, io_value=[io_value] + ) + + async def wait_for_bool_io(self, io_id: str, value: bool): + """Blocks until the requested IO equals the provided value.""" + # TODO proper implementation utilising also the comparison operators + await self._controller_ios_api.wait_for_io_event( + cell=self._cell, + controller=self._controller_id, + io=io_id, + comparison_type=ComparisonType.COMPARISON_TYPE_EQUAL, + boolean_value=value, + ) diff --git a/nova/core/nova.py b/nova/core/nova.py index 366163e..439d589 100644 --- a/nova/core/nova.py +++ b/nova/core/nova.py @@ -49,6 +49,10 @@ def __init__(self, api_gateway: ApiGateway, cell_id: str): self._api_gateway = api_gateway self._cell_id = cell_id + @property + def cell_id(self) -> str: + return self._cell_id + async def _get_controller_instances(self) -> list[models.ControllerInstance]: response = await self._api_gateway.controller_api.list_controllers(cell=self._cell_id) return response.instances diff --git a/nova/core/robot_cell.py b/nova/core/robot_cell.py index 48816fe..a4c90ce 100644 --- a/nova/core/robot_cell.py +++ b/nova/core/robot_cell.py @@ -1,4 +1,14 @@ -from typing import final, Union, Protocol, runtime_checkable, AsyncIterable, Callable +from typing import ( + final, + Union, + Protocol, + runtime_checkable, + AsyncIterable, + Callable, + TypeVar, + Awaitable, + Generic, +) from abc import ABC, abstractmethod import asyncio from contextlib import AsyncExitStack @@ -17,6 +27,18 @@ from nova.api import models +class RobotCellError(Exception): + """Base exception for all robot cell specific error""" + + +class RobotMotionError(RobotCellError): + """Robot can not move as requested""" + + +class RobotCellKeyError(KeyError): + pass + + class ConfigurablePeriphery: """A device which is configurable""" @@ -107,6 +129,29 @@ async def __aexit__(self, exc_type, exc_val, exc_tb): await self.close() +T = TypeVar("T") + + +class AsyncCallableDevice(Generic[T], Device): + """An awaitable external function or service in the robot cell""" + + async def __call__(self, *args, **kwargs) -> Awaitable[T]: + if not self._is_active: + raise ValueError("The device is not activated.") + return await self._call(*args) + + @abstractmethod + async def _call(self, key, *args) -> Awaitable[T]: + """The implementation of the call method. AbstractAwaitable guarantees that the device is activated. + + Args: + key: A key that represents the identifier of the external function or service that is called + *args: Parameters of the external callable + + Returns: the returned values of the external called function or service + """ + + ValueType = Union[int, str, bool, float, Pose] @@ -362,10 +407,6 @@ class ExecutionResult: recorded_trajectories: list[list[MotionState]] -class RobotCellKeyError(KeyError): - pass - - class RobotCell: """Access a simulated or real robot""" diff --git a/tests/core/test_io.py b/tests/core/test_io.py new file mode 100644 index 0000000..b516b2f --- /dev/null +++ b/tests/core/test_io.py @@ -0,0 +1,51 @@ +from nova.core.io import IOAccess, IOType, IOValueType +from nova import Nova +import pytest +# from decouple import config + +NOVA_API = "http://172.30.1.41" # config("NOVA_API") + + +@pytest.mark.skip("TODO: Setup integration tests") +@pytest.mark.asyncio +async def test_get_io_descriptions(): + nova = Nova(host=NOVA_API) + async with nova: + cell = nova.cell() + io = IOAccess(api_gateway=nova._api_client, cell=cell.cell_id, controller_id="ur") + io_descriptions = await io.get_io_descriptions() + assert len(io_descriptions) > 0 + filtered_io_descriptions = IOAccess.filter_io_descriptions( + io_descriptions, IOValueType.IO_VALUE_DIGITAL, IOType.IO_TYPE_INPUT + ) + assert len(filtered_io_descriptions) < len(io_descriptions) + + +@pytest.mark.skip("TODO: Setup integration tests") +@pytest.mark.asyncio +async def test_read(): + nova = Nova(host=NOVA_API) + async with nova: + cell = nova.cell() + io = IOAccess(api_gateway=nova._api_client, cell=cell.cell_id, controller_id="ur") + value1 = await io.read("tool_out[0]") + assert value1 is False + value2 = await io.read("digital_out[0]") + assert value2 is False + + +@pytest.mark.skip("TODO: Setup integration tests") +@pytest.mark.asyncio +async def test_write(): + nova = Nova(host=NOVA_API) + async with nova: + cell = nova.cell() + io = IOAccess(api_gateway=nova._api_client, cell=cell.cell_id, controller_id="ur") + value1 = await io.read("tool_out[0]") + assert value1 is False + await io.write("tool_out[0]", True) + value2 = await io.read("tool_out[0]") + assert value2 is True + await io.write("tool_out[0]", False) + value3 = await io.read("tool_out[0]") + assert value3 is False