feat(RPS-1206): implemented I/O access, read & write (#41)

Co-authored-by: cbiering <christoph.biering@wandelbots.com>

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__":

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__":

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__":

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__":

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__":

nova/core/controller.py

@@ -1,36 +1,61 @@
-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
     ):
         self._api_gateway = api_gateway
         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)

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,
+        )

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

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"""