diff --git a/cri_lib/__init__.py b/cri_lib/__init__.py index c57b55c..cca8f6b 100644 --- a/cri_lib/__init__.py +++ b/cri_lib/__init__.py @@ -2,7 +2,6 @@ .. include:: ../README.md """ - from .robot_state import ( RobotMode, KinematicsState, diff --git a/cri_lib/cri_controller.py b/cri_lib/cri_controller.py index 387d08b..bf2be76 100644 --- a/cri_lib/cri_controller.py +++ b/cri_lib/cri_controller.py @@ -13,6 +13,7 @@ from .cri_errors import CRICommandTimeOutError, CRIConnectionError +logger = logging.getLogger(__name__) class CRIController: """ @@ -96,7 +97,7 @@ def connect(self, host: str, port: int = 3920) -> bool: try: ip = socket.gethostbyname(host) self.sock.connect((ip, port)) - logging.debug("\t Robot connected: %s:%d", host, port) + logger.debug("\t Robot connected: %s:%d", host, port) self.connected = True # Start receiving commands @@ -108,12 +109,12 @@ def connect(self, host: str, port: int = 3920) -> bool: return True except ConnectionRefusedError: - logging.error( - f"Connection refused: Unable to connect to {self.ip_address}:{self.port}" + logger.error( + "Connection refused: Unable to connect to %s:%i", self.ip_address, self.port ) return False except Exception as e: - logging.error(f"An error occurred: {str(e)}") + logger.exception("An error occurred while attempting to connect.") return False def close(self) -> None: @@ -160,7 +161,7 @@ def _send_command( """ if not self.connected: - logging.error("Not connected. Use connect() to establish a connection.") + logger.error("Not connected. Use connect() to establish a connection.") raise CRIConnectionError( "Not connected. Use connect() to establish a connection." ) @@ -185,12 +186,12 @@ def _send_command( try: with self.socket_write_lock: self.sock.sendall(message.encode()) - logging.debug("Sent command: %s", message) + logger.debug("Sent command: %s", message) return command_counter except Exception as e: - logging.error(f"Failed to send command: {str(e)}") + logger.exception("Failed to send command.") if register_answer: with self.answer_events_lock: if fixed_answer_name is not None: @@ -213,7 +214,7 @@ def _bg_alivejog_thread(self) -> None: command = "ALIVEJOG 0 0 0 0 0 0 0 0 0" if self._send_command(command) is None: - logging.error("AliveJog Thread: Connection lost.") + logger.error("AliveJog Thread: Connection lost.") self.connected = False return @@ -233,7 +234,7 @@ def _bg_receive_thread(self) -> None: if recv_buffer == b"": self.connected = False - logging.error("Receive Thread: Connection lost.") + logger.error("Receive Thread: Connection lost.") return message_buffer.extend(recv_buffer) @@ -309,7 +310,7 @@ def _wait_for_answer( def _parse_message(self, message: str) -> None: """Internal function to parse a message. If an answer event is registered for a certain msg_id it is triggered.""" if "STATUS" not in message: - logging.debug("Received: %s", message) + logger.debug("Received: %s", message) if (notification := self.parser.parse_message(message)) is not None: if notification["answer"] == "status" and self.status_callback is not None: @@ -372,7 +373,7 @@ def reset(self) -> bool: f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in RESET command: %s", error_msg) + logger.debug("Error in RESET command: %s", error_msg) return False else: return True @@ -395,7 +396,7 @@ def enable(self) -> bool: f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in ENABLE command: %s", error_msg) + logger.debug("Error in ENABLE command: %s", error_msg) return False else: return True @@ -417,7 +418,7 @@ def disable(self) -> bool: f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in DISABLE command: %s", error_msg) + logger.debug("Error in DISABLE command: %s", error_msg) return False else: return True @@ -446,7 +447,7 @@ def set_active_control(self, active: bool) -> None: f"Active_{str(active).lower()}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in set active control command: %s", error_msg) + logger.debug("Error in set active control command: %s", error_msg) return False else: return True @@ -469,7 +470,7 @@ def zero_all_joints(self) -> bool: f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in SetJointsToZero command: %s", error_msg) + logger.debug("Error in SetJointsToZero command: %s", error_msg) return False else: return True @@ -490,7 +491,7 @@ def reference_all_joints(self) -> bool: if ( error_msg := self._wait_for_answer(f"{msg_id}", timeout=30.0) ) is not None: - logging.debug("Error in ReferenceAllJoints command: %s", error_msg) + logger.debug("Error in ReferenceAllJoints command: %s", error_msg) return False else: return True @@ -525,7 +526,7 @@ def reference_single_joint(self, joint: str) -> bool: if ( error_msg := self._wait_for_answer(f"{msg_id}", timeout=30.0) ) is not None: - logging.debug("Error in ReferenceSingleJoint command: %s", error_msg) + logger.debug("Error in ReferenceSingleJoint command: %s", error_msg) return False else: return True @@ -550,7 +551,7 @@ def get_referencing_info(self): "info_referencing", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in GetReferencingInfo command: %s", error_msg) + logger.debug("Error in GetReferencingInfo command: %s", error_msg) return False else: return True @@ -625,7 +626,7 @@ def move_joints( if ( error_msg := self._wait_for_answer(f"{msg_id}", timeout=30.0) ) is not None: - logging.debug("Error in Move Joints command: %s", error_msg) + logger.debug("Error in Move Joints command: %s", error_msg) return False if wait_move_finished: @@ -634,7 +635,7 @@ def move_joints( "EXECEND", timeout=move_finished_timeout ) ) is not None: - logging.debug("Exec Error in Move Joints command: %s", error_msg) + logger.debug("Exec Error in Move Joints command: %s", error_msg) return False return True @@ -681,7 +682,7 @@ def move_joints_relative( if ( error_msg := self._wait_for_answer(f"{msg_id}", timeout=30.0) ) is not None: - logging.debug("Error in Move Joints command: %s", error_msg) + logger.debug("Error in Move Joints command: %s", error_msg) return False if wait_move_finished: @@ -690,7 +691,7 @@ def move_joints_relative( "EXECEND", timeout=move_finished_timeout ) ) is not None: - logging.debug( + logger.debug( "Exec Error in Move Joints Relative command: %s", error_msg ) return False @@ -746,7 +747,7 @@ def move_cartesian( if ( error_msg := self._wait_for_answer(f"{msg_id}", timeout=30.0) ) is not None: - logging.debug("Error in Move Joints command: %s", error_msg) + logger.debug("Error in Move Joints command: %s", error_msg) return False if wait_move_finished: @@ -755,7 +756,7 @@ def move_cartesian( "EXECEND", timeout=move_finished_timeout ) ) is not None: - logging.debug("Exec Error in Move Cartesian command: %s", error_msg) + logger.debug("Exec Error in Move Cartesian command: %s", error_msg) return False return True @@ -808,7 +809,7 @@ def move_base_relative( if ( error_msg := self._wait_for_answer(f"{msg_id}", timeout=30.0) ) is not None: - logging.debug("Error in Move Joints command: %s", error_msg) + logger.debug("Error in Move Joints command: %s", error_msg) return False if wait_move_finished: @@ -817,7 +818,7 @@ def move_base_relative( "EXECEND", timeout=move_finished_timeout ) ) is not None: - logging.debug( + logger.debug( "Exec Error in Move BaseRelative command: %s", error_msg ) return False @@ -872,7 +873,7 @@ def move_tool_relative( if ( error_msg := self._wait_for_answer(f"{msg_id}", timeout=30.0) ) is not None: - logging.debug("Error in Move Joints command: %s", error_msg) + logger.debug("Error in Move Joints command: %s", error_msg) return False if wait_move_finished: @@ -881,7 +882,7 @@ def move_tool_relative( "EXECEND", timeout=move_finished_timeout ) ) is not None: - logging.debug("Exec Error in Move BaseTool command: %s", error_msg) + logger.debug("Exec Error in Move BaseTool command: %s", error_msg) return False return True @@ -903,7 +904,7 @@ def stop_move(self) -> None: if ( error_msg := self._wait_for_answer(f"{msg_id}", timeout=5.0) ) is not None: - logging.debug("Error in Move Stop command: %s", error_msg) + logger.debug("Error in Move Stop command: %s", error_msg) return False else: return True @@ -986,7 +987,7 @@ def set_motion_type(self, motion_type: MotionType): f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in MotionType command: %s", error_msg) + logger.debug("Error in MotionType command: %s", error_msg) return False else: return True @@ -1015,7 +1016,7 @@ def set_override(self, override: float): f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in Override command: %s", error_msg) + logger.debug("Error in Override command: %s", error_msg) return False else: return True @@ -1050,7 +1051,7 @@ def set_dout(self, id: int, value: bool): f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in DOUT command: %s", error_msg) + logger.debug("Error in DOUT command: %s", error_msg) return False else: return True @@ -1085,7 +1086,7 @@ def set_din(self, id: int, value: bool): f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in DIN command: %s", error_msg) + logger.debug("Error in DIN command: %s", error_msg) return False else: return True @@ -1120,7 +1121,7 @@ def set_global_signal(self, id: int, value: bool): f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in DIN command: %s", error_msg) + logger.debug("Error in DIN command: %s", error_msg) return False else: return True @@ -1149,7 +1150,7 @@ def load_programm(self, program_name: str) -> bool: f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in load_program command: %s", error_msg) + logger.debug("Error in load_program command: %s", error_msg) return False else: return True @@ -1178,7 +1179,7 @@ def load_logic_programm(self, program_name: str) -> bool: f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in load_logic_program command: %s", error_msg) + logger.debug("Error in load_logic_program command: %s", error_msg) return False else: return True @@ -1202,7 +1203,7 @@ def start_programm(self) -> bool: f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in start_program command: %s", error_msg) + logger.debug("Error in start_program command: %s", error_msg) return False else: return True @@ -1226,7 +1227,7 @@ def stop_programm(self) -> bool: f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in stop_program command: %s", error_msg) + logger.debug("Error in stop_program command: %s", error_msg) return False else: return True @@ -1250,7 +1251,7 @@ def pause_programm(self) -> bool: f"{msg_id}", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in pause_program command: %s", error_msg) + logger.debug("Error in pause_program command: %s", error_msg) return False else: return True @@ -1286,11 +1287,11 @@ def upload_file(self, path: str | Path, target_directory: str) -> bool: with open(file_path, "r") as fp: lines = [] while line := fp.readline(): - print(line) + logger.debug(line) lines.append(line) except OSError as e: - logging.error("Error reading %s: %s", str(Path), str(e)) + logger.error("Error reading %s: %s", str(Path), str(e)) return False command = f"CMD UploadFileInit {target_directory + '/' + str(file_path.name)} {len(lines)} 0" @@ -1338,7 +1339,7 @@ def can_send(self, msg_id: int, length: int, data: bytearray) -> None: data for CAN message always 8 bytes """ if not self.can_mode: - logging.debug("can_send: CAN mode not enabled") + logger.debug("can_send: CAN mode not enabled") return command = f"CANBridge Msg ID {msg_id} Len {length} Data " + " ".join( @@ -1358,7 +1359,7 @@ def can_receive( Returns a tuple of (msg_id, length, data) if a message was received or None if nothing was received within the timeout. """ if not self.can_mode: - logging.debug("can_receive: CAN mode not enabled") + logger.debug("can_receive: CAN mode not enabled") return try: @@ -1388,7 +1389,7 @@ def get_board_temperatures(self, blocking: bool = True, timeout: float | None = "info_boardtemp", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in GetBoardTemp command: %s", error_msg) + logger.debug("Error in GetBoardTemp command: %s", error_msg) return False else: return True @@ -1415,7 +1416,7 @@ def get_motor_temperatures(self, blocking: bool = True, timeout: float | None = "info_motortemp", timeout=self.DEFAULT_ANSWER_TIMEOUT ) ) is not None: - logging.debug("Error in GetMotorTemp command: %s", error_msg) + logger.debug("Error in GetMotorTemp command: %s", error_msg) return False else: return True diff --git a/cri_lib/cri_errors.py b/cri_lib/cri_errors.py index 5e3586f..4077803 100644 --- a/cri_lib/cri_errors.py +++ b/cri_lib/cri_errors.py @@ -1,12 +1,14 @@ class CRIError(Exception): - pass + """Base class for CRI-related errors.""" class CRIConnectionError(CRIError): + """Raised when there is a connection error.""" def __init__(self, message="Not connected to iRC or connection lost."): self.message = message super().__init__(self.message) class CRICommandTimeOutError(CRIError): + """Raised when a command times out.""" def __init__(self, message="Time out waiting for command response."): self.message = message super().__init__(self.message) \ No newline at end of file diff --git a/cri_lib/cri_protocol_parser.py b/cri_lib/cri_protocol_parser.py index 8b7d49c..00049b0 100644 --- a/cri_lib/cri_protocol_parser.py +++ b/cri_lib/cri_protocol_parser.py @@ -18,6 +18,7 @@ ReferencingAxisState, ) +logger = logging.getLogger(__name__) class CRIProtocolParser: """Class handling the parsing of CRI messages to the robot state.""" @@ -99,7 +100,7 @@ def parse_message(self, message: str) -> str | None: return self._parse_execerror(parts[3:-1]) case _: - logging.debug( + logger.debug( "Unknown message type %s received:\n%s", parts[2], " ".join(parts) ) return None @@ -274,7 +275,7 @@ def _parse_status(self, parameters: list[str]) -> None: segment_start_idx += 8 case _: - logging.debug( + logger.debug( "Unknown segment in status message: %s", parameters[segment_start_idx], ) @@ -353,7 +354,7 @@ def _parse_variables(self, parameters: list[str]) -> None: variables[parameters[idx + 1]] = PosVariable(*values) idx += 17 else: - logging.debug( + logger.debug( "Unknown variable type in VARIABLES message: %s", parameters[idx] ) idx += 1 @@ -401,7 +402,7 @@ def _parse_cmd(self, parameters: list[str]) -> str: self.robot_state.active_control = False return "Active_false" else: - logging.debug("Unknown Active state: %s", parameters[1]) + logger.debug("Unknown Active state: %s", parameters[1]) return None @@ -453,7 +454,7 @@ def _parse_message_message(self, parameters: list[str]) -> None: self.robot_state.gripper_type = gripper else: - logging.debug("MESSAGE: %s", " ".join(parameters)) + logger.debug("MESSAGE: %s", " ".join(parameters)) def _parse_can_bridge(self, parameters: list[str]) -> dict[str, any] | None: """Parses a can bridge message. diff --git a/examples/relative_move.py b/examples/relative_move.py index d5f68ff..7b97074 100644 --- a/examples/relative_move.py +++ b/examples/relative_move.py @@ -1,34 +1,44 @@ +import logging from cri_lib import CRIController +# 🔹 Configure logging +logging.basicConfig(level=logging.DEBUG, format="%(asctime)s - %(levelname)s - %(message)s") +logger = logging.getLogger(__name__) + # CRIController is the main interface for controlling the iRC controller = CRIController() -#connect to default iRC IP -#controller.connect("192.168.3.11") +# Connect to default iRC IP +# controller.connect("192.168.3.11") if not controller.connect("127.0.0.1", 3921): - print("Unable to connect") + logger.error("Unable to connect to iRC! Ensure the simulator is running.") quit() -#acquire active control. +# Acquire active control. controller.set_active_control(True) -print("enable") -#enable motors +logger.info("Acquired active control.") + +# Enable motors +logger.info("Enabling motors...") controller.enable() -print("waiting") -#wait until kinematics are ready to move +# Wait until kinematics are ready +logger.info("Waiting for kinematics to be ready...") controller.wait_for_kinematics_ready(10) controller.set_override(100.0) -print("move") -#relative move x,y,z +20mm and back -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) -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) +# 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) - -#Disable motors and disconnect +# Disable motors and disconnect +logger.info("Disabling motors and disconnecting...") controller.disable() -controller.close() \ No newline at end of file +controller.close() + +logger.info("Script execution completed successfully.") diff --git a/examples/start_program.py b/examples/start_program.py index 0ada83c..a25885f 100644 --- a/examples/start_program.py +++ b/examples/start_program.py @@ -1,66 +1,70 @@ +import logging from time import sleep from cri_lib import CRIController +# 🔹 Configure logging +logging.basicConfig(level=logging.DEBUG, format="%(asctime)s - %(levelname)s - %(message)s") +logger = logging.getLogger(__name__) # CRIController is the main interface for controlling the iRC controller = CRIController() #connect to default iRC IP #if not controller.connect("127.0.0.1", 3921): if not controller.connect("192.168.3.11"): - print("Unable to connect") + logger.error("Unable to connect") quit() #acquire active control. controller.set_active_control(True) -print("enable") +logger.info("Enabling motors...") #enable motors controller.enable() -print("waiting") +logger.info("Waiting for kinematics to be ready...") #wait until kinematics are ready to move controller.wait_for_kinematics_ready(10) controller.set_override(50.0) -print("Load program") -if not controller.load_programm("ReBeL_6DOF_01_Tischtest.xml"): - print("unable to load programm") +logger.info("Load program") +if not controller.load_programm("ReBeL_MoveToZero.xml"): + logger.error("unable to load programm") controller.disable() controller.close() quit() -print("Start program") +logger.info("Start program") if not controller.start_programm(): - print("Unable to start programm") + logger.error("Unable to start programm") controller.disable() controller.close() quit() sleep(5) -print("Pause program") +logger.info("Pause program") if not controller.pause_programm(): - print("Unable to pause programm") + logger.error("Unable to pause programm") controller.disable() controller.close() quit() sleep(5) -print("Start programm again") +logger.info("Start programm again") if not controller.start_programm(): - print("Unable to start programm") + logger.error("Unable to start programm") controller.disable() controller.close() quit() sleep(5) -print("Stop program") +logger.info("Stop program") if not controller.stop_programm(): - print("Unable to stop programm") + logger.error("Unable to stop programm") controller.disable() controller.close() quit()