From 1a3f0f4a18b0920b9417231b3d56fef7914df7e9 Mon Sep 17 00:00:00 2001 From: Michael Osthege Date: Fri, 6 Feb 2026 23:08:59 +0100 Subject: [PATCH 1/5] Make `connect` raise on errors --- cri_lib/cri_controller.py | 31 ++++++++++++++++--------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/cri_lib/cri_controller.py b/cri_lib/cri_controller.py index 3f8f135..345244d 100644 --- a/cri_lib/cri_controller.py +++ b/cri_lib/cri_controller.py @@ -6,7 +6,7 @@ from pathlib import Path from queue import Empty, Queue from time import sleep, time -from typing import Any, Callable +from typing import Any, Callable, Literal from .cri_errors import CRICommandTimeOutError, CRIConnectionError from .cri_protocol_parser import CRIProtocolParser @@ -45,7 +45,7 @@ def __init__(self) -> None: self.parser = CRIProtocolParser(self.robot_state, self.robot_state_lock) self.connected = False - self.sock: socket.socket | None = None + self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.socket_write_lock = threading.Lock() self.can_mode: bool = False @@ -85,7 +85,7 @@ def connect( port: int = 3920, application_name: str = "CRI-Python-Lib", application_version: str = "0-0-0-0", - ) -> bool: + ) -> Literal[True]: """ Connect to iRC. @@ -103,16 +103,21 @@ def connect( Returns ------- bool - True if connected - False if not connected + True if connected. + Otherwise an exception is raised. + Raises + ------ + CRIConnectionError + When already connected or connection fails. """ - self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + if self.connected: + raise CRIConnectionError("Already connected.") self.sock.settimeout(0.1) # Set a timeout of 0.1 seconds try: ip = socket.gethostbyname(host) self.sock.connect((ip, port)) - logger.debug("\t Robot connected: %s:%d", host, port) + # Mark as connected before starting threads self.connected = True # Start receiving commands @@ -124,19 +129,15 @@ def connect( hello_msg = f'INFO Hello "{application_name}" {application_version} {datetime.now(timezone.utc).strftime(format="%Y-%m-%dT%H:%M:%S")}' self._send_command(hello_msg) - + logger.debug("Connected to %s:%d", host, port) return True except ConnectionRefusedError: - logger.error( - "Connection refused: Unable to connect to %s:%i", - host, - port, + raise CRIConnectionError( + f"Connection refused: Unable to connect to {host}:{port}" ) - return False except Exception as e: - logger.exception("An error occurred while attempting to connect.") - return False + raise CRIConnectionError("Failed to connect to iRC.") from e def close(self) -> None: """ From 93d73d21c2177f8a982ab1aaf54cf8da698131d6 Mon Sep 17 00:00:00 2001 From: Michael Osthege Date: Fri, 6 Feb 2026 23:17:04 +0100 Subject: [PATCH 2/5] Move read-only methods up to prepare class split --- cri_lib/cri_controller.py | 148 +++++++++++++++++++------------------- 1 file changed, 74 insertions(+), 74 deletions(-) diff --git a/cri_lib/cri_controller.py b/cri_lib/cri_controller.py index 345244d..be09b83 100644 --- a/cri_lib/cri_controller.py +++ b/cri_lib/cri_controller.py @@ -393,6 +393,80 @@ def callback(state: RobotState) """ self.status_callback = callback + def wait_for_kinematics_ready(self, timeout: float = 30) -> bool: + """Wait until drive state is indicated as ready. + + Parameters + ---------- + timeout : float + maximum time to wait in seconds + + Returns + ------- + bool + `True`if drives are ready, `False` if not ready or timeout + """ + start_time = time() + new_timeout = timeout + while new_timeout > 0.0: + self.wait_for_status_update(timeout=new_timeout) + if (self.robot_state.kinematics_state == KinematicsState.NO_ERROR) and ( + self.robot_state.combined_axes_error == "NoError" + ): + return True + + new_timeout = timeout - (time() - start_time) + + return False + + def get_board_temperatures( + self, + blocking: bool = True, + timeout: float | None = DEFAULT, # type: ignore + ) -> bool: + """Receive motor controller PCB temperatures and save in robot state + + Parameters + ---------- + blocking: bool + wait for response, always returns True if not waiting + + timeout: float | None + timeout for waiting in seconds or None for infinite waiting + """ + self._send_command("SYSTEM GetBoardTemp", True, "info_boardtemp") + if ( + error_msg := self._wait_for_answer("info_boardtemp", timeout=timeout) + ) is not None: + logger.debug("Error in GetBoardTemp command: %s", error_msg) + return False + else: + return True + + def get_motor_temperatures( + self, + blocking: bool = True, + timeout: float | None = DEFAULT, # type: ignore + ) -> bool: + """Receive motor temperatures and save in robot state + + Parameters + ---------- + blocking: bool + wait for response, always returns True if not waiting + + timeout: float | None + timeout for waiting in seconds or None for infinite waiting + """ + self._send_command("SYSTEM GetMotorTemp", True, "info_motortemp") + if ( + error_msg := self._wait_for_answer("info_motortemp", timeout=timeout) + ) is not None: + logger.debug("Error in GetMotorTemp command: %s", error_msg) + return False + else: + return True + def reset(self) -> bool: """Reset robot @@ -541,32 +615,6 @@ def get_referencing_info(self): else: return True - def wait_for_kinematics_ready(self, timeout: float = 30) -> bool: - """Wait until drive state is indicated as ready. - - Parameters - ---------- - timeout : float - maximum time to wait in seconds - - Returns - ------- - bool - `True`if drives are ready, `False` if not ready or timeout - """ - start_time = time() - new_timeout = timeout - while new_timeout > 0.0: - self.wait_for_status_update(timeout=new_timeout) - if (self.robot_state.kinematics_state == KinematicsState.NO_ERROR) and ( - self.robot_state.combined_axes_error == "NoError" - ): - return True - - new_timeout = timeout - (time() - start_time) - - return False - def move_joints( self, A1: float, @@ -1323,54 +1371,6 @@ def can_receive( return item - def get_board_temperatures( - self, - blocking: bool = True, - timeout: float | None = DEFAULT, # type: ignore - ) -> bool: - """Receive motor controller PCB temperatures and save in robot state - - Parameters - ---------- - blocking: bool - wait for response, always returns True if not waiting - - timeout: float | None - timeout for waiting in seconds or None for infinite waiting - """ - self._send_command("SYSTEM GetBoardTemp", True, "info_boardtemp") - if ( - error_msg := self._wait_for_answer("info_boardtemp", timeout=timeout) - ) is not None: - logger.debug("Error in GetBoardTemp command: %s", error_msg) - return False - else: - return True - - def get_motor_temperatures( - self, - blocking: bool = True, - timeout: float | None = DEFAULT, # type: ignore - ) -> bool: - """Receive motor temperatures and save in robot state - - Parameters - ---------- - blocking: bool - wait for response, always returns True if not waiting - - timeout: float | None - timeout for waiting in seconds or None for infinite waiting - """ - self._send_command("SYSTEM GetMotorTemp", True, "info_motortemp") - if ( - error_msg := self._wait_for_answer("info_motortemp", timeout=timeout) - ) is not None: - logger.debug("Error in GetMotorTemp command: %s", error_msg) - return False - else: - return True - # Monkey patch to maintain backward compatibility CRIController.MotionType = MotionType # type: ignore From 0dc71101eb7f2f0e07f620e8e876bcdcbe2befdd Mon Sep 17 00:00:00 2001 From: Michael Osthege Date: Fri, 6 Feb 2026 23:22:47 +0100 Subject: [PATCH 3/5] Split class introducing read-only `CRIClient` This way one can create a client with only non-dangerous read methods. --- cri_lib/__init__.py | 2 +- cri_lib/cri_controller.py | 80 ++++++++++++++++++++++++++------------- 2 files changed, 54 insertions(+), 28 deletions(-) diff --git a/cri_lib/__init__.py b/cri_lib/__init__.py index eec1630..1f972c2 100644 --- a/cri_lib/__init__.py +++ b/cri_lib/__init__.py @@ -2,7 +2,7 @@ .. include:: ../README.md """ -from .cri_controller import CRIController, MotionType +from .cri_controller import CRIClient, CRIController, MotionType from .cri_errors import CRICommandTimeOutError, CRIConnectionError, CRIError from .cri_protocol_parser import CRIProtocolParser from .robot_state import ( diff --git a/cri_lib/cri_controller.py b/cri_lib/cri_controller.py index be09b83..d5d11fd 100644 --- a/cri_lib/cri_controller.py +++ b/cri_lib/cri_controller.py @@ -28,17 +28,18 @@ class MotionType(Enum): Platform = "Platform" -class CRIController: - """ - Class implementing the CRI network protocol for igus Robot Control. - """ +class CRIClient: + """Client with implementations for read-only communication.""" ALIVE_JOG_INTERVAL_SEC = 0.2 - ACTIVE_JOG_INTERVAL_SEC = 0.02 RECEIVE_TIMEOUT_SEC = 5 DEFAULT_ANSWER_TIMEOUT = 10.0 def __init__(self) -> None: + """Create a ``CRIClient`` without connecting it yet. + + Call ``connect`` to connect and start receiving data. + """ self.robot_state: RobotState = RobotState() self.robot_state_lock = threading.Lock() @@ -52,6 +53,7 @@ def __init__(self) -> None: self.can_queue: Queue = Queue() self.jog_thread = threading.Thread(target=self._bg_alivejog_thread, daemon=True) + self.jog_intervall = self.ALIVE_JOG_INTERVAL_SEC self.receive_thread = threading.Thread( target=self._bg_receive_thread, daemon=True ) @@ -64,21 +66,6 @@ def __init__(self) -> None: self.status_callback: Callable | None = None - self.live_jog_active: bool = False - self.jog_intervall = self.ALIVE_JOG_INTERVAL_SEC - self.jog_speeds: dict[str, float] = { - "A1": 0.0, - "A2": 0.0, - "A3": 0.0, - "A4": 0.0, - "A5": 0.0, - "A6": 0.0, - "E1": 0.0, - "E2": 0.0, - "E3": 0.0, - } - self.jog_speeds_lock = threading.Lock() - def connect( self, host: str, @@ -171,6 +158,8 @@ def _send_command( ) -> int: """Sends the given command to iRC. + The method is marked private because technically it can be used to send control commands as well. + Parameters ---------- command : str @@ -232,13 +221,7 @@ def _bg_alivejog_thread(self) -> None: Background Thread sending alivejog messages to keep connection alive. """ while self.connected: - if self.live_jog_active: - with self.jog_speeds_lock: - command = f"ALIVEJOG {self.jog_speeds['A1']} {self.jog_speeds['A2']} {self.jog_speeds['A3']} {self.jog_speeds['A4']} {self.jog_speeds['A5']} {self.jog_speeds['A6']} {self.jog_speeds['E1']} {self.jog_speeds['E2']} {self.jog_speeds['E3']}" - else: - command = "ALIVEJOG 0 0 0 0 0 0 0 0 0" - - if self._send_command(command) is None: + if self._send_command("ALIVEJOG 0 0 0 0 0 0 0 0 0") is None: logger.error("AliveJog Thread: Connection lost.") self.connected = False return @@ -467,6 +450,49 @@ def get_motor_temperatures( else: return True + +class CRIController(CRIClient): + """A connected ``CRIClient`` with control capabilities.""" + + ACTIVE_JOG_INTERVAL_SEC = 0.02 + + def __init__(self) -> None: + self.live_jog_active: bool = False + self.jog_intervall = self.ALIVE_JOG_INTERVAL_SEC + self.jog_speeds: dict[str, float] = { + "A1": 0.0, + "A2": 0.0, + "A3": 0.0, + "A4": 0.0, + "A5": 0.0, + "A6": 0.0, + "E1": 0.0, + "E2": 0.0, + "E3": 0.0, + } + self.jog_speeds_lock = threading.Lock() + super().__init__() + + def _bg_alivejog_thread(self) -> None: + """Overrides the ``CRIClient._bg_alivejog_thread`` to send alivejog messages with possibly nonzero jog speeds.""" + while self.connected: + if self.live_jog_active: + with self.jog_speeds_lock: + command = f"ALIVEJOG {self.jog_speeds['A1']} {self.jog_speeds['A2']} {self.jog_speeds['A3']} {self.jog_speeds['A4']} {self.jog_speeds['A5']} {self.jog_speeds['A6']} {self.jog_speeds['E1']} {self.jog_speeds['E2']} {self.jog_speeds['E3']}" + else: + command = "ALIVEJOG 0 0 0 0 0 0 0 0 0" + + if self._send_command(command) is None: + logger.error("AliveJog Thread: Connection lost.") + self.connected = False + return + + sleep(self.jog_intervall) + + def send_command(self, command, register_answer=False, fixed_answer_name=None): + """Wraps the superclass method to make it public.""" + return super()._send_command(command, register_answer, fixed_answer_name) + def reset(self) -> bool: """Reset robot From 4e6ee04cd7dc7ad4b56d3bbf5852d39f1812d479 Mon Sep 17 00:00:00 2001 From: Michael Osthege Date: Fri, 6 Feb 2026 22:10:48 +0100 Subject: [PATCH 4/5] Introduce `CRICommandError` --- cri_lib/__init__.py | 7 ++++++- cri_lib/cri_errors.py | 8 ++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/cri_lib/__init__.py b/cri_lib/__init__.py index 1f972c2..80b3fcf 100644 --- a/cri_lib/__init__.py +++ b/cri_lib/__init__.py @@ -3,7 +3,12 @@ """ from .cri_controller import CRIClient, CRIController, MotionType -from .cri_errors import CRICommandTimeOutError, CRIConnectionError, CRIError +from .cri_errors import ( + CRICommandError, + CRICommandTimeOutError, + CRIConnectionError, + CRIError, +) from .cri_protocol_parser import CRIProtocolParser from .robot_state import ( ErrorStates, diff --git a/cri_lib/cri_errors.py b/cri_lib/cri_errors.py index 4069dc5..5c665fd 100644 --- a/cri_lib/cri_errors.py +++ b/cri_lib/cri_errors.py @@ -10,6 +10,14 @@ def __init__(self, message="Not connected to iRC or connection lost."): super().__init__(self.message) +class CRICommandError(CRIError): + """Raised when a command fails to execute properly.""" + + def __init__(self, message="Command execution failed."): + self.message = message + super().__init__(self.message) + + class CRICommandTimeOutError(CRIError): """Raised when a command times out.""" From af4e9379c94459d91780dc65115ddc7a1e2bdf8d Mon Sep 17 00:00:00 2001 From: Michael Osthege Date: Fri, 6 Feb 2026 23:41:32 +0100 Subject: [PATCH 5/5] Introduce `CRIConnector` for async client/control sessions --- cri_lib/__init__.py | 2 +- cri_lib/cri_controller.py | 75 +++++++++++++++++++++++++++++++- examples/asynchronous_control.py | 73 +++++++++++++++++++++++++++++++ 3 files changed, 148 insertions(+), 2 deletions(-) create mode 100644 examples/asynchronous_control.py diff --git a/cri_lib/__init__.py b/cri_lib/__init__.py index 80b3fcf..5aaf958 100644 --- a/cri_lib/__init__.py +++ b/cri_lib/__init__.py @@ -2,7 +2,7 @@ .. include:: ../README.md """ -from .cri_controller import CRIClient, CRIController, MotionType +from .cri_controller import CRIClient, CRIConnector, CRIController, MotionType from .cri_errors import ( CRICommandError, CRICommandTimeOutError, diff --git a/cri_lib/cri_controller.py b/cri_lib/cri_controller.py index d5d11fd..5a7dd65 100644 --- a/cri_lib/cri_controller.py +++ b/cri_lib/cri_controller.py @@ -1,6 +1,8 @@ +import contextlib import logging import socket import threading +from collections.abc import AsyncIterator from datetime import datetime, timezone from enum import Enum from pathlib import Path @@ -8,7 +10,7 @@ from time import sleep, time from typing import Any, Callable, Literal -from .cri_errors import CRICommandTimeOutError, CRIConnectionError +from .cri_errors import CRICommandError, CRICommandTimeOutError, CRIConnectionError from .cri_protocol_parser import CRIProtocolParser from .robot_state import KinematicsState, RobotState @@ -1400,3 +1402,74 @@ def can_receive( # Monkey patch to maintain backward compatibility CRIController.MotionType = MotionType # type: ignore + + +class CRIConnector: + """Factory providing context managers for connecting with clean resource lifecycle management. + + The context managers will yield ``CRIClient`` or ``CRIController`` instances + and ensure that the connection is properly closed and resources disposed when the context is exited. + """ + + def __init__( + self, + host: str, + port: int = 3920, + application_name: str = "CRI-Python-Lib", + application_version: str = "0-0-0-0", + ) -> None: + """Create a factory for active or passive connection to the robot controller. + + Parameters + ---------- + host : str + IP address or hostname of iRC + port : int + port of iRC + application_name : str + optional name of your application sent to controller + application_version: str + optional version of your application sent to controller + """ + # Remember connection parameters so that context entry methods don't need them. + self.host = host + self.port = port + self.application_name = application_name + self.application_version = application_version + super().__init__() + + @contextlib.asynccontextmanager + async def observe(self) -> AsyncIterator[CRIClient]: + """Establish connection for reading robot state.""" + client = CRIClient() + try: + client.connect( + self.host, self.port, self.application_name, self.application_version + ) + yield client + # Graceful context exit; nothing to do other than disconnect in finally block. + finally: + client.close() + return + + @contextlib.asynccontextmanager + async def control(self) -> AsyncIterator[CRIController]: + """Establish connection for controlling robot state.""" + controller = CRIController() + try: + controller.connect( + self.host, self.port, self.application_name, self.application_version + ) + # Take active control + if not controller.set_active_control(True): + raise CRICommandError("Failed to acquire active control.") + if not controller.enable(): + raise CRICommandError("Failed to enable robot.") + if not controller.wait_for_kinematics_ready(10): + raise CRICommandError("Kinematics not ready.") + yield controller + # Graceful context exit: give up control and disable robot, then disconnect in finally block. + controller.disable() + finally: + controller.close() + return diff --git a/examples/asynchronous_control.py b/examples/asynchronous_control.py new file mode 100644 index 0000000..16ca9df --- /dev/null +++ b/examples/asynchronous_control.py @@ -0,0 +1,73 @@ +"""Example of asynchronous control of an iRC robot. + +This is relevant for applications that need to perform other tasks concurrently while controlling the robot, +for example monitoring other inputs, controlling other actuators, or coordinating with other systems. +""" + +import asyncio +import logging + +from cri_lib import CRIConnector + +logging.basicConfig( + level=logging.DEBUG, format="%(asctime)s - %(levelname)s - %(message)s" +) +logger = logging.getLogger(__name__) + + +async def main(): + # The connector creates passive or active control sessions with proper resource management. + connector = CRIConnector( + host="127.0.0.1", + port=3921, + ) + + # connect asynchronously + async with connector.observe() as client: + logger.info("Current state is: %s", client.robot_state) + # disconnect automatically when exiting the context + + # connect and take control + async with connector.control() as controller: + controller.set_override(100.0) + + # Perform relative movement + logger.info("Moving base relative: +20mm in X, Y, Z...") + controller.move_base_relative( + 20.0, + 20.0, + 20.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 10.0, + wait_move_finished=True, + move_finished_timeout=1000, + ) + + logger.info("Moving back: -20mm in X, Y, Z...") + controller.move_base_relative( + -20.0, + -20.0, + -20.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 10.0, + wait_move_finished=True, + move_finished_timeout=1000, + ) + # disconnect and release control automatically by exiting the context + + logger.info("Script execution completed successfully.") + return + + +if __name__ == "__main__": + asyncio.run(main())