1- from enum import Enum
21import logging
32import socket
43import threading
5- from time import sleep , time
4+ from enum import Enum
65from pathlib import Path
7- from typing import Callable
8- from queue import Queue , Empty
9-
10- from .robot_state import RobotState , KinematicsState
11-
12- from .cri_protocol_parser import CRIProtocolParser
6+ from queue import Empty , Queue
7+ from time import sleep , time
8+ from typing import Any , Callable
139
1410from .cri_errors import CRICommandTimeOutError , CRIConnectionError
11+ from .cri_protocol_parser import CRIProtocolParser
12+ from .robot_state import KinematicsState , RobotState
1513
1614logger = logging .getLogger (__name__ )
1715
@@ -34,14 +32,14 @@ class MotionType(Enum):
3432 CartTool = "CartTool"
3533 Platform = "Platform"
3634
37- def __init__ (self ):
35+ def __init__ (self ) -> None :
3836 self .robot_state : RobotState = RobotState ()
3937 self .robot_state_lock = threading .Lock ()
4038
4139 self .parser = CRIProtocolParser (self .robot_state , self .robot_state_lock )
4240
4341 self .connected = False
44- self .sock = None
42+ self .sock : socket . socket | None = None
4543 self .socket_write_lock = threading .Lock ()
4644
4745 self .can_mode : bool = False
@@ -58,7 +56,7 @@ def __init__(self):
5856 self .answer_events : dict [str , threading .Event ] = {}
5957 self .error_messages : dict [str , str ] = {}
6058
61- self .status_callback = None
59+ self .status_callback : Callable | None = None
6260
6361 self .live_jog_active : bool = False
6462 self .jog_intervall = self .ALIVE_JOG_INTERVAL_SEC
@@ -125,7 +123,7 @@ def close(self) -> None:
125123 Close network connection. Might block for a while waiting for the threads to finish.
126124 """
127125
128- if not self .connected :
126+ if not self .connected or self . sock is None :
129127 return
130128
131129 self ._send_command ("QUIT" )
@@ -163,7 +161,7 @@ def _send_command(
163161 If the command was sent the message_id gets returned or None if there was an error.
164162
165163 """
166- if not self .connected :
164+ if not self .connected or self . sock is None :
167165 logger .error ("Not connected. Use connect() to establish a connection." )
168166 raise CRIConnectionError (
169167 "Not connected. Use connect() to establish a connection."
@@ -208,7 +206,6 @@ def _bg_alivejog_thread(self) -> None:
208206 """
209207 Background Thread sending alivejog messages to keep connection alive.
210208 """
211-
212209 while self .connected :
213210 if self .live_jog_active :
214211 with self .jog_speeds_lock :
@@ -227,6 +224,10 @@ def _bg_receive_thread(self) -> None:
227224 """
228225 Background thread receiving data and parsing it to the robot state.
229226 """
227+ if self .sock is None :
228+ logger .error ("Receive Thread: Not connected." )
229+ return
230+
230231 message_buffer = bytearray ()
231232
232233 while self .connected :
@@ -291,7 +292,7 @@ def _wait_for_answer(
291292
292293 with self .answer_events_lock :
293294 if message_id not in self .answer_events :
294- return False
295+ return None
295296 wait_event = self .answer_events [message_id ]
296297
297298 # prevent deadlock through answer_events_lock
@@ -326,8 +327,8 @@ def _parse_message(self, message: str) -> None:
326327 msg_id = notification ["answer" ]
327328
328329 if msg_id in self .answer_events :
329- if "error" in notification :
330- self .error_messages [msg_id ] = notification [ "error" ]
330+ if ( error_msg := notification . get ( "error" , None )) is not None :
331+ self .error_messages [msg_id ] = error_msg
331332
332333 self .answer_events [msg_id ].set ()
333334
@@ -428,7 +429,7 @@ def disable(self) -> bool:
428429 else :
429430 return False
430431
431- def set_active_control (self , active : bool ) -> None :
432+ def set_active_control (self , active : bool ) -> bool :
432433 """Acquire or return active control of robot
433434
434435 Parameters
@@ -480,7 +481,7 @@ def zero_all_joints(self) -> bool:
480481 else :
481482 return False
482483
483- def reference_all_joints (self ) -> bool :
484+ def reference_all_joints (self , * , timeout : float = 30 ) -> bool :
484485 """Reference all joints. Long timout of 30 seconds.
485486
486487 Returns
@@ -492,7 +493,7 @@ def reference_all_joints(self) -> bool:
492493
493494 if (msg_id := self ._send_command ("CMD ReferenceAllJoints" , True )) is not None :
494495 if (
495- error_msg := self ._wait_for_answer (f"{ msg_id } " , timeout = 30.0 )
496+ error_msg := self ._wait_for_answer (f"{ msg_id } " , timeout = timeout )
496497 ) is not None :
497498 logger .debug ("Error in ReferenceAllJoints command: %s" , error_msg )
498499 return False
@@ -501,7 +502,7 @@ def reference_all_joints(self) -> bool:
501502 else :
502503 return False
503504
504- def reference_single_joint (self , joint : str ) -> bool :
505+ def reference_single_joint (self , joint : str , * , timeout : float = 30 ) -> bool :
505506 """Reference a single joint. Long timout of 30 seconds.
506507
507508 Parameters
@@ -527,7 +528,7 @@ def reference_single_joint(self, joint: str) -> bool:
527528 msg_id := self ._send_command (f"CMD ReferenceSingleJoint { joint_msg } " , True )
528529 ) is not None :
529530 if (
530- error_msg := self ._wait_for_answer (f"{ msg_id } " , timeout = 30.0 )
531+ error_msg := self ._wait_for_answer (f"{ msg_id } " , timeout = timeout )
531532 ) is not None :
532533 logger .debug ("Error in ReferenceSingleJoint command: %s" , error_msg )
533534 return False
@@ -561,7 +562,7 @@ def get_referencing_info(self):
561562 else :
562563 return False
563564
564- def wait_for_kinematics_ready (self , timeout : float | None = 30 ) -> bool :
565+ def wait_for_kinematics_ready (self , timeout : float = 30 ) -> bool :
565566 """Wait until drive state is indicated as ready.
566567
567568 Parameters
@@ -955,7 +956,7 @@ def move_tool_relative(
955956 else :
956957 return False
957958
958- def stop_move (self ) -> None :
959+ def stop_move (self ) -> bool :
959960 """Stop movement
960961
961962 Returns
@@ -1374,6 +1375,7 @@ def upload_file(self, path: str | Path, target_directory: str) -> bool:
13741375
13751376 if self ._send_command (command , True ) is None :
13761377 return False
1378+ return True
13771379
13781380 def enable_can_bridge (self , enabled : bool ) -> None :
13791381 """Enables or diables CAN bridge mode. All other functions are disabled in CAN bridge mode.
@@ -1415,7 +1417,7 @@ def can_send(self, msg_id: int, length: int, data: bytearray) -> None:
14151417
14161418 def can_receive (
14171419 self , blocking : bool = True , timeout : float | None = None
1418- ) -> dict [str , any ] | None :
1420+ ) -> dict [str , Any ] | None :
14191421 """Receive CAN message in CAN bridge mode from the recveive queue.
14201422
14211423 Returns
@@ -1425,7 +1427,7 @@ def can_receive(
14251427 """
14261428 if not self .can_mode :
14271429 logger .debug ("can_receive: CAN mode not enabled" )
1428- return
1430+ return None
14291431
14301432 try :
14311433 item = self .can_queue .get (blocking , timeout )
0 commit comments