#!/usr/bin/env python
"""
K.NAKADA, kengo.nakada@gmail.com
"""
from collections import deque
from dataclasses import dataclass
import socket
import struct
import threading
import time
from typing import Any, Dict, List, Optional, Sequence, Tuple, Union
[docs]
@dataclass
class UrError:
_ROBOT_MODE_NAMES = {
-1: "NO_CONTROLLER",
0: "DISCONNECTED",
1: "CONFIRM_SAFETY",
2: "BOOTING",
3: "POWER_OFF",
4: "POWER_ON",
5: "IDLE",
6: "BACKDRIVE",
7: "RUNNING",
8: "UPDATING_FIRMWARE",
}
_ROBOT_MODE_DESCRIPTIONS = {
-1: "RTDE からコントローラ状態を取得できない。ネットワーク、RTDE サービス、または接続タイミングを確認する",
0: "RTDE から見てロボットコントローラが切断状態。IP 到達性と UR 側サービス状態を確認する",
1: "TP 側で安全確認待ち。外部制御前に TP 上の安全確認操作が必要な可能性がある",
2: "ロボット制御系が起動中。POWER_OFF/POWER_ON/IDLE/RUNNING への状態遷移を待つ",
3: "コントローラと TP は起動しているが、ロボット駆動系が Power Off。TP/コントローラ電源 OFF ではない。Dashboard powerOn() の対象",
4: "ロボット駆動系は Power On。ブレーキ解除が未完了なら Dashboard brakeRelease() の対象",
5: "アイドル状態。ロボット駆動系は起動済み。RTDE control script を開始できる候補状態",
6: "Backdrive 状態。手動誘導状態のため通常の外部移動指令は不可",
7: "実行中。UR プログラムまたは RTDE control script が動作中",
8: "ファームウェア更新中。外部制御不可",
}
_SAFETY_MODE_NAMES = {
1: "SAFETY_MODE_NORMAL",
2: "SAFETY_MODE_REDUCED",
3: "SAFETY_MODE_PROTECTIVE_STOP",
4: "SAFETY_MODE_RECOVERY",
5: "SAFETY_MODE_SAFEGUARD_STOP",
6: "SAFETY_MODE_SYSTEM_EMERGENCY_STOP",
7: "SAFETY_MODE_ROBOT_EMERGENCY_STOP",
8: "SAFETY_MODE_VIOLATION",
9: "SAFETY_MODE_FAULT",
10: "SAFETY_MODE_VALIDATE_JOINT_ID",
11: "SAFETY_MODE_UNDEFINED_SAFETY_MODE",
}
_SAFETY_MODE_DESCRIPTIONS = {
1: "通常安全状態。安全停止・Safety Fault は検出されていない",
2: "Reduced mode。安全設定により速度・力などが制限されている",
3: "Protective Stop。原因確認後、closeSafetyPopup() と unlockProtectiveStop() の対象",
4: "Recovery Mode。安全復帰用の低速/手動復帰状態",
5: "Safeguard Stop。安全扉、ライトカーテン、外部安全入力などが停止要求中",
6: "System Emergency Stop。システム側非常停止が有効",
7: "Robot Emergency Stop。ロボット側非常停止が有効",
8: "Safety Violation。安全制限違反。原因除去後 restartSafety() の対象",
9: "Safety Fault。安全系異常。原因除去後 restartSafety()、続いて powerOn()/brakeRelease() の対象",
10: "Validate Joint ID。関節 ID 確認状態。通常の外部制御状態ではない",
11: "未定義の安全モード。状態値の解釈不可",
}
_SAFETY_STATUS_BIT_NAMES = {
0: "normal_mode",
1: "reduced_mode",
2: "protective_stopped",
3: "recovery_mode",
4: "safeguard_stopped",
5: "system_emergency_stopped",
6: "robot_emergency_stopped",
7: "emergency_stopped",
8: "violation",
9: "fault",
10: "stopped_due_to_safety",
}
_SAFETY_STATUS_BIT_DESCRIPTIONS = {
"normal_mode": "通常安全状態",
"reduced_mode": "縮退安全モード",
"protective_stopped": "Protective Stop 発生中",
"recovery_mode": "Recovery Mode 発生中",
"safeguard_stopped": "Safeguard Stop 発生中",
"system_emergency_stopped": "システム側非常停止発生中",
"robot_emergency_stopped": "ロボット側非常停止発生中",
"emergency_stopped": "非常停止発生中",
"violation": "Safety Violation 発生中",
"fault": "Safety Fault 発生中",
"stopped_due_to_safety": "安全機能により停止中",
}
ROBOT_STATUS_BIT_DESCRIPTIONS = {
"power_on": "ロボット駆動系（サーボ電源）が ON。TP/コントローラ電源の意味ではない",
"program_running": "UR プログラムまたは RTDE 制御スクリプトが実行中",
"teach_button_pressed": "TP の teach button が押下中",
"power_button_pressed": "TP/操作画面上の power button が押下中",
}
_SAFETY_ERROR_PRIORITIES = (
"system_emergency_stopped",
"robot_emergency_stopped",
"emergency_stopped",
"fault",
"violation",
"protective_stopped",
"safeguard_stopped",
"recovery_mode",
"stopped_due_to_safety",
)
_RUNTIME_EXCEPTION_DESCRIPTIONS = {
"inv_kin_convergence": {
"error_type": "TARGET_UNREACHABLE",
"description_ja": (
"ロボットは指定された目標姿勢に到達できません。"
),
"description_en": (
"The robot cannot reach the specified target pose."
),
},
}
[docs]
@classmethod
def decode_runtime_exception(
cls,
raw_message: str,
) -> Dict[str, Any]:
"""Primary Interface の runtime exception を解釈する。"""
raw_text = str(raw_message or "").strip()
parts = raw_text.split(":")
error_id = ""
routine = ""
if parts:
error_id = parts[0].strip()
if len(parts) >= 2:
routine = parts[1].strip()
definition = cls._RUNTIME_EXCEPTION_DESCRIPTIONS.get(
error_id
)
if definition is None:
return {
"raw_message": raw_text,
"error_id": error_id or None,
"routine": routine or None,
"error_type": "UNKNOWN_RUNTIME_EXCEPTION",
"description_ja": (
"URコントローラで未登録の実行時エラーが"
"発生しました。"
),
"description_en": (
"An unregistered runtime exception occurred "
"in the UR controller."
),
"is_known": False,
}
decoded = dict(definition)
decoded["raw_message"] = raw_text
decoded["error_id"] = error_id
decoded["routine"] = routine or None
decoded["is_known"] = True
return decoded
[docs]
@classmethod
def decode_safety_state(
cls,
state: Dict[str, Any],
) -> Optional[Dict[str, Any]]:
"""RTDE safety state を具体的なエラー分類へ変換する。"""
flags = state.get("safety_status_flags") or {}
definitions = (
(
"system_emergency_stopped",
"SYSTEM_EMERGENCY_STOP",
"システム非常停止が作動しています。",
"The system emergency stop is active.",
),
(
"robot_emergency_stopped",
"ROBOT_EMERGENCY_STOP",
"ロボット非常停止が作動しています。",
"The robot emergency stop is active.",
),
(
"emergency_stopped",
"EMERGENCY_STOP",
"非常停止が作動しています。",
"An emergency stop is active.",
),
(
"fault",
"SAFETY_FAULT",
"安全システムで異常が発生しています。",
"A safety-system fault is active.",
),
(
"violation",
"SAFETY_VIOLATION",
"安全制限違反が発生しています。",
"A safety-limit violation is active.",
),
(
"protective_stopped",
"PROTECTIVE_STOP",
(
"ロボットが保護停止しました。障害物との接触、"
"過大な外力、または動作監視条件の超過を確認してください。"
),
(
"The robot entered a protective stop. Check for contact "
"with an obstacle, excessive external force, or a motion-"
"monitoring limit violation."
),
),
(
"safeguard_stopped",
"SAFEGUARD_STOP",
"セーフガード停止入力が作動しています。",
"A safeguard stop input is active.",
),
(
"recovery_mode",
"RECOVERY_MODE",
"ロボットは安全回復モードです。",
"The robot is in safety recovery mode.",
),
(
"stopped_due_to_safety",
"SAFETY_STOP",
"安全機能によりロボットが停止しました。",
"The robot was stopped by a safety function.",
),
)
for flag_name, error_type, description_ja, description_en in definitions:
if flags.get(flag_name, False):
return {
"error_type": error_type,
"safety_flag": flag_name,
"description_ja": description_ja,
"description_en": description_en,
}
return None
[docs]
class UrPrimaryErrorReceiver:
"""UR Primary Interface からロボットメッセージを受信する。"""
MESSAGE_TYPE_ROBOT_MESSAGE = 20
ROBOT_MESSAGE_TYPE_TEXT = 0
ROBOT_MESSAGE_TYPE_ERROR_CODE = 6
ROBOT_MESSAGE_TYPE_RUNTIME_EXCEPTION = 10
def __init__(
self,
robot_ip: str,
logger: Any,
port: int = 30001,
) -> None:
self._robot_ip = robot_ip
self._port = int(port)
self._logger = logger
self._socket = None
self._thread = None
self._stop_event = threading.Event()
self._messages = deque(maxlen=256)
self._lock = threading.Lock()
self._sequence = 0
[docs]
def connect(self) -> bool:
if self.is_connected():
return True
try:
sock = socket.create_connection(
(self._robot_ip, self._port),
timeout=2.0,
)
sock.settimeout(0.5)
self._socket = sock
self._stop_event.clear()
self._thread = threading.Thread(
target=self._receive_loop,
name="UrPrimaryErrorReceiver",
daemon=True,
)
self._thread.start()
return True
except Exception as exc:
self._logger.error(
"Primary Interface connection failed / "
"Primary Interface 接続失敗: %s",
exc,
)
self._socket = None
return False
[docs]
def disconnect(self) -> bool:
self._stop_event.set()
sock = self._socket
self._socket = None
if sock is not None:
try:
sock.shutdown(socket.SHUT_RDWR)
except Exception:
pass
try:
sock.close()
except Exception:
pass
thread = self._thread
self._thread = None
if thread is not None:
thread.join(timeout=1.0)
return True
[docs]
def is_connected(self) -> bool:
return self._socket is not None
[docs]
def mark(self) -> int:
with self._lock:
return self._sequence
[docs]
def get_messages_after(
self,
sequence: int,
timeout: float = 0.5,
) -> List[Dict[str, Any]]:
deadline = time.monotonic() + timeout
while time.monotonic() < deadline:
with self._lock:
result = [
dict(message)
for message in self._messages
if message["sequence"] > sequence
]
if result:
return result
time.sleep(0.01)
with self._lock:
return [
dict(message)
for message in self._messages
if message["sequence"] > sequence
]
[docs]
def _receive_exact(self, size: int) -> bytes:
data = bytearray()
while len(data) < size:
sock = self._socket
if sock is None:
raise ConnectionError(
"Primary Interface socket is not connected"
)
chunk = sock.recv(size - len(data))
if not chunk:
raise ConnectionError(
"Primary Interface connection was closed"
)
data.extend(chunk)
return bytes(data)
[docs]
def _receive_loop(self) -> None:
while not self._stop_event.is_set():
try:
header = self._receive_exact(4)
message_size = struct.unpack(">I", header)[0]
if message_size < 5:
raise ValueError(
f"Invalid Primary message size: {message_size}"
)
body = self._receive_exact(message_size - 4)
self._parse_message(header + body)
except socket.timeout:
continue
except Exception as exc:
if not self._stop_event.is_set():
self._logger.warning(
"Primary Interface receive stopped / "
"Primary Interface 受信停止: %s",
exc,
)
break
[docs]
def _parse_message(self, packet: bytes) -> None:
if len(packet) < 5:
return
message_type = packet[4]
if message_type != self.MESSAGE_TYPE_ROBOT_MESSAGE:
return
if len(packet) < 15:
return
timestamp, source, robot_message_type = struct.unpack(
">QbB",
packet[5:15],
)
message = None
if robot_message_type == self.ROBOT_MESSAGE_TYPE_RUNTIME_EXCEPTION:
if len(packet) < 23:
return
script_line, script_column = struct.unpack(
">II",
packet[15:23],
)
text = packet[23:].decode(
"utf-8",
errors="replace",
).rstrip("\x00")
message = {
"layer": "UR Primary Interface",
"message_type": "RUNTIME_EXCEPTION",
"message_type_id": robot_message_type,
"timestamp": timestamp,
"source": source,
"script_line": script_line,
"script_column": script_column,
"message": text,
}
elif robot_message_type == self.ROBOT_MESSAGE_TYPE_ERROR_CODE:
if len(packet) < 25:
return
message_code, message_argument = struct.unpack(
">II",
packet[15:23],
)
report_level = packet[23]
data_type = packet[24]
data = packet[25:].decode(
"utf-8",
errors="replace",
).rstrip("\x00")
message = {
"layer": "UR Primary Interface",
"message_type": "ERROR_CODE",
"message_type_id": robot_message_type,
"timestamp": timestamp,
"source": source,
"message_code": message_code,
"message_argument": message_argument,
"report_level": report_level,
"data_type": data_type,
"message": data,
}
elif robot_message_type == self.ROBOT_MESSAGE_TYPE_TEXT:
text = packet[15:].decode(
"utf-8",
errors="replace",
).rstrip("\x00")
message = {
"layer": "UR Primary Interface",
"message_type": "TEXT",
"message_type_id": robot_message_type,
"timestamp": timestamp,
"source": source,
"message": text,
}
if message is None:
return
with self._lock:
self._sequence += 1
message["sequence"] = self._sequence
self._messages.append(message)