r1-python-sdk 0.2.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
r1_python_sdk/base.py ADDED
@@ -0,0 +1,1075 @@
1
+ """Base: connection, background threads, send / receive, state derivation, enable / mode, callbacks, user control loop.
2
+
3
+ The other mixins (servo / gripper / board / kinematics) only use the internal methods exposed here and never touch
4
+ the serial port themselves.
5
+
6
+ Control-ownership model (see bottom-board firmware r1_arm.c / r1_usb_can.c):
7
+ - When the board is NORMAL the PC's motion frames pass straight to CAN; in TEACH/REPLAY/HOLD/ESTOP they are all
8
+ dropped and answered with TX_FAIL.
9
+ - Only enable / disable / clear-error / set-zero control frames (FF..Fx) and 0x7FF register **writes** (0x55) count
10
+ as "intent to take over" and make the board hand control back from HOLD. Position frames and 0x7FF read / refresh
11
+ do not. So motion_enable(True) is the takeover action; request_feedback() uses a 0x7FF 0xCC refresh and never
12
+ takes over by accident.
13
+ - E-stop (board mushroom button): pressed while the PC is in control, the board switches each joint to MIT and pins
14
+ it; after reset the board stays in HOLD and the PC must call motion_enable(True) again to regain control.
15
+ - The gripper motor (ID 7) belongs to the end board; frames the PC sends to it directly are dropped by the bottom
16
+ board. The SDK uses 0x7F1 command frames instead.
17
+
18
+ Board status (HOLD/TEACH/REPLAY/ESTOP, end-effector buttons, events) comes from the local reports 0x20~0x24 on the
19
+ USB-CAN bridge, so no second cable is needed; the USART2 log port ``$S/$E`` can still be connected and feeds the same
20
+ BoardStatus/BoardEvent.
21
+ """
22
+
23
+ from __future__ import annotations
24
+
25
+ import math
26
+ import threading
27
+ import time
28
+ from dataclasses import dataclass
29
+ from typing import Callable, Dict, List, Optional, Tuple
30
+
31
+ from . import damiao as dm
32
+ from . import protocol as pr
33
+ from .code import ArmMode, ArmState, R1Error, R1State, R1Warn
34
+ from .config import ConfigSource, R1Config
35
+ from .decorator import r1_catch, r1_connected
36
+ from .logport import LogPort
37
+ from .transport import (REP_HEARTBEAT, REP_LOCAL_EVENT, REP_LOCAL_POS, REP_LOCAL_STATUS,
38
+ REP_LOCAL_TAU, REP_LOCAL_TAUG, REP_LOCAL_VERSION, REP_RX_OK, REP_TX_FAIL, REP_TX_OK,
39
+ CanFrame, DmUsbCan, is_local_report)
40
+ from .utils import find_one_bottom_board, seq_from_rad
41
+ from .version import __version__
42
+
43
+ NJ = pr.NJ
44
+
45
+ BOARD_STALE_S = 0.5 # board status (local report / $S) older than this counts as absent
46
+ BLOCKED_STREAK = 6 # N consecutive TX_FAIL on motion frames => considered blocked
47
+ STOPPING_S = 0.5 # how long to stay in STOPPING after emergency_stop
48
+ POKE_MIN_GAP_S = 0.2 # minimum gap between automatic feedback refreshes while disabled
49
+ MOVING_VEL_RAD = 0.03
50
+ POS_SANE_RAD = 6.3
51
+
52
+ _MODE_TO_DM = {ArmMode.POSITION: dm.MODE_POS_VEL, ArmMode.SERVO: dm.MODE_MIT, ArmMode.VELOCITY: dm.MODE_VEL}
53
+
54
+ ControlFn = Callable[["Base", float], None]
55
+
56
+
57
+ @dataclass
58
+ class JointState:
59
+ """Latest feedback + command of one joint. Angles are always rad."""
60
+ index: int # 0..5
61
+ esc_id: int # CAN ID
62
+ pos: float = 0.0
63
+ vel: float = 0.0
64
+ tau: float = 0.0
65
+ status: int = 0 # Damiao status nibble
66
+ t_mos: int = 0
67
+ t_rotor: int = 0
68
+ t: float = 0.0 # time of latest feedback, monotonic; 0 = never received
69
+ dm_mode: int = 0 # last confirmed control mode, 0 = unknown
70
+ target: float = 0.0
71
+ vlim: float = 0.5
72
+ vel_target: float = 0.0
73
+ kp: float = 20.0
74
+ kd: float = 1.0
75
+
76
+ def fresh(self, now: float, within: float) -> bool:
77
+ return self.t > 0.0 and (now - self.t) <= within
78
+
79
+
80
+ class Base:
81
+ def __init__(self, port: Optional[str] = None, log_port: Optional[str] = None,
82
+ is_radian: bool = False, do_not_open: bool = False,
83
+ config: ConfigSource = None,
84
+ baud: int = pr.BOTTOM_CDC_BAUD, log_baud: int = pr.LOG_BAUD) -> None:
85
+ self._cfg = R1Config.load(config)
86
+ self._port = port
87
+ self._log_port = log_port
88
+ self._baud = baud
89
+ self._log_baud = log_baud
90
+ self._is_radian = bool(is_radian)
91
+
92
+ self._bus: Optional[DmUsbCan] = None
93
+ self._log: Optional[LogPort] = None
94
+ self._lock = threading.RLock()
95
+ self._cv = threading.Condition(self._lock)
96
+ self._last_error = ""
97
+
98
+ kp, kd = self._cfg.kp_list(), self._cfg.kd_list()
99
+ self._joints: List[JointState] = [
100
+ JointState(index=j, esc_id=int(self._cfg.esc_ids[j]), kp=kp[j], kd=kd[j],
101
+ vlim=math.radians(self._cfg.speed_default))
102
+ for j in range(NJ)
103
+ ]
104
+ self._limits: Optional[List[Tuple[float, float]]] = None
105
+ if self._cfg.joint_limits is not None:
106
+ self._limits = [(math.radians(lo), math.radians(hi)) for lo, hi in self._cfg.joint_limits]
107
+
108
+ self._enabled = False
109
+ self._mode = ArmMode.POSITION
110
+ self._targets_valid = False
111
+ self._stopping_until = 0.0
112
+ self._vel_until = 0.0
113
+
114
+ self._blocked = False
115
+ self._tx_fail_streak = 0
116
+ self._tx_results: Dict[int, Tuple[int, float]] = {} # can_id -> (cmd, t)
117
+ self._reg_replies: Dict[Tuple[int, int, int], dm.RegReply] = {}
118
+ self._heartbeat_t = 0.0
119
+ self._last_poke_t = 0.0
120
+
121
+ self._gripper: Optional[pr.GripperStatus] = None
122
+ self._gripper_t = 0.0
123
+ self._board: Optional[pr.BoardStatus] = None
124
+ self._board_version: Optional[pr.BoardVersion] = None
125
+ self._estop_by_event: Optional[bool] = None
126
+
127
+ self._cb: Dict[str, List[Callable]] = {k: [] for k in ("state", "error_warn", "event", "feedback")}
128
+ self._last_state = ArmState.UNKNOWN
129
+ self._last_err_warn = (R1Error.NONE, R1Warn.NONE)
130
+
131
+ self._ka_thread: Optional[threading.Thread] = None
132
+ self._ka_stop = threading.Event()
133
+
134
+ self._ctrl_fn: Optional[ControlFn] = None
135
+ self._ctrl_thread: Optional[threading.Thread] = None
136
+ self._ctrl_stop = threading.Event()
137
+ self._ctrl_rate = float(self._cfg.keepalive_hz)
138
+
139
+ if not do_not_open and (port is not None or find_one_bottom_board() is not None):
140
+ self.connect()
141
+
142
+ # =================================================================== properties
143
+ @property
144
+ def config(self) -> R1Config:
145
+ return self._cfg
146
+
147
+ @property
148
+ def connected(self) -> bool:
149
+ return self._bus is not None and self._bus.is_open
150
+
151
+ @property
152
+ def ready(self) -> bool:
153
+ return self.connected and self._ready_code() == R1State.NORMAL
154
+
155
+ @property
156
+ def motion_enabled(self) -> bool:
157
+ """Whether the PC really holds control right now: ``motion_enable(True)`` succeeded and the board has not
158
+ blocked it. During teach (blue button) / replay / HOLD / E-stop the board reports HOST_BLOCKED and this
159
+ becomes False immediately -- all keep-alive frames are rejected by the board, and ``motion_enable(True)`` is
160
+ needed to get control back. Also False when disabled / disconnected."""
161
+ return self.connected and self._enabled and not self._blocked
162
+
163
+ @property
164
+ def default_is_radian(self) -> bool:
165
+ return self._is_radian
166
+
167
+ @property
168
+ def version(self) -> str:
169
+ return __version__
170
+
171
+ @property
172
+ def last_error(self) -> str:
173
+ return self._last_error
174
+
175
+ @property
176
+ def state(self) -> int:
177
+ return self._compute_state()
178
+
179
+ @property
180
+ def mode(self) -> int:
181
+ return self._mode
182
+
183
+ @property
184
+ def angles(self) -> List[float]:
185
+ with self._lock:
186
+ return seq_from_rad([j.pos for j in self._joints], self._is_radian)
187
+
188
+ @property
189
+ def joints_torque(self) -> List[float]:
190
+ with self._lock:
191
+ return [j.tau for j in self._joints]
192
+
193
+ @property
194
+ def temperatures(self) -> List[int]:
195
+ with self._lock:
196
+ return [j.t_mos for j in self._joints]
197
+
198
+ @property
199
+ def error_code(self) -> int:
200
+ return self._compute_err_warn()[0]
201
+
202
+ @property
203
+ def warn_code(self) -> int:
204
+ return self._compute_err_warn()[1]
205
+
206
+ @property
207
+ def has_error(self) -> bool:
208
+ return self.error_code != R1Error.NONE
209
+
210
+ @property
211
+ def has_warn(self) -> bool:
212
+ return self.warn_code != R1Warn.NONE
213
+
214
+ @property
215
+ def board_status(self) -> Optional[pr.BoardStatus]:
216
+ """Latest board status (USB local report or ``$S``); None if none received."""
217
+ return self._board
218
+
219
+ @property
220
+ def board_version(self) -> Optional[pr.BoardVersion]:
221
+ """Bottom-board firmware version (the one reported on power-up, or obtained via ``get_board_version``).
222
+ None for old firmware / not received yet."""
223
+ return self._board_version
224
+
225
+ @property
226
+ def gripper_status(self) -> Optional[pr.GripperStatus]:
227
+ return self._gripper
228
+
229
+ @property
230
+ def joints(self) -> List[JointState]:
231
+ with self._lock:
232
+ return [JointState(**vars(j)) for j in self._joints]
233
+
234
+ @property
235
+ def bus_stats(self):
236
+ return self._bus.stats if self._bus is not None else None
237
+
238
+ @property
239
+ def control_loop_active(self) -> bool:
240
+ t = self._ctrl_thread
241
+ return t is not None and t.is_alive()
242
+
243
+ # =================================================================== A. connection
244
+ @r1_catch("set")
245
+ def connect(self, port: Optional[str] = None, log_port: Optional[str] = None) -> int:
246
+ """Connect to the bottom board's USB-CAN bridge; ``log_port`` is the USART2 log port (optional, just an
247
+ additional source of the same status)."""
248
+ if self.connected:
249
+ return R1State.NORMAL
250
+ port = port or self._port or find_one_bottom_board()
251
+ if not port:
252
+ self._set_last_error("connect: no bottom-board CDC port found (VID 0483 PID 7154), or more than one")
253
+ return R1State.NOT_CONNECTED
254
+ bus = DmUsbCan(port, self._baud)
255
+ bus.add_callback(self._on_bus)
256
+ bus.open()
257
+ self._bus = bus
258
+ self._port = port
259
+
260
+ # heartbeat handshake to confirm the other side is the board and not some random serial port
261
+ self._heartbeat_t = 0.0
262
+ bus.heartbeat()
263
+ if not self._wait_until(lambda: self._heartbeat_t > 0.0, 0.5):
264
+ bus.close()
265
+ self._bus = None
266
+ self._set_last_error(f"connect: {port} did not answer the heartbeat")
267
+ return R1State.RES_TIMEOUT
268
+
269
+ log_port = log_port or self._log_port
270
+ if log_port:
271
+ try:
272
+ lp = LogPort(log_port, self._log_baud)
273
+ lp.add_callback(self._on_log)
274
+ lp.open()
275
+ self._log = lp
276
+ self._log_port = log_port
277
+ except Exception as exc: # noqa: BLE001
278
+ self._set_last_error(f"connect: failed to open log port {log_port}: {exc!r} (ignored, continuing)")
279
+ self._log = None
280
+
281
+ self._reset_runtime()
282
+ self._ka_stop.clear()
283
+ self._ka_thread = threading.Thread(target=self._keepalive_loop, name="r1-keepalive", daemon=True)
284
+ self._ka_thread.start()
285
+ # ask for the firmware version once (the power-up 0x25 may have been flushed before the port was opened).
286
+ # old firmware replies TX_FAIL, ignored
287
+ self.get_board_version(timeout=0.3)
288
+ return R1State.NORMAL
289
+
290
+ def disconnect(self) -> int:
291
+ """Disconnect. Does not disable the motors itself: once keep-alive stops, the motors disable on their own CAN
292
+ TIMEOUT (the whole arm goes limp). To keep the pose, hand control to the board first (blue button -> teach /
293
+ pinned) and then disconnect."""
294
+ self.stop_control_loop()
295
+ self._ka_stop.set()
296
+ t = self._ka_thread
297
+ if t is not None and t.is_alive():
298
+ t.join(1.0)
299
+ self._ka_thread = None
300
+ if self._log is not None:
301
+ self._log.close()
302
+ self._log = None
303
+ if self._bus is not None:
304
+ self._bus.close()
305
+ self._bus = None
306
+ self._enabled = False
307
+ self._targets_valid = False
308
+ return R1State.NORMAL
309
+
310
+ def get_version(self) -> Tuple[int, dict]:
311
+ """(code, {'sdk': ..., 'firmware': None}). The firmware version query frame is not defined yet (FIRMWARE_PENDING item)."""
312
+ return R1State.NORMAL, {"sdk": __version__, "firmware": None}
313
+
314
+ # =================================================================== B. enable / mode / state
315
+ @r1_catch("set")
316
+ @r1_connected("set")
317
+ def motion_enable(self, enable: bool = True, servo_id: Optional[int] = None) -> int:
318
+ """Enable / disable. Enable sequence: clear error -> FC -> write CTRL_MODE and confirm -> seed the hold
319
+ targets from feedback -> keep-alive starts sending frames. This is also the action that takes control back
320
+ from the board's HOLD. ``servo_id`` 1~6 acts on a single joint (mostly used when disabling)."""
321
+ idx = self._joint_indices(servo_id)
322
+ if idx is None:
323
+ return R1State.SERVO_NOT_EXIST
324
+ if not enable:
325
+ self.stop_control_loop()
326
+ self._enabled = False
327
+ self._targets_valid = False
328
+ for j in idx:
329
+ self._send_ctrl(self._joints[j].esc_id, dm.CTRL_DISABLE)
330
+ return R1State.NORMAL
331
+
332
+ if self._estop_active():
333
+ return R1State.EMERGENCY_STOP
334
+
335
+ code = R1State.NORMAL
336
+ want = _MODE_TO_DM.get(self._mode)
337
+ for j in idx:
338
+ self._send_ctrl(self._joints[j].esc_id, dm.CTRL_CLEAR_ERR)
339
+ time.sleep(0.01)
340
+ for j in idx:
341
+ c = self._send_ctrl(self._joints[j].esc_id, dm.CTRL_ENABLE, confirm=True)
342
+ if c == R1State.TX_FAIL:
343
+ code = R1State.TX_FAIL
344
+ time.sleep(0.02)
345
+ if want is not None:
346
+ for j in idx:
347
+ c = self._ensure_mode(j, want)
348
+ if c != R1State.NORMAL and code == R1State.NORMAL:
349
+ code = c
350
+ if code == R1State.TX_FAIL:
351
+ self._set_last_error("motion_enable: failed to send the enable frame (bus? blocked?)")
352
+ return code
353
+
354
+ # wait for feedback from all joints (each FC / register write triggers one feedback frame)
355
+ if not self._wait_until(lambda: all(self._joints[j].fresh(time.monotonic(), 0.3) for j in idx), 0.5):
356
+ self._set_last_error("motion_enable: no feedback from some joints")
357
+ if code == R1State.NORMAL:
358
+ code = R1State.FB_LOST
359
+ self._reseed_targets()
360
+ self._enabled = True
361
+ self._stopping_until = 0.0
362
+ self._blocked = False
363
+ self._tx_fail_streak = 0
364
+ self._send_hold_once()
365
+ return code
366
+
367
+ @r1_connected("get")
368
+ def get_state(self) -> Tuple[int, int]:
369
+ """(code, state); state values are in ArmState."""
370
+ return R1State.NORMAL, self._compute_state()
371
+
372
+ def get_mode(self) -> Tuple[int, int]:
373
+ return R1State.NORMAL, self._mode
374
+
375
+ @r1_catch("set")
376
+ @r1_connected("set")
377
+ def set_mode(self, mode: int = 0) -> int:
378
+ """0 position / 1 servo (MIT) / 4 velocity. When already enabled, switches the motors' CTRL_MODE online and
379
+ re-seeds the targets. 2 teach is a board state that can only be entered with the end-effector blue button;
380
+ the PC cannot set it (get_state() shows TEACHING)."""
381
+ if mode == ArmMode.TEACH:
382
+ self._set_last_error("set_mode(2): teach is entered with the end-effector blue button, the PC cannot set it")
383
+ return R1State.CMD_NOT_EXIST
384
+ if mode not in _MODE_TO_DM:
385
+ return R1State.PARAM_ERROR
386
+ if mode == self._mode:
387
+ return R1State.NORMAL
388
+ if self._enabled:
389
+ if self.get_is_moving():
390
+ self._set_last_error("set_mode: cannot switch mode while moving (manual: commands are cleared on a switch)")
391
+ return R1State.NOT_READY
392
+ self._targets_valid = False
393
+ code = R1State.NORMAL
394
+ for j in range(NJ):
395
+ c = self._ensure_mode(j, _MODE_TO_DM[mode])
396
+ if c != R1State.NORMAL:
397
+ code = c
398
+ self._mode = mode
399
+ self._reseed_targets()
400
+ return code
401
+ self._mode = mode
402
+ return R1State.NORMAL
403
+
404
+ @r1_connected("get")
405
+ def get_err_warn_code(self) -> Tuple[int, List[int]]:
406
+ """(code, [err, warn]). err values are in R1Error, warn values in R1Warn."""
407
+ e, w = self._compute_err_warn()
408
+ return R1State.NORMAL, [e, w]
409
+
410
+ @staticmethod
411
+ def error_text(err: int) -> str:
412
+ if err == R1Error.NONE:
413
+ return "none"
414
+ if err == R1Error.ESTOP:
415
+ return "E-stop"
416
+ if err == R1Error.FB_LOST:
417
+ return "feedback lost"
418
+ if err == R1Error.BOARD_ALARM:
419
+ return "board alarm"
420
+ if R1Error.is_motor(err):
421
+ j, st = R1Error.motor_joint(err), R1Error.motor_status(err)
422
+ return f"joint{j} {dm.STATUS_NAMES.get(st, hex(st))}"
423
+ return f"error {err}"
424
+
425
+ @r1_catch("set")
426
+ @r1_connected("set")
427
+ def clean_error(self) -> int:
428
+ """Send a clear-error frame to all six joints. Note: the clear-error frame counts as "intent to take over";
429
+ a board in HOLD will hand control back to the PC."""
430
+ if self._blocked and not self._enabled:
431
+ self._set_last_error("clean_error: refused, the board is in local control; clearing errors would take "
432
+ "control back with nobody holding the arm")
433
+ return R1State.HOST_BLOCKED
434
+ for j in self._joints:
435
+ self._send_ctrl(j.esc_id, dm.CTRL_CLEAR_ERR)
436
+ return R1State.NORMAL
437
+
438
+ @r1_catch("set")
439
+ @r1_connected("set")
440
+ def emergency_stop(self) -> int:
441
+ """Software E-stop: stops immediately in any state, motors keep torque (R1 carries a gravity load; an E-stop
442
+ that disables would let the arm crash down):
443
+
444
+ - PC in control (NORMAL): stop the user control loop, pin the six joint targets at the current position,
445
+ STOPPING for 0.5 s then back to READY; motion commands are rejected meanwhile.
446
+ - Board in control (teach / replay / HOLD / E-stop, PC blocked): none of the PC's motion frames get through,
447
+ so send the local command 0x7F4 PAUSE instead; the board enters HOLD and pins, just like releasing the
448
+ blue button / pressing green. Success only when the board replies TX_OK; afterwards get_state() = PAUSED
449
+ and motion_enable(True) is needed to regain control. Firmware that does not know this command (too old)
450
+ times out and RES_TIMEOUT is returned; then only the end-effector buttons or the mushroom button can help.
451
+
452
+ The real hard E-stop is still the board's mushroom button: the board switches each joint to MIT, pins it
453
+ and blocks the PC."""
454
+ self.stop_control_loop()
455
+ if self._blocked:
456
+ code = self._send(CanFrame(pr.CAN_ID_HOST_CMD, bytes([pr.HOST_CMD_PAUSE])),
457
+ confirm=True, timeout=0.3)
458
+ if code == R1State.HOST_BLOCKED:
459
+ code = R1State.TX_FAIL # this frame is not subject to blocking; FAIL means "the board is not in charge in the current state"
460
+ if code != R1State.NORMAL:
461
+ self._set_last_error(f"emergency_stop: the board did not confirm the pause ({R1State.text(code)})")
462
+ return code
463
+ self._hold_here()
464
+ self._stopping_until = time.monotonic() + STOPPING_S
465
+ return R1State.NORMAL
466
+
467
+ def request_board_pause(self) -> int:
468
+ """Only send 0x7F4 PAUSE without touching the PC-side targets (this is what ``emergency_stop`` does when
469
+ blocked). In NORMAL state the board replies TX_FAIL -- the PC is in control itself, just use ``emergency_stop``."""
470
+ if not self.connected:
471
+ return R1State.NOT_CONNECTED
472
+ code = self._send(CanFrame(pr.CAN_ID_HOST_CMD, bytes([pr.HOST_CMD_PAUSE])),
473
+ confirm=True, timeout=0.3)
474
+ return R1State.TX_FAIL if code == R1State.HOST_BLOCKED else code
475
+
476
+ def get_board_version(self, timeout: float = 0.3) -> Optional[pr.BoardVersion]:
477
+ """Ask the board for its firmware version once (0x7F4 GET_VERSION -> 0x25); works even while blocked.
478
+
479
+ The board sends 0x25 before replying TX_OK, so by the time the reply arrives the version is already in
480
+ ``board_version``. Old firmware does not know the command, replies TX_FAIL, and None is returned
481
+ (``board_version`` stays None too)."""
482
+ if not self.connected:
483
+ return None
484
+ code = self._send(CanFrame(pr.CAN_ID_HOST_CMD, bytes([pr.HOST_CMD_GET_VERSION])),
485
+ confirm=True, timeout=timeout)
486
+ return self._board_version if code == R1State.NORMAL else None
487
+
488
+ def get_is_moving(self) -> bool:
489
+ now = time.monotonic()
490
+ with self._lock:
491
+ return any(j.fresh(now, self._cfg.feedback_stale_s) and abs(j.vel) > MOVING_VEL_RAD
492
+ for j in self._joints)
493
+
494
+ # =================================================================== callbacks
495
+ def register_state_changed_callback(self, cb: Callable[[dict], None]) -> bool:
496
+ """``{'state': ArmState}``. Also fires when the user enters TEACH/REPLAY with the blue / green button."""
497
+ return self._reg("state", cb)
498
+
499
+ def release_state_changed_callback(self, cb=None) -> bool:
500
+ return self._unreg("state", cb)
501
+
502
+ def register_error_warn_changed_callback(self, cb: Callable[[dict], None]) -> bool:
503
+ """``{'error_code': R1Error, 'warn_code': R1Warn}``."""
504
+ return self._reg("error_warn", cb)
505
+
506
+ def release_error_warn_changed_callback(self, cb=None) -> bool:
507
+ return self._unreg("error_warn", cb)
508
+
509
+ def register_event_callback(self, cb: Callable[[pr.BoardEvent], None]) -> bool:
510
+ """Board events BoardEvent (TRAJ_SAVED / REPLAY_DONE / ESTOP_ON ...), from USB local report 0x21 or the log port ``$E``."""
511
+ return self._reg("event", cb)
512
+
513
+ def release_event_callback(self, cb=None) -> bool:
514
+ return self._unreg("event", cb)
515
+
516
+ def register_feedback_callback(self, cb: Callable[[JointState], None]) -> bool:
517
+ """Once per motor feedback frame, for logging / plotting. Called on the receive thread, so keep it fast."""
518
+ return self._reg("feedback", cb)
519
+
520
+ def release_feedback_callback(self, cb=None) -> bool:
521
+ return self._unreg("feedback", cb)
522
+
523
+ # =================================================================== user control loop
524
+ @r1_catch("set")
525
+ @r1_connected("set")
526
+ def start_control_loop(self, fn: ControlFn, rate: Optional[float] = None) -> int:
527
+ """Call ``fn(arm, dt)`` repeatedly at ``rate`` Hz on a background thread; inside fn use set_servo_angle_j /
528
+ vc_set_joint_velocity to update the targets. If fn raises, the loop stops and the error is recorded in
529
+ last_error; the keep-alive thread keeps pinning the last target so the arm does not drop.
530
+ rate defaults to config.keepalive_hz."""
531
+ if not callable(fn):
532
+ return R1State.PARAM_ERROR
533
+ if self.control_loop_active:
534
+ self._set_last_error("start_control_loop: already running, call stop_control_loop() first")
535
+ return R1State.NOT_READY
536
+ code = self._ready_code()
537
+ if code != R1State.NORMAL:
538
+ self._set_last_error(f"start_control_loop: {R1State.text(code)}")
539
+ return code
540
+ rate = float(self._cfg.keepalive_hz if rate is None else rate)
541
+ if not (1.0 <= rate <= 1000.0):
542
+ return R1State.OUT_OF_RANGE
543
+ self._ctrl_fn = fn
544
+ self._ctrl_rate = rate
545
+ self._ctrl_stop.clear()
546
+ self._ctrl_thread = threading.Thread(target=self._control_loop_impl, name="r1-control", daemon=True)
547
+ self._ctrl_thread.start()
548
+ return R1State.NORMAL
549
+
550
+ def stop_control_loop(self) -> int:
551
+ self._ctrl_stop.set()
552
+ t = self._ctrl_thread
553
+ if t is not None and t.is_alive() and t is not threading.current_thread():
554
+ t.join(1.0)
555
+ self._ctrl_thread = None
556
+ self._ctrl_fn = None
557
+ return R1State.NORMAL
558
+
559
+ def _control_loop_impl(self) -> None:
560
+ dt = 1.0 / self._ctrl_rate
561
+ next_t = time.monotonic()
562
+ while not self._ctrl_stop.is_set():
563
+ fn = self._ctrl_fn
564
+ if fn is None:
565
+ break
566
+ try:
567
+ fn(self, dt)
568
+ except Exception as exc: # noqa: BLE001
569
+ self._set_last_error(f"control_loop: {exc!r}, loop stopped")
570
+ break
571
+ next_t += dt
572
+ wait = next_t - time.monotonic()
573
+ if wait > 0:
574
+ self._ctrl_stop.wait(wait)
575
+ else:
576
+ next_t = time.monotonic()
577
+
578
+ # =================================================================== internal: state derivation
579
+ def _ready_code(self) -> int:
580
+ if not self._enabled:
581
+ return R1State.NOT_READY
582
+ if self._estop_active():
583
+ return R1State.EMERGENCY_STOP
584
+ if self._blocked:
585
+ return R1State.HOST_BLOCKED
586
+ if time.monotonic() < self._stopping_until:
587
+ return R1State.NOT_READY
588
+ return R1State.NORMAL
589
+
590
+ def _board_fresh(self) -> Optional[pr.BoardStatus]:
591
+ b = self._board
592
+ if b is None or (time.monotonic() - b.t) > BOARD_STALE_S:
593
+ return None
594
+ return b
595
+
596
+ def _estop_active(self) -> bool:
597
+ b = self._board_fresh()
598
+ if b is not None:
599
+ return b.state == pr.ST_ESTOP or b.flag(pr.SF_ESTOP)
600
+ return bool(self._estop_by_event)
601
+
602
+ def _compute_state(self) -> int:
603
+ if not self.connected:
604
+ return ArmState.UNKNOWN
605
+ if self._estop_active():
606
+ return ArmState.ESTOP
607
+ b = self._board_fresh()
608
+ if b is not None:
609
+ if b.state == pr.ST_TEACH:
610
+ return ArmState.TEACHING
611
+ if b.state == pr.ST_REPLAY:
612
+ return ArmState.PLAYBACK
613
+ if b.state == pr.ST_HOLD:
614
+ return ArmState.PAUSED
615
+ if b.state in (pr.ST_BOOT, pr.ST_PROBE, pr.ST_BRINGUP):
616
+ return ArmState.INITIALIZING
617
+ elif self._blocked:
618
+ return ArmState.BLOCKED
619
+ if time.monotonic() < self._stopping_until:
620
+ return ArmState.STOPPING
621
+ return ArmState.MOVING if self.get_is_moving() else ArmState.READY
622
+
623
+ def _compute_err_warn(self) -> Tuple[int, int]:
624
+ now = time.monotonic()
625
+ err = R1Error.NONE
626
+ warn = R1Warn.NONE
627
+ if self._estop_active():
628
+ err = R1Error.ESTOP
629
+ else:
630
+ with self._lock:
631
+ for j in self._joints:
632
+ if j.fresh(now, 2.0) and dm.status_is_fault(j.status):
633
+ err = R1Error.motor(j.index + 1, j.status)
634
+ break
635
+ if err == R1Error.NONE:
636
+ b = self._board_fresh()
637
+ if b is not None:
638
+ if b.flag(pr.SF_ALARM):
639
+ err = R1Error.BOARD_ALARM
640
+ elif b.state == pr.ST_NORMAL and b.fb_mask == 0 and self._enabled:
641
+ err = R1Error.FB_LOST
642
+ if self._blocked:
643
+ warn = R1Warn.HOST_BLOCKED
644
+ elif self._enabled:
645
+ with self._lock:
646
+ if not all(j.fresh(now, self._cfg.feedback_stale_s) for j in self._joints):
647
+ warn = R1Warn.FEEDBACK_STALE
648
+ if warn == R1Warn.NONE:
649
+ g = self._gripper_fresh()
650
+ if g is not None:
651
+ if g.status == pr.GRIP_ST_FAULT:
652
+ warn = R1Warn.GRIPPER_FAULT
653
+ elif not g.homed:
654
+ warn = R1Warn.GRIPPER_NOT_INIT
655
+ return err, warn
656
+
657
+ def _gripper_fresh(self) -> Optional[pr.GripperStatus]:
658
+ if self._gripper is None or (time.monotonic() - self._gripper_t) > 0.5:
659
+ return None
660
+ return self._gripper
661
+
662
+ def _joints_fresh(self, idx: Optional[List[int]] = None) -> bool:
663
+ now = time.monotonic()
664
+ with self._lock:
665
+ js = self._joints if idx is None else [self._joints[j] for j in idx]
666
+ return all(j.fresh(now, self._cfg.feedback_stale_s) for j in js)
667
+
668
+ # =================================================================== internal: sending
669
+ def _send_ctrl(self, esc: int, cmd: int, confirm: bool = False) -> int:
670
+ f = CanFrame(dm.ID_MIT + esc, dm.ctrl_frame(cmd))
671
+ return self._send(f, confirm)
672
+
673
+ def _send(self, f: CanFrame, confirm: bool = False, timeout: float = 0.15) -> int:
674
+ bus = self._bus
675
+ if bus is None:
676
+ return R1State.NOT_CONNECTED
677
+ with self._lock:
678
+ self._tx_results.pop(f.id, None)
679
+ bus.send(f)
680
+ if not confirm:
681
+ return R1State.NORMAL
682
+ if not self._wait_until(lambda: f.id in self._tx_results, timeout):
683
+ return R1State.RES_TIMEOUT
684
+ cmd, _ = self._tx_results[f.id]
685
+ if cmd == REP_TX_OK:
686
+ return R1State.NORMAL
687
+ return R1State.HOST_BLOCKED if self._blocked else R1State.TX_FAIL
688
+
689
+ def _reg_read(self, esc: int, rid: int, timeout: Optional[float]):
690
+ key = (esc, rid, dm.REG_READ)
691
+ with self._lock:
692
+ self._reg_replies.pop(key, None)
693
+ c = self._send(CanFrame(dm.ID_REG, dm.reg_read_frame(esc, rid)), confirm=True)
694
+ if c != R1State.NORMAL:
695
+ return c, None
696
+ tmo = self._cfg.timeout if timeout is None else float(timeout)
697
+ if not self._wait_until(lambda: key in self._reg_replies, tmo):
698
+ return R1State.RES_TIMEOUT, None
699
+ return R1State.NORMAL, self._reg_replies[key].value
700
+
701
+ def _reg_write(self, esc: int, rid: int, value, timeout: Optional[float] = None) -> int:
702
+ key = (esc, rid, dm.REG_WRITE)
703
+ with self._lock:
704
+ self._reg_replies.pop(key, None)
705
+ c = self._send(CanFrame(dm.ID_REG, dm.reg_write_frame(esc, rid, value)), confirm=True)
706
+ if c != R1State.NORMAL:
707
+ return c
708
+ tmo = self._cfg.timeout if timeout is None else float(timeout)
709
+ if not self._wait_until(lambda: key in self._reg_replies, tmo):
710
+ return R1State.RES_TIMEOUT
711
+ return R1State.NORMAL
712
+
713
+ def _ensure_mode(self, j: int, want: int) -> int:
714
+ """Write CTRL_MODE and confirm with the reply; on success record it in JointState.dm_mode and immediately send
715
+ this joint a "stay here" target frame.
716
+
717
+ As soon as the register is written the motor swaps its internal controller (MIT kp/kd/tau_ff are gone, the
718
+ new loop starts from zero); until the PC's first target frame arrives it has no target, and a loaded joint
719
+ sags a little. The six joints are switched serially, so the whole arm visibly twitches on takeover. Hence
720
+ right after each switch a hold frame of that mode is sent with target = the joint's latest feedback position,
721
+ shrinking the no-target window from hundreds of ms to 1~2 ms."""
722
+ js = self._joints[j]
723
+ code = self._reg_write(js.esc_id, dm.RID_CTRL_MODE, want)
724
+ if code != R1State.NORMAL:
725
+ return code
726
+ rep = self._reg_replies.get((js.esc_id, dm.RID_CTRL_MODE, dm.REG_WRITE))
727
+ got = int(rep.value) if rep is not None else -1
728
+ if got != want:
729
+ self._set_last_error(f"joint{j + 1} CTRL_MODE: wrote {want}, read back {got}")
730
+ return R1State.MODE_MISMATCH
731
+ js.dm_mode = want
732
+ self._send_stay_frame(j, want)
733
+ return R1State.NORMAL
734
+
735
+ def _send_stay_frame(self, j: int, dm_mode: int) -> None:
736
+ """Send one "stay at the current feedback position" frame to a single joint (frame type chosen by motor mode).
737
+ If the feedback is stale, send nothing -- better not to move."""
738
+ bus = self._bus
739
+ js = self._joints[j]
740
+ now = time.monotonic()
741
+ if bus is None or not js.fresh(now, self._cfg.feedback_stale_s):
742
+ return
743
+ with self._lock:
744
+ pos = js.pos
745
+ js.target = pos
746
+ js.vel_target = 0.0
747
+ if dm_mode == dm.MODE_POS_VEL:
748
+ f = CanFrame(dm.ID_POS_VEL + js.esc_id, dm.pos_vel_frame(pos, js.vlim))
749
+ elif dm_mode == dm.MODE_MIT:
750
+ f = CanFrame(dm.ID_MIT + js.esc_id,
751
+ dm.mit_frame(pos, 0.0, js.kp, js.kd, 0.0, dm.R1_JOINT_LIMITS[js.index]))
752
+ elif dm_mode == dm.MODE_VEL:
753
+ f = CanFrame(dm.ID_VEL + js.esc_id, dm.vel_frame(0.0))
754
+ else:
755
+ return
756
+ try:
757
+ bus.send(f)
758
+ except Exception: # noqa: BLE001
759
+ pass
760
+
761
+ def _poke_feedback(self, force: bool = False) -> None:
762
+ """When disabled and the feedback is stale, use 0x7FF 0xCC to make each of the six motors send one feedback.
763
+ Rate-limited; never takes over by accident."""
764
+ now = time.monotonic()
765
+ if not force and (now - self._last_poke_t) < POKE_MIN_GAP_S:
766
+ return
767
+ if self._blocked or self._bus is None:
768
+ return
769
+ self._last_poke_t = now
770
+ try:
771
+ self._bus.send_many([CanFrame(dm.ID_REG, dm.reg_refresh_frame(j.esc_id)) for j in self._joints])
772
+ except Exception: # noqa: BLE001
773
+ pass
774
+
775
+ def _hold_frames(self) -> List[CanFrame]:
776
+ now = time.monotonic()
777
+ out: List[CanFrame] = []
778
+ with self._lock:
779
+ if not self._targets_valid:
780
+ return out
781
+ if self._mode == ArmMode.POSITION:
782
+ for j in self._joints:
783
+ out.append(CanFrame(dm.ID_POS_VEL + j.esc_id, dm.pos_vel_frame(j.target, j.vlim)))
784
+ elif self._mode == ArmMode.SERVO:
785
+ for j in self._joints:
786
+ out.append(CanFrame(dm.ID_MIT + j.esc_id,
787
+ dm.mit_frame(j.target, 0.0, j.kp, j.kd, 0.0, dm.R1_JOINT_LIMITS[j.index])))
788
+ elif self._mode == ArmMode.VELOCITY:
789
+ expired = self._vel_until and now >= self._vel_until
790
+ for j in self._joints:
791
+ v = 0.0 if expired else j.vel_target
792
+ out.append(CanFrame(dm.ID_VEL + j.esc_id, dm.vel_frame(v)))
793
+ return out
794
+
795
+ def _send_hold_once(self) -> None:
796
+ bus = self._bus
797
+ frames = self._hold_frames()
798
+ if bus is not None and frames:
799
+ try:
800
+ bus.send_many(frames)
801
+ except Exception: # noqa: BLE001
802
+ pass
803
+
804
+ def _keepalive_loop(self) -> None:
805
+ period = self._cfg.keepalive_s
806
+ next_t = time.monotonic()
807
+ while not self._ka_stop.is_set():
808
+ next_t += period
809
+ dt = next_t - time.monotonic()
810
+ if dt > 0:
811
+ self._ka_stop.wait(dt)
812
+ else:
813
+ next_t = time.monotonic()
814
+ if self._ka_stop.is_set():
815
+ break
816
+ if self._enabled:
817
+ self._send_hold_once()
818
+ self._poll_changes()
819
+
820
+ def _poll_changes(self) -> None:
821
+ st = self._compute_state()
822
+ if st != self._last_state:
823
+ self._last_state = st
824
+ self._fire("state", {"state": st})
825
+ ew = self._compute_err_warn()
826
+ if ew != self._last_err_warn:
827
+ self._last_err_warn = ew
828
+ self._fire("error_warn", {"error_code": ew[0], "warn_code": ew[1]})
829
+
830
+ # =================================================================== internal: target management
831
+ def _reseed_targets(self) -> None:
832
+ now = time.monotonic()
833
+ with self._lock:
834
+ for j in self._joints:
835
+ if j.fresh(now, 1.0):
836
+ j.target = j.pos
837
+ j.vel_target = 0.0
838
+ self._targets_valid = all(j.fresh(now, 1.0) for j in self._joints)
839
+
840
+ def _hold_here(self) -> None:
841
+ with self._lock:
842
+ for j in self._joints:
843
+ if j.fresh(time.monotonic(), 1.0):
844
+ j.target = j.pos
845
+ j.vel_target = 0.0
846
+ self._vel_until = 0.0
847
+ self._send_hold_once()
848
+
849
+ # =================================================================== internal: receive callbacks
850
+ def _on_bus(self, cmd: int, frame: Optional[CanFrame], now: float) -> None:
851
+ if cmd == -1:
852
+ # serial port died: keep-alive sends will fail and the state callback will report UNKNOWN
853
+ return
854
+ if cmd == REP_HEARTBEAT:
855
+ self._heartbeat_t = now
856
+ return
857
+ if frame is None:
858
+ return
859
+ if is_local_report(cmd):
860
+ self._on_local(cmd, frame.data, now)
861
+ return
862
+ if cmd == REP_RX_OK:
863
+ self._on_can_rx(frame, now)
864
+ return
865
+ if cmd in (REP_TX_OK, REP_TX_FAIL):
866
+ with self._cv:
867
+ self._tx_results[frame.id] = (cmd, now)
868
+ if self._is_arm_frame(frame):
869
+ if cmd == REP_TX_FAIL:
870
+ self._tx_fail_streak += 1
871
+ if self._tx_fail_streak >= BLOCKED_STREAK and not self._blocked:
872
+ self._blocked = True
873
+ self._targets_valid = False
874
+ else:
875
+ self._tx_fail_streak = 0
876
+ if self._blocked:
877
+ self._blocked = False
878
+ self._cv.notify_all()
879
+ return
880
+
881
+ def _on_local(self, cmd: int, p: bytes, now: float) -> None:
882
+ """Board local reports (USB-CAN bridge 0x20~0x25); merged with the log port $S/$E into the same place."""
883
+ if cmd == REP_LOCAL_STATUS:
884
+ st = pr.decode_local_status(p, now, self._board)
885
+ if st is not None:
886
+ self._on_board_status(st)
887
+ elif cmd == REP_LOCAL_EVENT:
888
+ ev = pr.decode_local_event(p, now)
889
+ if ev is not None:
890
+ self._on_board_event(ev)
891
+ elif cmd == REP_LOCAL_VERSION:
892
+ v = pr.decode_local_version(p)
893
+ if v is not None:
894
+ with self._cv:
895
+ self._board_version = v
896
+ self._cv.notify_all()
897
+ elif cmd in (REP_LOCAL_POS, REP_LOCAL_TAU, REP_LOCAL_TAUG):
898
+ b = self._board
899
+ if b is None or len(p) < 2 * NJ:
900
+ return
901
+ if cmd == REP_LOCAL_POS:
902
+ b.pos = pr.decode_local_pos(p, [lim.pmax for lim in dm.R1_JOINT_LIMITS])
903
+ elif cmd == REP_LOCAL_TAU:
904
+ b.tau = pr.decode_local_i16x6(p)
905
+ else:
906
+ b.tau_g = pr.decode_local_i16x6(p)
907
+
908
+ def _on_can_rx(self, f: CanFrame, now: float) -> None:
909
+ if f.ide:
910
+ return
911
+ esc_lo, esc_hi = min(self._cfg.esc_ids), max(self._cfg.esc_ids)
912
+ if f.id == dm.ID_REG or dm.looks_like_reg_reply(f.data, esc_lo, esc_hi):
913
+ rep = dm.decode_reg_reply(f.data)
914
+ if rep is not None:
915
+ with self._cv:
916
+ self._reg_replies[(rep.esc_id, rep.rid, rep.op)] = rep
917
+ self._cv.notify_all()
918
+ return
919
+ if f.id == pr.CAN_ID_GRIP_ST:
920
+ g = pr.decode_grip_status(f.data)
921
+ if g is not None:
922
+ self._gripper = g
923
+ self._gripper_t = now
924
+ return
925
+ if f.id in (pr.CAN_ID_BTN, pr.CAN_ID_UI, pr.CAN_ID_GRIP_CMD):
926
+ return
927
+ if len(f.data) < 6:
928
+ return
929
+ esc = f.data[0] & 0x0F
930
+ j = self._joint_by_esc(esc)
931
+ if j is None:
932
+ return
933
+ # Must also check CAN ID == MST_ID (0x10 + motor number; 0x00 is the Damiao factory default, kept as fallback).
934
+ # The board forwards every frame on the bus; in the MIT frames the end board sends to the gripper motor
935
+ # (ESC 0x07), data[0] is the high byte of the target position, whose low nibble collides with joints 1~6 with
936
+ # probability 6/16 -- this once overwrote J3 with -187 deg / status 0x4 / a fake fault.
937
+ # The bottom board r1_arm.c and the end board r1_gripper_dm.c filter the same way.
938
+ if f.id != pr.JOINT_MST_BASE + esc and f.id != 0:
939
+ return
940
+ fb = dm.decode_feedback(f.data, dm.R1_JOINT_LIMITS[j])
941
+ if fb is None or abs(fb.pos) > POS_SANE_RAD:
942
+ return
943
+ with self._cv:
944
+ js = self._joints[j]
945
+ js.pos, js.vel, js.tau = fb.pos, fb.vel, fb.tau
946
+ js.status, js.t_mos, js.t_rotor = fb.status, fb.t_mos, fb.t_rotor
947
+ js.t = now
948
+ snap = JointState(**vars(js))
949
+ self._cv.notify_all()
950
+ self._fire("feedback", snap)
951
+
952
+ def _on_log(self, item) -> None:
953
+ if isinstance(item, pr.BoardStatus):
954
+ self._on_board_status(item)
955
+ elif isinstance(item, pr.BoardEvent):
956
+ self._on_board_event(item)
957
+
958
+ def _on_board_status(self, item: pr.BoardStatus) -> None:
959
+ """The board status is the authoritative source of "who is in control": the blocked flag sets _blocked
960
+ directly, no need to wait for enough TX_FAILs to accumulate."""
961
+ self._board = item
962
+ if item.state == pr.ST_ESTOP or item.flag(pr.SF_ESTOP):
963
+ self._estop_by_event = True
964
+ elif self._estop_by_event:
965
+ self._estop_by_event = False
966
+ if item.flag(pr.SF_HOST_BLOCKED):
967
+ if not self._blocked:
968
+ self._blocked = True
969
+ self._targets_valid = False
970
+ elif item.state == pr.ST_NORMAL and self._blocked:
971
+ self._blocked = False
972
+ self._tx_fail_streak = 0
973
+
974
+ def _on_board_event(self, item: pr.BoardEvent) -> None:
975
+ if item.code == pr.EV_ESTOP_ON:
976
+ self._estop_by_event = True
977
+ elif item.code == pr.EV_ESTOP_OFF:
978
+ self._estop_by_event = False
979
+ self._fire("event", item)
980
+
981
+ def _is_arm_frame(self, f: CanFrame) -> bool:
982
+ if f.ide:
983
+ return False
984
+ base = f.id & 0x700
985
+ if base not in (dm.ID_MIT, dm.ID_POS_VEL, dm.ID_VEL, dm.ID_FORCE_POS):
986
+ return False
987
+ return self._joint_by_esc(f.id & 0x0FF) is not None
988
+
989
+ # =================================================================== internal: misc
990
+ def _reset_runtime(self) -> None:
991
+ with self._lock:
992
+ for j in self._joints:
993
+ j.t = 0.0
994
+ j.dm_mode = 0
995
+ self._enabled = False
996
+ self._targets_valid = False
997
+ self._stopping_until = 0.0
998
+ self._blocked = False
999
+ self._tx_fail_streak = 0
1000
+ self._tx_results.clear()
1001
+ self._reg_replies.clear()
1002
+ self._gripper = None
1003
+ self._gripper_t = 0.0
1004
+ self._board = None
1005
+ self._board_version = None
1006
+ self._estop_by_event = None
1007
+ self._last_poke_t = 0.0
1008
+ self._last_state = ArmState.UNKNOWN
1009
+ self._last_err_warn = (R1Error.NONE, R1Warn.NONE)
1010
+
1011
+ def _joint_indices(self, servo_id: Optional[int]) -> Optional[List[int]]:
1012
+ if servo_id is None:
1013
+ return list(range(NJ))
1014
+ if isinstance(servo_id, int) and not isinstance(servo_id, bool) and 1 <= servo_id <= NJ:
1015
+ return [servo_id - 1]
1016
+ return None
1017
+
1018
+ def _joint_by_esc(self, esc: int) -> Optional[int]:
1019
+ for j in self._joints:
1020
+ if j.esc_id == esc:
1021
+ return j.index
1022
+ return None
1023
+
1024
+ def _speed_rad(self, speed, rad: bool) -> Optional[float]:
1025
+ if speed is None:
1026
+ return math.radians(self._cfg.speed_default)
1027
+ if not isinstance(speed, (int, float)) or speed <= 0:
1028
+ return None
1029
+ v = float(speed) if rad else math.radians(float(speed))
1030
+ return max(0.02, min(10.0, v))
1031
+
1032
+ def _wait_until(self, pred: Callable[[], bool], timeout: float) -> bool:
1033
+ deadline = time.monotonic() + timeout
1034
+ with self._cv:
1035
+ while not pred():
1036
+ left = deadline - time.monotonic()
1037
+ if left <= 0:
1038
+ return False
1039
+ self._cv.wait(min(left, 0.05))
1040
+ return True
1041
+
1042
+ def _set_last_error(self, msg: str) -> None:
1043
+ self._last_error = msg
1044
+
1045
+ def _reg(self, kind: str, cb) -> bool:
1046
+ if not callable(cb):
1047
+ return False
1048
+ if cb not in self._cb[kind]:
1049
+ self._cb[kind].append(cb)
1050
+ return True
1051
+
1052
+ def _unreg(self, kind: str, cb) -> bool:
1053
+ if cb is None:
1054
+ self._cb[kind].clear()
1055
+ return True
1056
+ if cb in self._cb[kind]:
1057
+ self._cb[kind].remove(cb)
1058
+ return True
1059
+ return False
1060
+
1061
+ def _fire(self, kind: str, payload) -> None:
1062
+ for cb in list(self._cb.get(kind, ())):
1063
+ try:
1064
+ cb(payload)
1065
+ except Exception: # noqa: BLE001
1066
+ pass
1067
+
1068
+ # =================================================================== with
1069
+ def __enter__(self):
1070
+ if not self.connected:
1071
+ self.connect()
1072
+ return self
1073
+
1074
+ def __exit__(self, exc_type, exc, tb) -> None:
1075
+ self.disconnect()