From afed996ee442e07e06a77d381b80eb4844d25164 Mon Sep 17 00:00:00 2001 From: yd-sl <166189879+yd-sl@users.noreply.github.com> Date: Tue, 29 Sep 2026 15:47:51 +0800 Subject: [PATCH 1/2] feat: run gripper teleop over the validated zenoh link The teleop shipped in #11 invented its own network layer -- a plain-UDP transport on `litegrip/teleop/{master_id}` -- and was merged on hardware-free tests alone. The system actually validated on hardware is a zenoh point-to-point link, so the SDK now uses that structure: the topic and port come from the shared litearm namespace, both ends run in peer mode with multicast and gossip off, the leader listens on a TCP port and the follower connects to it, and the frame stays byte-identical to the litearm stack's. zenoh is an optional extra (`pip install litegrip[zenoh]`), resolved lazily, so `import litegrip` still works on a machine that will never teleoperate. The UDP transport stays available through `link="udp"` and is no longer the default. Alongside the transport, port the safety rules the validated design spec has and #11 was missing: non-finite frames are dropped at the wire boundary rather than clamped onto a hard stop; the follower's target is clamped into its own calibrated travel every cycle; a `send_mit_frame` that returns False and a gripper `error_code` other than "enabled" are counted instead of swallowed; follow gains fall back to the calibration's own kp/kd; a non-positive watchdog is rejected at construction; staleness is read from the slot's "never received" state rather than a 0.0 timestamp sentinel; teleop refuses to start on an uncalibrated or zero-travel gripper; and the leader's zenoh listener is resident for the life of the gripper, because rebuilding it per session leaves the port bound and makes matching fail intermittently. --- README.md | 57 +++++-- examples/teleop.py | 82 ++++++--- pyproject.toml | 6 + readme_zn.md | 49 ++++-- src/litegrip/__init__.py | 39 +++++ src/litegrip/gripper.py | 117 ++++++++++--- src/litegrip/teleop.py | 281 +++++++++++++++++++++++++------ src/litegrip/zenoh_link.py | 329 +++++++++++++++++++++++++++++++++++++ tests/test_teleop.py | 176 +++++++++++++++++++- tests/test_zenoh_link.py | 171 +++++++++++++++++++ 10 files changed, 1173 insertions(+), 134 deletions(-) create mode 100644 src/litegrip/zenoh_link.py create mode 100644 tests/test_zenoh_link.py diff --git a/README.md b/README.md index e6c3b8c..af81584 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,7 @@ using the MIT control protocol. | Platform | Linux only (SocketCAN) | | Python | 3.8 or newer | | Runtime dependencies | none, standard library only | +| Optional extra | `litegrip[zenoh]` — the point-to-point zenoh teleoperation link | ## Installation @@ -117,20 +118,39 @@ you push its jaws by hand — and it publishes how far open it is at the loop ra normalized opening in `[0, 1]`, not an angle, so the two ends do not need the same calibration, mount, or zero point. +### Transport + +The link is a **point-to-point zenoh** session — the same structure the field teleoperation runs +on. Both ends use `mode="peer"` with all discovery **off** (no multicast, no gossip), so the only +way they find each other is an explicit endpoint: the leader listens on a TCP port, the follower +connects to the leader's address. The topic is the shared litearm namespace, +`litearm/v4/{grip_id}/gripper_teleop`, and the frame is byte-identical to the litearm stack's, so +the two interoperate. + +zenoh is an optional dependency — the base SDK stays stdlib + SocketCAN: + +```bash +pip install 'litegrip[zenoh]' +``` + +`link="udp"` selects a plain-UDP fallback for a trusted LAN; it has no authentication or +encryption. Pass `transport=` a `TeleopTransport` to supply your own; an injected one is never +closed by the SDK. + ```python from litegrip import LiteGrip -# Leader: publish this gripper's opening to the follower at 192.168.1.20. +# Leader: listen and publish this gripper's opening. with LiteGrip("can0") as master: master.load_calibration() master.enable() - master.teleop_start("master", host="192.168.1.20") + master.teleop_start("master") # zenoh, gripA, port 17448 -# Follower: bind, align to the first frame, then follow. +# Follower: connect to the leader, align to the first frame, then follow. with LiteGrip("can0") as slave: slave.load_calibration() slave.enable() - slave.teleop_start("slave", host="0.0.0.0") + slave.teleop_start("slave", host="192.168.1.20") while True: print(slave.teleop_status()) # frames, openness, loop_hz, stale, ... ``` @@ -139,27 +159,34 @@ with LiteGrip("can0") as slave: ```bash # Machine A — the leader you push by hand: -python3 examples/teleop.py --mode master --channel can0 --host 192.168.1.20 +python3 examples/teleop.py --mode master --channel can0 # Machine B — the follower: -python3 examples/teleop.py --mode slave --channel can0 --host 0.0.0.0 +python3 examples/teleop.py --mode slave --channel can0 --host 192.168.1.20 ``` -Both ends must share `master_id` (default `master`) and be connected and enabled first. Teleop is +Both ends must share `grip_id` (default `gripA`) and be connected and enabled first. Teleop is exclusive: the background loop owns the CAN I/O, so do not drive the gripper from the caller until `teleop_stop()`. `teleop_start` returns the initial `teleop_status()` snapshot; `teleop_status()` -reports `active`, `mode`, `topic`, `frames`, `last_frame_age_ms`, `stale`, `openness`, `loop_hz`. +reports `active`, `mode`, `topic`, `frames`, `last_frame_age_ms`, `stale`, `openness`, +`loop_hz`, `rejected`, `send_failed`, `fault`, and (master) `matching`. -- **The transport is plain UDP**, with no authentication or encryption. Use it only on a trusted - network. Pass `transport=` a `TeleopTransport` to supply your own; an injected one is never closed - by the SDK. - **A follower that loses the leader holds its position, it does not go slack.** After `watchdog_s` (default `0.2`) without a fresh frame it keeps commanding its last target under the follow gains, so `stale` goes true but the jaws stay put — and can hold whatever is between them. -- **The follower clamps the incoming opening to `[0, 1]`**, i.e. to its own calibrated travel, so a - bad frame cannot command it past a limit. +- **Non-finite frames are dropped, never clamped.** A NaN opening would pass a `[0, 1]` clamp and + then fold onto a hard stop, silently driving the follower closed. Both ends reject NaN / ±inf at + the wire boundary — including the first frame used for the align — count them in `rejected`, and + hold position instead. +- **The follower clamps the target into its own calibrated travel every cycle**, and checks what + the SDK tells it: a `send_mit_frame` that returns `False` bumps `send_failed`, and a gripper + `error_code` other than "enabled" is reported in `fault` — neither is swallowed. - **Stopping leaves the gripper holding**, not slack: the master leaves zero-gravity mode on - `teleop_stop()`, so its jaws hold under the configured gains. -- Follow gains default to `kp=100.0`, `kd=2.0`; override with `kp=` / `kd=`. + `teleop_stop()` and the follower sends one final frame at its current angle, so both hold under + the configured gains and neither disables. +- Teleop refuses to start on an uncalibrated gripper, a zero-travel one, or one with + `rad_to_mm == 0` (`TeleopNotReady`), before anything is enabled or driven. +- Follow gains default to the calibration's `kp` / `kd` (`100.0` / `2.0` out of the box); override + with `kp=` / `kd=`. ## The six actions diff --git a/examples/teleop.py b/examples/teleop.py index 4a32de4..625cb06 100644 --- a/examples/teleop.py +++ b/examples/teleop.py @@ -5,28 +5,43 @@ meant to be started once per machine — one process per gripper: # Machine A (the leader you push by hand): - python3 examples/teleop.py --mode master --channel can0 --host 192.168.1.20 + python3 examples/teleop.py --mode master --channel can0 # Machine B (the follower that copies it): - python3 examples/teleop.py --mode slave --channel can0 --host 0.0.0.0 + python3 examples/teleop.py --mode slave --channel can0 --host 192.168.1.20 -Both ends must share ``--master-id``. The default transport is plain UDP on -``--port``; it carries no authentication or encryption, so keep it on a trusted -network. Press Ctrl+C on either end to stop; the gripper holds its position. +Both ends must share ``--grip-id``. The default link is the point-to-point zenoh +transport used by the field teleoperation (``pip install litegrip[zenoh]``): the +leader listens on ``--port``, the follower connects to ``--host``. ``--link udp`` +selects plain UDP instead, which carries no authentication or encryption — keep +either on a trusted network. Press Ctrl+C on either end to stop; the gripper +holds its position and does not disable. This script talks to real hardware. It does not detect an object in the jaws, and the follower holds its position on a leader dropout rather than going slack, so it can clamp whatever is between the fingers. Keep a hand on the power switch. + +It runs from a source checkout as well as from an installed package: ``src/`` is +put on the import path below if ``litegrip`` is not installed yet. """ from __future__ import annotations import argparse +import os import sys import time -from litegrip import LiteGrip, LiteGripError +# Like tests/_sdkpath.py: import the SDK straight out of the checkout, so the +# example works without `pip install -e .`. Inserted first, so the checkout wins +# over an installed copy — running the example exercises the code next to it. +_SRC = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "src") +if _SRC not in sys.path: + sys.path.insert(0, _SRC) + +from litegrip import (DEFAULT_GRIP_ID, DEFAULT_GRIP_PORT, # noqa: E402 + LiteGrip, LiteGripError) def build_parser() -> argparse.ArgumentParser: @@ -41,22 +56,26 @@ def build_parser() -> argparse.ArgumentParser: "--can-id", type=lambda s: int(s, 0), default=0x08, help="motor CAN ID (default: 0x08)") parser.add_argument( - "--host", required=True, - help="master: the follower's address; slave: the local bind address") + "--link", choices=("zenoh", "udp"), default="zenoh", + help="transport (default: zenoh; udp is the plain-LAN fallback)") + parser.add_argument( + "--host", default=None, + help="slave: the leader's address (the master only listens)") parser.add_argument( - "--port", type=int, default=7448, help="UDP port (default: 7448)") + "--port", type=int, default=DEFAULT_GRIP_PORT, + help=f"TCP (zenoh) or UDP port (default: {DEFAULT_GRIP_PORT})") parser.add_argument( - "--master-id", default="master", - help="topic id both ends must agree on (default: master)") + "--grip-id", default=DEFAULT_GRIP_ID, + help=f"topic id both ends must agree on (default: {DEFAULT_GRIP_ID})") parser.add_argument( "--mount", choices=("normal", "reverse"), default=None, help="load a mount template instead of this channel's calibration") parser.add_argument( "--kp", type=float, default=None, - help="follower stiffness (default: 100.0)") + help="follower stiffness (default: the calibration's kp)") parser.add_argument( "--kd", type=float, default=None, - help="follower damping (default: 2.0)") + help="follower damping (default: the calibration's kd)") parser.add_argument( "--no-align", action="store_true", help="follower: skip the one-shot align to the first frame") @@ -77,10 +96,19 @@ def _print_status(status: dict) -> None: age_txt = "-" if age is None else f"{age:6.1f}" openness = status.get("openness") open_txt = "-" if openness is None else f"{openness:5.3f}" + extra = "" + if status.get("matching") is not None: + extra += f" matching={str(status['matching']):>5}" + if status.get("rejected"): + extra += f" rejected={status['rejected']}" + if status.get("send_failed"): + extra += f" send_failed={status['send_failed']}" + if status.get("fault"): + extra += f" fault={status['fault']}" print(f"frames={status.get('frames', 0):>7} " f"age_ms={age_txt} stale={str(status.get('stale', False)):>5} " - f"openness={open_txt} loop_hz={status.get('loop_hz', 0.0):4.1f}", - flush=True) + f"openness={open_txt} " f"loop_hz={status.get('loop_hz', 0.0):4.1f}" + f"{extra}", flush=True) def main(argv: list[str] | None = None) -> int: @@ -94,18 +122,28 @@ def main(argv: list[str] | None = None) -> int: print(f"mount={gripper.mount} closed={gripper.config.pos_closed_rad:+.4f} " f"open={gripper.config.pos_open_rad:+.4f} rad_to_mm={gripper.config.rad_to_mm}") + target = f"{args.host}:{args.port}" if args.host else f"*:{args.port}" if args.dry_run: - print(f"dry run: would start {args.mode} on {args.channel} at " - f"{args.host}:{args.port} (topic litegrip/teleop/{args.master_id})") + print(f"dry run: would start {args.mode} on {args.channel} over " + f"{args.link} at {target} " + f"(topic litearm/v4/{args.grip_id}/gripper_teleop)") return 0 gripper.connect() - gripper.enable() + # enable() does not raise on failure — it returns a falsy EnableResult. + result = gripper.enable() + if not result.ok: + code = None if result.state is None else result.state.error_code + print(f"error: enable failed after {result.tries} tries " + f"(last error_code={code}); check the 24V supply and that " + f"{args.channel} is up at the right bitrate", file=sys.stderr) + gripper.disconnect() + return 1 + status = gripper.teleop_start( - args.mode, host=args.host, port=args.port, - kp=args.kp, kd=args.kd, align=not args.no_align, - watchdog_s=args.watchdog, rate_hz=args.rate, - master_id=args.master_id) + args.mode, link=args.link, host=args.host, port=args.port, + grip_id=args.grip_id, kp=args.kp, kd=args.kd, align=not args.no_align, + watchdog_s=args.watchdog, rate_hz=args.rate) print(f"teleop {args.mode} running; Ctrl+C to stop") _print_status(status) diff --git a/pyproject.toml b/pyproject.toml index b789d4d..05e540e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -50,6 +50,12 @@ classifiers = [ # that this installs on a bare robot controller. dependencies = [] +[project.optional-dependencies] +# The point-to-point zenoh link used by the field teleoperation. Optional so +# the base install stays stdlib + SocketCAN; only `litegrip.zenoh_link` and the +# default `teleop_start` transport need it. +zenoh = ["eclipse-zenoh>=1.0"] + [project.urls] Repository = "https://github.com/nexform-tech/litegrip-python" Issues = "https://github.com/nexform-tech/litegrip-python/issues" diff --git a/readme_zn.md b/readme_zn.md index f6f5d61..2f4a16a 100644 --- a/readme_zn.md +++ b/readme_zn.md @@ -16,6 +16,7 @@ | 平台 | 仅 Linux（SocketCAN） | | Python | 3.8 及以上 | | 运行时依赖 | 无，只用标准库 | +| 可选扩展 | `litegrip[zenoh]` —— 点对点 zenoh 遥操链路 | ## 安装 @@ -104,20 +105,36 @@ with LiteGrip("can1", mount="reverse") as gripper: 它按循环频率把「张开程度」发出去；**从夹爪**（follower）收到后驱动自己的爪子跟到位。线上传的 是归一化到 `[0, 1]` 的张开度，不是角度，所以两端不需要相同的标定、装法或零点。 +### 传输 + +链路是**点对点 zenoh** —— 与真机上跑的遥操同一套结构。两端都是 `mode="peer"`，**关掉全部 +发现机制**（无多播、无 gossip），所以两端只能靠显式端点互相找到：主端监听一个 TCP 端口，从端 +连到主端的地址。话题是共用的 litearm 命名空间 `litearm/v4/{grip_id}/gripper_teleop`，帧格式 +与 litearm 那套逐字节相同 ⇒ 两边可以互通。 + +zenoh 是**可选依赖**，基础 SDK 仍是「标准库 + SocketCAN」： + +```bash +pip install 'litegrip[zenoh]' +``` + +`link="udp"` 可切到明文 UDP 回退方案（仅限可信局域网，无鉴权、无加密）。要给自定义传输，传 +`transport=` 一个 `TeleopTransport`；注入的传输不会被 SDK 关闭。 + ```python from litegrip import LiteGrip -# 主端：把本夹爪的张开度发到 192.168.1.20 的从端。 +# 主端：监听并发布本夹爪的张开度。 with LiteGrip("can0") as master: master.load_calibration() master.enable() - master.teleop_start("master", host="192.168.1.20") + master.teleop_start("master") # zenoh，gripA，端口 17448 -# 从端：绑定端口，先对齐首帧，然后跟随。 +# 从端：连到主端，先对齐首帧，然后跟随。 with LiteGrip("can0") as slave: slave.load_calibration() slave.enable() - slave.teleop_start("slave", host="0.0.0.0") + slave.teleop_start("slave", host="192.168.1.20") while True: print(slave.teleop_status()) # frames, openness, loop_hz, stale, ... ``` @@ -126,24 +143,30 @@ with LiteGrip("can0") as slave: ```bash # A 机 —— 你用手掰的主夹爪： -python3 examples/teleop.py --mode master --channel can0 --host 192.168.1.20 +python3 examples/teleop.py --mode master --channel can0 # B 机 —— 从夹爪： -python3 examples/teleop.py --mode slave --channel can0 --host 0.0.0.0 +python3 examples/teleop.py --mode slave --channel can0 --host 192.168.1.20 ``` -两端必须共用 `master_id`（默认 `master`），且都已连接、已使能。遥操是互斥的：后台循环独占 CAN +两端必须共用 `grip_id`（默认 `gripA`），且都已连接、已使能。遥操是互斥的：后台循环独占 CAN 读写，在 `teleop_stop()` 之前不要再从调用方驱动夹爪。`teleop_start` 返回初始的 `teleop_status()`；`teleop_status()` 报告 `active`、`mode`、`topic`、`frames`、 -`last_frame_age_ms`、`stale`、`openness`、`loop_hz`。 +`last_frame_age_ms`、`stale`、`openness`、`loop_hz`、`rejected`、`send_failed`、 +`fault`，主端另有 `matching`。 -- **传输是明文 UDP**，无鉴权、无加密，只用在可信网络里。要给自定义传输，传 `transport=` 一个 - `TeleopTransport`；注入的传输不会被 SDK 关闭。 - **从端与主端失联时是「持位」，不是「卸力」。** 超过 `watchdog_s`（默认 `0.2`）没有新帧后， 它仍按跟随增益顶着上一个目标继续发帧 —— 于是 `stale` 变真，但爪子停在原地，可能夹住中间的 东西。 -- **从端会把收到的张开度夹到 `[0, 1]`**，也就是夹在自己的标定行程内，坏帧无法把它指到限位之外。 -- **停止后是持位**，不是卸力：主端在 `teleop_stop()` 时退出零重力模式，爪子按配置增益持位。 -- 跟随增益默认 `kp=100.0`、`kd=2.0`，用 `kp=` / `kd=` 覆盖。 +- **非有限值帧一律丢弃，绝不夹位。** NaN 会原样穿过 `[0, 1]` 的钳位，再被折到某个端点 —— 静默 + 地把从端指到全闭限位。两端都在**协议边界**上拒收 NaN / ±inf（包括对齐用的首帧），计入 + `rejected`，并改为持位。 +- **从端每拍都把目标夹进自己的标定行程**，并且会看 SDK 的返回值：`send_mit_frame` 返回 `False` + 会计入 `send_failed`，夹爪自报的 `error_code` 不是「已使能」会记进 `fault` —— 都不吞掉。 +- **停止后是持位**，不是卸力：主端在 `teleop_stop()` 时退出零重力模式，从端在收尾时按当前角度 + 补发一帧 —— 两边都按配置增益持位，都不失能。 +- 未标定、行程为零、或 `rad_to_mm == 0` 的夹爪会**拒绝启动**（`TeleopNotReady`），且在使能或 + 驱动之前就拒掉。 +- 跟随增益默认取标定里的 `kp` / `kd`（出厂是 `100.0` / `2.0`），用 `kp=` / `kd=` 覆盖。 ## 六个动作接口 diff --git a/src/litegrip/__init__.py b/src/litegrip/__init__.py index d8186e0..fcec1c3 100644 --- a/src/litegrip/__init__.py +++ b/src/litegrip/__init__.py @@ -119,6 +119,11 @@ def _detect_version(dist_name: str = "litegrip") -> str: TeleopError, TeleopBusyError, TeleopNotActiveError, + TeleopNotReady, + check_ready, + clamp_to_calibrated, + DEFAULT_GRIP_ID, + DEFAULT_GRIP_PORT, FRAME_SIZE, encode_frame, decode_frame, @@ -128,6 +133,30 @@ def _detect_version(dist_name: str = "litegrip") -> str: # ── CAN subpackage (expert) ───────────────────────────────────────────── from . import can + +# The zenoh link needs the optional ``zenoh`` dependency, so it is resolved on +# first access rather than at import time: ``import litegrip`` must work on a +# bare robot controller that will never teleoperate. Install the extra with +# ``pip install litegrip[zenoh]``. +_ZENOH_EXPORTS = frozenset( + {"ZenohTeleopTransport", "Listener", "Connector", "LatestSlot"}) + + +def __getattr__(name: str): + if name in _ZENOH_EXPORTS: + try: + from . import zenoh_link + except ImportError as e: + raise ImportError( + f"litegrip.{name} needs the optional zenoh dependency — install " + "it with `pip install litegrip[zenoh]`") from e + return getattr(zenoh_link, name) + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + + +def __dir__(): + return sorted(set(globals()) | _ZENOH_EXPORTS) + __all__ = [ "__version__", # High-level @@ -177,10 +206,20 @@ def _detect_version(dist_name: str = "litegrip") -> str: "TeleopError", "TeleopBusyError", "TeleopNotActiveError", + "TeleopNotReady", + "check_ready", + "clamp_to_calibrated", + "DEFAULT_GRIP_ID", + "DEFAULT_GRIP_PORT", "FRAME_SIZE", "encode_frame", "decode_frame", "teleop_topic", + # Teleoperation — zenoh link (resolved lazily; needs litegrip[zenoh]) + "ZenohTeleopTransport", + "Listener", + "Connector", + "LatestSlot", # Subpackages "can", ] diff --git a/src/litegrip/gripper.py b/src/litegrip/gripper.py index 02856ed..70cf451 100644 --- a/src/litegrip/gripper.py +++ b/src/litegrip/gripper.py @@ -42,6 +42,25 @@ MoveProgress, MoveResult, ) +from .teleop import DEFAULT_GRIP_ID, DEFAULT_GRIP_PORT + + +def _zenoh_transport(role: str, key: str, port: int, + host: Optional[str]) -> "TeleopTransport": + """Build a point-to-point zenoh transport, or explain how to get one. + + zenoh is an optional dependency, so a bare ``import`` failure is turned into + an actionable message rather than a ModuleNotFoundError naming a module the + user never asked for. + """ + try: + from .zenoh_link import ZenohTeleopTransport + except ImportError as e: + raise ImportError( + "the zenoh teleoperation link needs the optional zenoh dependency — " + "install it with `pip install litegrip[zenoh]`") from e + return ZenohTeleopTransport(role, key, port=port, host=host) + # Path to built-in factory calibration (ships with the package, read-only fallback). _FACTORY_CALIB = _os.path.join(_os.path.dirname(__file__), "factory_calibration.json") @@ -229,6 +248,10 @@ def __init__( # Transport teleop_start built itself (as opposed to one the caller # injected), so teleop_stop knows what it is allowed to close. self._teleop_transport: Optional["TeleopTransport"] = None + # The leader's zenoh publisher endpoint, kept for the life of this + # gripper rather than per session — rebuilding it per session leaves the + # port bound and makes matching fail intermittently. + self._teleop_pub: Optional["TeleopTransport"] = None # Declaring the mount is just loading the matching template, so it # costs no CAN traffic and is safe this early. The template carries @@ -347,6 +370,7 @@ def disconnect(self) -> None: if self._teleop is not None: self.teleop_stop() + self._close_teleop_pub() if self._can: self._can.disconnect(disable=self._disable_on_disconnect) @@ -1756,15 +1780,17 @@ def read_param(self, rid: int, timeout_s: float = 0.5) -> float: def teleop_start( self, mode: str, + *, transport: Optional["TeleopTransport"] = None, + link: str = "zenoh", host: Optional[str] = None, - port: int = 7448, + port: int = DEFAULT_GRIP_PORT, + grip_id: str = DEFAULT_GRIP_ID, kp: Optional[float] = None, kd: Optional[float] = None, align: bool = True, watchdog_s: float = 0.2, rate_hz: float = 50.0, - master_id: str = "master", ) -> dict: """Start leader/follower teleoperation on this gripper. @@ -1772,68 +1798,107 @@ def teleop_start( pushed by hand — and publishes the opening. ``mode="slave"`` (follower) receives the opening and follows it. - Both ends must agree on ``master_id``. Teleoperation is exclusive: - the background loop owns the CAN I/O until :meth:`teleop_stop`, so do - not drive this gripper from the caller while it runs. + Both ends must agree on ``grip_id``. Teleoperation is exclusive: the + background loop owns the CAN I/O until :meth:`teleop_stop`, so do not + drive this gripper from the caller while it runs. Args: mode: ``"master"`` or ``"slave"``. - transport: A :class:`~litegrip.TeleopTransport`. When omitted, a - :class:`~litegrip.UdpTeleopTransport` is built — the master - sends to ``host:port`` (the follower's address), the slave - binds ``host:port``. An injected transport is never closed by - this class. - host: Address for the default UDP transport (required when - ``transport`` is omitted). - port: UDP port for the default transport. - kp, kd: Follow gains (slave). ``None`` uses 100.0 / 2.0. + transport: A :class:`~litegrip.TeleopTransport`. When omitted, one + is built from ``link``. An injected transport is never closed + by this class. + link: ``"zenoh"`` (default) builds the point-to-point zenoh link + used by the field teleoperation — see + :mod:`litegrip.zenoh_link`, needs ``pip install litegrip[zenoh]``. + ``"udp"`` builds the plain-UDP transport, for a trusted LAN. + host: The peer's address. Required for a ``slave``; the master only + listens, so it needs no host. + port: TCP (zenoh) or UDP port. + grip_id: Topic id shared by both ends. + kp, kd: Follow gains (slave). ``None`` uses the calibration's own. align: Slave only — align to the first received frame before following. watchdog_s: Slave only — hold position after this long without a fresh frame. rate_hz: Loop rate. - master_id: Topic id shared by both ends. Returns: The initial :meth:`teleop_status` snapshot. Raises: + TeleopNotReady: uncalibrated, zero travel, or ``rad_to_mm == 0``. TeleopBusyError: teleoperation is already running. NotInitializedError: not connected or not enabled. """ - from .teleop import (GripperTeleop, TeleopBusyError, - UdpTeleopTransport, teleop_topic) + from .teleop import (GripperTeleop, TeleopBusyError, check_ready, + teleop_topic) + from .teleop import UdpTeleopTransport self._check_connected() self._check_enabled() if mode not in ("master", "slave"): raise ValueError(f"mode must be 'master' or 'slave', got {mode!r}") + # Before anything is enabled or driven: ``send_mit_frame`` and + # ``goto_rad`` do not check ``calibrated`` themselves. + check_ready(self.config) if self._teleop is not None and self._teleop.is_running: raise TeleopBusyError("teleop is already running") + key = teleop_topic(grip_id) created_transport = None if transport is None: - if host is None: - raise ValueError("host is required when no transport is given") - addr = f"{host}:{port}" - if mode == "master": - transport = created_transport = UdpTeleopTransport(pub_addr=addr) + if link == "zenoh": + if mode == "master": + transport = created_transport = self._open_teleop_pub(port, key) + else: + transport = created_transport = _zenoh_transport( + "slave", key, port, host) + elif link == "udp": + if host is None: + raise ValueError("host is required for the udp link") + addr = f"{host}:{port}" + if mode == "master": + transport = created_transport = UdpTeleopTransport(pub_addr=addr) + else: + transport = created_transport = UdpTeleopTransport(bind_addr=addr) else: - transport = created_transport = UdpTeleopTransport(bind_addr=addr) + raise ValueError(f"link must be 'zenoh' or 'udp', got {link!r}") manager = GripperTeleop( - self, transport, mode, teleop_topic(master_id), + self, transport, mode, key, rate_hz=rate_hz, kp=kp, kd=kd, align=align, watchdog_s=watchdog_s) manager.start() self._teleop = manager self._teleop_transport = created_transport return manager.status() + def _open_teleop_pub(self, port: int, key: str) -> "TeleopTransport": + """Return the leader's resident publisher, building it on first use. + + ⚠ **Resident, not per session.** Rebuilding the zenoh listener on every + session leaves the port bound and makes publisher↔subscriber matching + fail intermittently; keeping one for the life of the gripper removes + both. + """ + if self._teleop_pub is None: + self._teleop_pub = _zenoh_transport("master", key, port, None) + return self._teleop_pub + + def _close_teleop_pub(self) -> None: + pub, self._teleop_pub = self._teleop_pub, None + if pub is not None: + try: + pub.close() + except Exception as e: # noqa: BLE001 + log.debug("teleop publisher close failed: %s", e) + def teleop_stop(self, timeout: float = 2.0) -> dict: """Stop teleoperation and leave the gripper holding its position. - The master also leaves zero-gravity mode, so the jaws hold under the - configured gains rather than falling slack. + Neither side disables: the master leaves zero-gravity mode and the slave + sends one final frame at its current angle, so the jaws hold under gain. + The leader's resident publisher is **not** closed here — only the + per-session subscriber is. """ manager = self._teleop if manager is None: diff --git a/src/litegrip/teleop.py b/src/litegrip/teleop.py index 0c7cdce..5a3d8d2 100644 --- a/src/litegrip/teleop.py +++ b/src/litegrip/teleop.py @@ -31,18 +31,26 @@ * ``openness`` is clamped to ``[0, 1]``, which keeps every commanded target inside the calibrated travel. That clamp is the only limit this layer applies; there is no red-line logic here. +* A **non-finite** frame (NaN / ±inf) is *dropped*, never clamped. ``_clamp01`` + passes NaN through and ``min(hi, NaN)`` returns ``hi``, so clamping a NaN + target silently commands the follower to its closed stop. Bad readings are + rejected at the wire boundary on both ends; the follower then holds, which is + the safe side. * A ``slave`` whose leader goes quiet **holds** its last target at the follow gains (it does not relax to zero torque). The jaws therefore keep pressing whatever is between them — the same behaviour as the litearm original. * Teleoperation is exclusive: stop any motion you started elsewhere before calling :meth:`~litegrip.LiteGrip.teleop_start`. * :class:`UdpTeleopTransport` is plain, unauthenticated UDP. Use it only on a - trusted network. + trusted network. For real deployments use the point-to-point zenoh link in + :mod:`litegrip.zenoh_link` (``pip install litegrip[zenoh]``), which is what the + field-validated litearm teleoperation runs on. """ from __future__ import annotations import logging +import math import socket import struct import threading @@ -61,6 +69,12 @@ #: Size of one teleop frame in bytes (four doubles). FRAME_SIZE = _FRAME.size +#: Default grip id and TCP port. The topic and the port come from the litearm +#: teleoperation namespace: the arm uses ``armA``/17447, the gripper +#: ``gripA``/17448, so one machine can run both without a collision. +DEFAULT_GRIP_ID = "gripA" +DEFAULT_GRIP_PORT = 17448 + def encode_frame(openness: float, position_mm: float, force_n: float, timestamp: float) -> bytes: @@ -90,14 +104,17 @@ def decode_frame(payload: bytes) -> Tuple[float, float, float, float]: return _FRAME.unpack(payload) -def teleop_topic(master_id: str = "master") -> str: +def teleop_topic(grip_id: str = DEFAULT_GRIP_ID) -> str: """Topic the leader publishes and the follower subscribes to. - Both ends must agree on ``master_id``; it defaults to ``"master"`` so a - single pair needs no configuration. Use a distinct id per pair when more - than one teleoperation runs on the same transport. + ⚠ This is the **litearm teleoperation namespace**, shared with the arm and + gripper teleoperation stacks: ``litearm/v4/{grip_id}/gripper_teleop``. The + frame format is byte-identical, so the two interoperate — deliberately. + Both ends must agree on ``grip_id``; it defaults to ``"gripA"`` so a single + pair needs no configuration. Use a distinct id per pair when more than one + teleoperation runs on the same transport. """ - return f"litegrip/teleop/{master_id}" + return f"litearm/v4/{grip_id}/gripper_teleop" # ── Transport abstraction ───────────────────────────────────────────────── @@ -128,8 +145,9 @@ class TeleopTransport: """Publish/subscribe transport between a leader and a follower. Implement this to carry teleop frames over anything (a different network - stack, an in-process bus, ...). Two implementations ship with the SDK: - :class:`UdpTeleopTransport` and :class:`InProcTeleopTransport`. + stack, an in-process bus, ...). Three implementations ship with the SDK: + :class:`UdpTeleopTransport`, :class:`InProcTeleopTransport`, and — for real + deployments — the point-to-point zenoh link in :mod:`litegrip.zenoh_link`. """ def pub(self, topic: str, payload: bytes) -> None: @@ -173,7 +191,7 @@ class UdpTeleopTransport(TeleopTransport): The leader sends frames to ``pub_addr``; the follower receives on ``bind_addr``. Either or both may be given, so one object can both send and receive (not needed for a single leader/follower pair). Same machine: - ``"127.0.0.1:7448"``. Across machines: the peer's real address, + ``"127.0.0.1:17448"``. Across machines: the peer's real address, ``"0.0.0.0:"`` to accept on every interface. Unauthenticated and unencrypted — trusted networks only. A dropped @@ -303,7 +321,26 @@ def openness_to_rad(openness: float, cfg: Any) -> float: - cfg.close_sign * openness * travel_mm(cfg) / cfg.rad_to_mm) +def clamp_to_calibrated(cfg: Any, q: float) -> float: + """Clamp a motor angle into the calibrated travel. + + Either limit may be the larger one (a reverse mount swaps them), so this + takes ``min``/``max`` rather than assuming an order. Same source as the + SDK's own ``goto_rad`` clamp. + """ + lo = min(float(cfg.pos_closed_rad), float(cfg.pos_open_rad)) + hi = max(float(cfg.pos_closed_rad), float(cfg.pos_open_rad)) + return max(lo, min(hi, float(q))) + + def _clamp01(x: float) -> float: + """Clamp to ``[0, 1]``. + + ⚠ **This does not sanitise NaN** — every comparison against NaN is false, so + NaN passes straight through, and a later ``min(hi, NaN)`` yields ``hi``. + Callers must reject non-finite values *before* clamping, not rely on the + clamp to bound them. + """ return 0.0 if x < 0.0 else (1.0 if x > 1.0 else x) @@ -322,6 +359,37 @@ class TeleopNotActiveError(TeleopError): """Raised when an operation needs an active session but none is running.""" +class TeleopNotReady(TeleopError): + """Raised when the gripper cannot safely be teleoperated yet. + + ``send_mit_frame`` and ``goto_rad`` do **not** check ``calibrated`` — only + ``open``/``close``/``grasp`` do. Without this precheck an uncalibrated unit + is driven from placeholder limits of unknown direction. + """ + + +def check_ready(cfg: Any) -> None: + """Verify the calibration teleoperation depends on. **Call before enable.** + + Raises: + TeleopNotReady: uncalibrated, zero travel, or ``rad_to_mm == 0``. + + The zero-travel test is written in **rad space** (matching the SDK); testing + ``travel_mm == 0`` in mm space is a strictly weaker condition. + """ + if not bool(getattr(cfg, "calibrated", False)): + raise TeleopNotReady( + "gripper is not calibrated: pos_closed_rad / pos_open_rad are still " + "placeholder defaults and the direction is a guess. Run " + "load_calibration() / load_template(), or zero() first.") + if abs(float(cfg.pos_closed_rad) - float(cfg.pos_open_rad)) <= 1e-6: + raise TeleopNotReady( + "zero travel: pos_closed_rad equals pos_open_rad — recalibrate.") + if not float(cfg.rad_to_mm): + raise TeleopNotReady( + "rad_to_mm is 0: the openness<->radian conversion would divide by zero.") + + # ── the algorithm ───────────────────────────────────────────────────────── @@ -345,10 +413,13 @@ class GripperTeleop: topic: Topic to publish/subscribe. rate_hz: Loop rate. ~50 Hz is plenty; the CAN frame stream and the publish share the same cycle. - kp, kd: Follow gains (slave). ``None`` uses ``100.0`` / ``2.0``. + kp, kd: Follow gains (slave). ``None`` uses the calibration's ``kp`` / + ``kd``, falling back to ``100.0`` / ``2.0``. align: Slave only — align to the first frame before following. watchdog_s: Slave only — seconds without a fresh frame before the - follower is considered stale and starts holding. + follower is considered stale and starts holding. Must be > 0: a + non-positive watchdog makes the follower permanently stale, which is + a session that starts and then silently does nothing. sub_transport: Slave only — a separate transport to subscribe on when the leader is remote (the master's transport is local-only). sleep_fn, time_fn: Timing seams for tests. ``time_fn`` must be @@ -372,8 +443,10 @@ def __init__( ) -> None: if mode not in ("master", "slave"): raise ValueError(f"mode must be 'master' or 'slave', got {mode!r}") - if rate_hz <= 0.0: + if not rate_hz > 0.0: raise ValueError("rate_hz must be > 0") + if not watchdog_s > 0.0: + raise ValueError("watchdog_s must be > 0") self._g = gripper self._tp = transport self._mode = mode @@ -394,7 +467,14 @@ def __init__( self._frames = 0 self._last_openness = 0.0 self._last_frame_ts = 0.0 + #: Explicit "have we ever received a frame" flag. The watchdog only + #: applies after the first frame, and this must not be inferred from a + #: ``0.0`` timestamp — see the ``LatestSlot`` note in the design spec. + self._ever_received = False self._stale = False + self._rejected = 0 + self._send_failed = 0 + self._fault = "" self._loops = 0 self._loop_hz = 0.0 self._hz_t0 = 0.0 @@ -432,26 +512,38 @@ def _run(self) -> None: def stop(self, timeout: float = 2.0) -> None: """Stop the loop and leave the gripper holding its position. - The master also leaves zero-gravity mode, so the jaws hold under the - configured gains instead of falling slack. + Neither side disables the motor: the master leaves zero-gravity mode and + the slave sends one final frame at its current angle, so both hold under + gain and whatever is between the jaws stays there. """ self._running = False thread = self._thread if thread is not None and thread.is_alive(): thread.join(timeout=timeout) self._thread = None - if self._mode == "master": - try: - self._g.exit_zero_gravity() - except Exception as e: # noqa: BLE001 - log.debug("exit_zero_gravity on stop failed: %s", e) + self._handoff() log.info("teleop stopped: mode=%s frames=%d", self._mode, self._frames) + def _handoff(self) -> None: + """Leave the gripper holding position — never disable (§8 rule 4/6).""" + try: + if self._mode == "master": + # Internally one frame at the current angle under the config + # gains, which is exactly the hand-off we want. + self._g.exit_zero_gravity() + else: + q = clamp_to_calibrated( + self._g.config, self._g.get_state(wait=False).position_rad) + self._send(q, self._resolve_kp(), self._resolve_kd()) + except Exception as e: # noqa: BLE001 + log.debug("hand-off on stop failed: %s", e) + def status(self) -> dict: """A snapshot of the session, for logging and diagnostics.""" age_ms = None - if self._mode == "slave" and self._last_frame_ts > 0.0: + if self._mode == "slave" and self._ever_received: age_ms = (self._time_fn() - self._last_frame_ts) * 1000.0 + matching = getattr(self._tp, "matching", None) return { "active": self._running, "mode": self._mode, @@ -461,6 +553,14 @@ def status(self) -> dict: "stale": self._stale, "openness": round(self._last_openness, 4), "loop_hz": round(self._loop_hz, 1), + # Frames dropped at the protocol boundary (non-finite values). + "rejected": self._rejected, + # ``send_mit_frame`` returned False — the motor is not following. + "send_failed": self._send_failed, + # The gripper's own error_code was not "enabled". + "fault": self._fault, + # Master only: whether a subscriber is matched. + "matching": matching if isinstance(matching, bool) else None, } # ── master ──────────────────────────────────────────────────────── @@ -475,20 +575,28 @@ def _master_loop(self) -> None: # zero-torque frame goes out every cycle even though nothing # is being commanded. try: - self._g.send_mit_frame(q=0.0, kp=0.0, kd=0.0) + self._send(0.0, 0.0, 0.0, quiet=True) state = self._g.get_state(wait=False) except Exception: # noqa: BLE001 log.exception("[master] CAN error; loop exiting") break + self._note_grip_fault(state) openness = rad_to_openness(state.position_rad, self._g.config) - self._last_openness = openness - try: - self._tp.pub(self._topic, encode_frame( - openness, state.position_mm, state.force_n, - self._time_fn())) - except Exception as e: # noqa: BLE001 - log.debug("[master] publish failed: %s", e) - self._frames += 1 + if not all(math.isfinite(v) for v in + (openness, state.position_mm, state.force_n)): + # Bad reading: publish nothing. The follower's watchdog then + # times out and **holds**, which is the safe side. Sending + # the frame would hand the follower a NaN target. + self._count_rejected(openness) + else: + self._last_openness = openness + try: + self._tp.pub(self._topic, encode_frame( + openness, state.position_mm, state.force_n, + self._time_fn())) + except Exception as e: # noqa: BLE001 + log.debug("[master] publish failed: %s", e) + self._frames += 1 self._sleep_rest(t0) finally: log.info("[master] loop exited (%d frames sent)", self._frames) @@ -502,23 +610,26 @@ def _slave_loop(self) -> None: log.info("[slave] subscribed %s (align=%s watchdog=%.0fms)", self._topic, self._align, self._watchdog_s * 1000.0) - # Until a frame arrives, hold wherever the jaws already are. - openness_cmd = rad_to_openness(self._g.get_state(wait=False).position_rad, cfg) - q_cmd = openness_to_rad(openness_cmd, cfg) + # Until a frame arrives, hold wherever the jaws already are. A ``0.0`` + # target here would be a real position command — the open or the closed + # stop, depending on the mount. + q_cmd = clamp_to_calibrated(cfg, self._g.get_state(wait=False).position_rad) if self._align: - first = self._wait_first_frame(sub, timeout_s=5.0) + first = self._wait_first_frame(sub, timeout_s=5.0, hold_q=q_cmd) if first is not None: - openness_cmd = _clamp01(first[0]) - q_cmd = openness_to_rad(openness_cmd, cfg) - log.info("[slave] aligning to first frame: openness=%.3f -> %.3f rad", - openness_cmd, q_cmd) + q_cmd = clamp_to_calibrated( + cfg, openness_to_rad(_clamp01(first[0]), cfg)) + log.info("[slave] aligning to first frame: openness=%.3f -> %+.4f rad", + _clamp01(first[0]), q_cmd) try: self._g.goto_rad(q_cmd, kp=self._resolve_kp(), kd=self._resolve_kd(), duration=1.0) except Exception as e: # noqa: BLE001 log.warning("[slave] align goto_rad failed: %s", e) self._last_frame_ts = self._time_fn() + self._ever_received = True + self._last_openness = _clamp01(first[0]) else: log.warning("[slave] no frame within align timeout; " "holding current position") @@ -533,13 +644,19 @@ def _slave_loop(self) -> None: except ValueError as e: log.debug("[slave] ignoring bad frame: %s", e) else: - openness_cmd = _clamp01(openness) - q_cmd = openness_to_rad(openness_cmd, cfg) - self._last_openness = openness_cmd - self._last_frame_ts = self._time_fn() - self._frames += 1 - self._stale = False - elif (self._last_frame_ts > 0.0 + # Protocol boundary: reject non-finite values. Clamping + # a NaN target folds it onto a hard stop, silently. + if not all(math.isfinite(v) for v in (openness, _mm, _force)): + self._count_rejected(openness) + else: + rx_ts = self._time_fn() + q_cmd = openness_to_rad(_clamp01(openness), cfg) + self._last_openness = _clamp01(openness) + self._last_frame_ts = rx_ts + self._ever_received = True + self._frames += 1 + self._stale = False + elif (self._ever_received and (self._time_fn() - self._last_frame_ts) > self._watchdog_s): if not self._stale: log.warning("[slave] frames stale (>%.0fms); holding position", @@ -548,9 +665,10 @@ def _slave_loop(self) -> None: # Always send — including while stale. The frame both holds # the position and keeps the motor from self-locking. + q_cmd = clamp_to_calibrated(cfg, q_cmd) try: - self._g.send_mit_frame(q=q_cmd, kp=self._resolve_kp(), - kd=self._resolve_kd(), dq=0.0) + self._send(q_cmd, self._resolve_kp(), self._resolve_kd()) + self._note_grip_fault(self._g.get_state(wait=False)) except Exception: # noqa: BLE001 log.exception("[slave] CAN error; loop exiting") break @@ -560,24 +678,85 @@ def _slave_loop(self) -> None: # ── helpers ─────────────────────────────────────────────────────── - def _wait_first_frame(self, sub: TeleopSubscription, - timeout_s: float) -> Optional[Tuple[float, float, float, float]]: + def _wait_first_frame(self, sub: TeleopSubscription, timeout_s: float, + hold_q: float) -> Optional[Tuple[float, float, float, float]]: + """Wait for the first frame, holding position while we wait. + + ⚠ A **non-finite** frame is skipped and the wait continues, never + returned: this value feeds ``goto_rad``, and a NaN folds onto an end + stop — one bad frame would pull the follower to the closed limit, on the + ``align=True`` default path, before the loop's own guard could see it. + ⚠ The wait **keeps sending hold frames**: staying silent for up to 5 s + contradicts this module's own "stop sending ⇒ lose force" rule, and the + gripper may be holding something. + """ deadline = self._time_fn() + timeout_s while self._running and self._time_fn() < deadline: msg = sub.drain_latest() if msg is not None: try: - return decode_frame(msg) + values = decode_frame(msg) except ValueError: continue + if all(math.isfinite(v) for v in values): + return values + self._count_rejected(values[0]) + try: + self._send(hold_q, self._resolve_kp(), self._resolve_kd()) + self._note_grip_fault(self._g.get_state(wait=False)) + except Exception: # noqa: BLE001 + log.exception("[slave] CAN error during align; loop exiting") + return None self._sleep_fn(0.01) return None + def _count_rejected(self, got: Any) -> None: + """Record a frame dropped at the protocol boundary (§8 rule 9).""" + self._rejected += 1 + if self._rejected == 1: + log.warning("[%s] dropped a non-finite frame (openness=%r) — " + "holding position rather than folding onto a stop", + self._mode, got) + + def _send(self, q: float, kp: float, kd: float, + quiet: bool = False) -> None: + """Send one MIT frame and **check the return value** (§8 rule 10). + + ``send_mit_frame`` returns ``False`` when the motor is not enabled or the + CAN write fails — it does not raise, so ignoring the result means + believing we are driving a gripper that is not moving. + """ + if not self._g.send_mit_frame(q=q, kp=kp, kd=kd): + self._send_failed += 1 + if self._send_failed == 1 and not quiet: + log.warning("[%s] send_mit_frame returned False — the motor is " + "not enabled or the CAN write failed; the gripper may " + "not be moving at all", self._mode) + + def _note_grip_fault(self, state: Any) -> None: + """Consume the gripper's own ``error_code`` (§8 rule 10). + + Polling without reading ``error_code`` is the same as not polling: ``1`` + means enabled, ``0`` disabled, anything else is a real fault (over-temp, + over-current). Only the first one is reported. + """ + code = int(getattr(state, "error_code", 1)) + if code != 1 and not self._fault: + self._fault = (f"gripper reports error_code={code}" + + (" (disabled)" if code == 0 else " (fault)") + + " — still streaming hold frames") + log.warning("[%s] %s", self._mode, self._fault) + def _resolve_kp(self) -> float: - return self._kp if self._kp is not None else 100.0 + """Follow gain: the explicit one, else the calibration's ``kp``.""" + if self._kp is not None: + return float(self._kp) + return float(getattr(self._g.config, "kp", 100.0)) def _resolve_kd(self) -> float: - return self._kd if self._kd is not None else 2.0 + if self._kd is not None: + return float(self._kd) + return float(getattr(self._g.config, "kd", 2.0)) def _sleep_rest(self, t0: float) -> None: self._loops += 1 diff --git a/src/litegrip/zenoh_link.py b/src/litegrip/zenoh_link.py new file mode 100644 index 0000000..6d50e5a --- /dev/null +++ b/src/litegrip/zenoh_link.py @@ -0,0 +1,329 @@ +"""Pure point-to-point zenoh link for gripper teleoperation. + +This is a port of ``litearm-teleop-isomorphic/liteteleop/link.py`` — the transport +structure the arm teleoperation runs on in the field today. It is not a new +design; the function names, the config keys and the endpoint roles are kept +identical so the two can be compared line by line. What is new here is only the +:class:`ZenohTeleopTransport` adapter, which presents the same behaviour through +this SDK's :class:`~litegrip.TeleopTransport` interface. + +Both ends run ``mode="peer"`` with **all discovery off** (no multicast scouting, +no gossip): the only way two peers find each other is the explicit +``listen``/``connect`` endpoints given here. The leader listens, the follower +connects. + +Two rules worth losing a session over: + +* **The process will not exit until ``close()`` is called.** This was measured, + not assumed — an un-closed session leaves the interpreter hanging and only + ``timeout`` kills it. +* **The topic is shared with the litearm arm/gripper stack** + (``litearm/v4/{id}/gripper_teleop``). The frame *format* is byte-identical to + that stack, so the two interoperate — deliberately. Sharing a topic across + divergent formats would decode silently wrong, which is why the format is + pinned in :mod:`litegrip.teleop`. + +zenoh is an optional dependency: install it with ``pip install litegrip[zenoh]``. +This module is imported lazily so the base SDK still works without it. +""" + +from __future__ import annotations + +import logging +import threading +import time +from typing import Callable, Optional + +import zenoh + +from .teleop import (DEFAULT_GRIP_PORT, TeleopError, TeleopSubscription, + TeleopTransport) + +log = logging.getLogger("litegrip.zenoh") + +__all__ = ["DEFAULT_GRIP_PORT", "Listener", "Connector", "LatestSlot", + "ZenohTeleopTransport"] + + +def _base_config() -> zenoh.Config: + """The shared point-to-point config: **every discovery mechanism off**.""" + c = zenoh.Config() + c.insert_json5("scouting/multicast/enabled", "false") + c.insert_json5("scouting/gossip/enabled", "false") + c.insert_json5("mode", '"peer"') + return c + + +class _Endpoint: + """Common lifecycle: an idempotent :meth:`close` that must be called.""" + + def __init__(self) -> None: + self._session: Optional[zenoh.Session] = None + self._closed = False + + def close(self) -> None: + """Release the session. Idempotent, but **must** be called — without it + the process never exits.""" + if self._closed: + return + self._closed = True + s, self._session = self._session, None + if s is not None: + try: + s.close() + except Exception: # noqa: BLE001 - exit path never raises + pass + + def __enter__(self): + return self + + def __exit__(self, *exc) -> bool: + self.close() + return False + + +class Listener(_Endpoint): + """Leader side: listen on a port and wait for a follower to connect.""" + + def __init__(self, port: int, key: str) -> None: + super().__init__() + cfg = _base_config() + cfg.insert_json5("listen/endpoints", f'["tcp/0.0.0.0:{int(port)}"]') + cfg.insert_json5("connect/endpoints", "[]") + self._session = zenoh.open(cfg) + self._pub = self._session.declare_publisher(key) + self._matching = False + + def _on_match(status) -> None: + self._matching = bool(status.matching) + + self._pub.declare_matching_listener(_on_match) + + def put(self, payload: bytes) -> None: + """Publish one frame. Non-blocking.""" + self._pub.put(payload) + + @property + def matching(self) -> bool: + """Whether any subscriber is matched. + + This is a **boolean, not a count** — ``zenoh.MatchingStatus`` exposes only + ``.matching``. The UI shows "matched / not matched". + """ + return self._matching + + +class Connector(_Endpoint): + """Follower side: connect to the leader's host:port and subscribe to its stream. + + ``on_frame`` is called on **zenoh's own thread** ⇒ it must only write a + latest slot. Blocking work there (e.g. a ``goto_rad`` that waits for an ACK) + stalls the zenoh thread. + """ + + def __init__(self, host: str, port: int, key: str, + on_frame: Optional[Callable[[bytes], None]] = None) -> None: + super().__init__() + cfg = _base_config() + cfg.insert_json5("listen/endpoints", "[]") + cfg.insert_json5("connect/endpoints", f'[\"tcp/{host}:{int(port)}\"]') + self._session = zenoh.open(cfg) + cb = on_frame or (lambda _b: None) + #: Read-only frame counter, for the status snapshot. + #: Its safety comes from *a single subscriber's callbacks running serially + #: on one zenoh thread*, not from ``+=`` being atomic under the GIL — + #: attribute ``+=`` is not an atomic bytecode sequence. + self.received = 0 + + def _handler(sample) -> None: + self.received += 1 + cb(bytes(sample.payload)) + + self._sub = self._session.declare_subscriber(key, _handler) + + def latest(self) -> int: + """Frames received so far (used to display a rate).""" + return self.received + + +class LatestSlot: + """A latest-wins slot — the only hand-off between the zenoh callback and the + servo loop. + + Only the **newest** frame is kept: a late frame overwrites, nothing queues. + Teleoperation wants the latest pose only; a backlog would make the follower + chase a stale trajectory. + + ⚠ **"Never received" and "just received" must be distinguishable.** An + earlier version used ``0.0`` as the "never received" sentinel, so once + ``take()`` cleared the timestamp ``peek_age()`` reported "never received" + right after a frame was taken ⇒ a caller that takes a frame and then asks its + age is instantly judged dead and drops into HOLDING, triggering a blocking + realign — a single read order could stall the follower mid-motion. Now + ``_last_recv_ts is None`` means "never received", and ``take()`` clears only + the payload, never the timestamp. + """ + + def __init__(self) -> None: + self._lock = threading.Lock() + self._payload: Optional[bytes] = None + self._last_recv_ts: Optional[float] = None # None = never received + self.dropped = 0 + + def put(self, payload: bytes, now: float) -> None: + with self._lock: + if self._payload is not None: + self.dropped += 1 + self._payload = payload + self._last_recv_ts = now + + def take(self): + """Take the newest payload, returning ``(payload_or_None, last_recv_ts_or_None)``. + + ⚠ **The timestamp is left alone** — it is the fact of "when we last + received", which taking the payload must not erase. + """ + with self._lock: + p = self._payload + self._payload = None + return p, self._last_recv_ts + + @property + def ever_received(self) -> bool: + """Whether *any* frame ever arrived. + + The "no first frame within N seconds of start" diagnostic needs this, and + it is a **different question** from "the steady-state watchdog timed out" + (the latter only applies after a first frame). + """ + with self._lock: + return self._last_recv_ts is not None + + def peek_age(self, now: float) -> Optional[float]: + """Local seconds since the newest frame; **``None`` if never received**. + + ⚠ Returning ``None`` rather than ``0.0`` is deliberate: ``0.0`` is a + *legitimate* age ("just arrived"), so using it for "never received" would + void the diagnostic above. + ⚠ The result is clamped to ``≥ 0``: the loop may read ``now`` and then a + frame carrying a larger ``now`` lands, making the difference negative — + which is a hair away from a spurious watchdog trip. + ⚠ This is a **local** time difference, unrelated to the frame's own ``ts`` + (the leader's clock); the two machines' clocks are not a common source. + """ + with self._lock: + t = self._last_recv_ts + if t is None: + return None + return max(0.0, now - t) + + +class _SlotSubscription(TeleopSubscription): + """A :class:`LatestSlot` presented as a pull-style subscription.""" + + def __init__(self, slot: LatestSlot) -> None: + self._slot = slot + + def try_recv(self) -> Optional[bytes]: + payload, _ts = self._slot.take() + return payload + + @property + def ever_received(self) -> bool: + return self._slot.ever_received + + def peek_age(self, now: float) -> Optional[float]: + return self._slot.peek_age(now) + + @property + def dropped(self) -> int: + return self._slot.dropped + + +class ZenohTeleopTransport(TeleopTransport): + """A :class:`~litegrip.TeleopTransport` carried over the point-to-point zenoh link. + + One instance is one endpoint: ``role="master"`` wraps a :class:`Listener` + (publish only), ``role="slave"`` wraps a :class:`Connector` (subscribe only). + The follower's frames land in a :class:`LatestSlot` and are drained by the + servo loop, so the zenoh callback never blocks. + + ⚠ The leader's :class:`Listener` must be **resident across teleoperation + sessions**: rebuilding it per session both leaves the port bound and makes + publisher↔subscriber matching fail intermittently (5 of 12 restarts when + rebuilt; 0 of 12 when resident). The follower's :class:`Connector` is + per-session and must be closed when the session ends — see + :meth:`~litegrip.LiteGrip.teleop_stop`. + + Args: + role: ``"master"`` (listen/publish) or ``"slave"`` (connect/subscribe). + key: The zenoh topic, normally ``teleop_topic(grip_id)``. + port: TCP port. The leader listens on it, the follower connects to it. + host: Follower only — the leader's address. Ignored by the master. + """ + + def __init__(self, role: str, key: str, port: int = DEFAULT_GRIP_PORT, + host: Optional[str] = None) -> None: + if role not in ("master", "slave"): + raise ValueError(f"role must be 'master' or 'slave', got {role!r}") + if role == "slave" and not host: + raise ValueError("host is required for the slave (connecting) end") + self._role = role + self._key = key + self._endpoint: Optional[_Endpoint] = None + self._slot: Optional[LatestSlot] = None + self._sub: Optional[_SlotSubscription] = None + + if role == "master": + self._endpoint = Listener(int(port), key) + else: + self._slot = LatestSlot() + + def _on_wire(payload: bytes) -> None: + # Wrapped, not ``slot.put`` directly: the callback has one + # parameter, ``put`` needs two. Runs on zenoh's thread ⇒ only + # the slot is touched. + self._slot.put(payload, time.monotonic()) + + self._endpoint = Connector(host, int(port), key, on_frame=_on_wire) + + @property + def matching(self) -> bool: + """Master only — whether a subscriber is matched.""" + if isinstance(self._endpoint, Listener): + return self._endpoint.matching + return False + + @property + def received(self) -> int: + """Slave only — frames received.""" + if isinstance(self._endpoint, Connector): + return self._endpoint.received + return 0 + + def pub(self, topic: str, payload: bytes) -> None: + if self._role != "master": + raise TeleopError("this zenoh transport subscribes; it cannot publish") + self._check_topic(topic) + self._endpoint.put(payload) + + def sub(self, topic: str) -> TeleopSubscription: + if self._role != "slave": + raise TeleopError("this zenoh transport publishes; it cannot subscribe") + self._check_topic(topic) + if self._sub is None: + self._sub = _SlotSubscription(self._slot) + return self._sub + + def _check_topic(self, topic: str) -> None: + # The zenoh key is fixed when the endpoint is opened; a mismatched topic + # here means two ends were wired to different keys, which would otherwise + # look like "the leader just never publishes". + if topic != self._key: + raise TeleopError( + f"topic {topic!r} does not match this transport's key {self._key!r}") + + def close(self) -> None: + endpoint, self._endpoint = self._endpoint, None + if endpoint is not None: + endpoint.close() diff --git a/tests/test_teleop.py b/tests/test_teleop.py index dea3ab7..3db9495 100644 --- a/tests/test_teleop.py +++ b/tests/test_teleop.py @@ -13,17 +13,21 @@ from __future__ import annotations import time +import types import unittest +import unittest.mock import _sdkpath # noqa: F401 -from litegrip import (FRAME_SIZE, GripperTeleop, InProcTeleopTransport, - TeleopBusyError, UdpTeleopTransport, decode_frame, - encode_frame, teleop_topic) -from litegrip.teleop import openness_to_rad, rad_to_openness, travel_mm +from litegrip import (FRAME_SIZE, GripperTeleop, + InProcTeleopTransport, TeleopBusyError, TeleopNotReady, + UdpTeleopTransport, decode_frame, encode_frame, + teleop_topic) +from litegrip.teleop import (clamp_to_calibrated, check_ready, + openness_to_rad, rad_to_openness, travel_mm) from fake_can import POS_CLOSED_RAD, POS_OPEN_RAD, RAD_TO_MM, make_gripper -TOPIC = teleop_topic("master") +TOPIC = teleop_topic("gripA") # Far longer than the stub-sleep loop needs; a timeout here means the loop is # not running at all, not that the machine is slow. @@ -55,6 +59,11 @@ def sub(self, topic: str): class FrameCodecTest(unittest.TestCase): + def test_topic_matches_the_litearm_namespace(self): + # Shared with the validated litearm teleoperation stack, deliberately. + self.assertEqual(TOPIC, "litearm/v4/gripA/gripper_teleop") + self.assertEqual(teleop_topic(), "litearm/v4/gripA/gripper_teleop") + def test_size_is_four_doubles(self): self.assertEqual(FRAME_SIZE, 32) self.assertEqual(len(encode_frame(0.0, 0.0, 0.0, 0.0)), FRAME_SIZE) @@ -277,8 +286,8 @@ def test_rejects_unknown_mode(self): def test_start_discovers_udp_transport_and_closes_it(self): g, _ = make_gripper() - g.teleop_start("slave", host="127.0.0.1", port=0, align=False, - rate_hz=200.0) + g.teleop_start("slave", link="udp", host="127.0.0.1", port=0, + align=False, rate_hz=200.0) self.assertIsNotNone(g._teleop_transport) self.assertIsNotNone(g._teleop_transport._sock) g.teleop_stop() @@ -291,6 +300,159 @@ def test_disconnect_stops_teleop(self): g.disconnect() self.assertIsNone(g._teleop) + def test_start_refuses_uncalibrated_gripper(self): + g, _ = make_gripper() + g.config.calibrated = False + with self.assertRaises(TeleopNotReady): + g.teleop_start("slave", transport=InProcTeleopTransport()) + + def test_start_refuses_zenoh_link_without_the_extra(self): + # Without the optional dependency the failure must name the extra to + # install, not the bare ``zenoh`` module. + g, _ = make_gripper() + from litegrip import gripper as gripper_mod + with unittest.mock.patch.object( + gripper_mod, "_zenoh_transport", + side_effect=ImportError("pip install litegrip[zenoh]")): + with self.assertRaises(ImportError): + g.teleop_start("master") + + +class ReadinessTest(unittest.TestCase): + """The three preconditions that must hold before anything is enabled.""" + + def test_accepts_a_calibrated_gripper(self): + g, _ = make_gripper() + check_ready(g.config) + + def test_rejects_uncalibrated(self): + g, _ = make_gripper() + g.config.calibrated = False + with self.assertRaises(TeleopNotReady): + check_ready(g.config) + + def test_rejects_zero_travel(self): + g, _ = make_gripper() + g.config.pos_open_rad = g.config.pos_closed_rad + with self.assertRaises(TeleopNotReady): + check_ready(g.config) + + def test_rejects_zero_rad_to_mm(self): + g, _ = make_gripper() + g.config.rad_to_mm = 0.0 + with self.assertRaises(TeleopNotReady): + check_ready(g.config) + + +class ClampToCalibratedTest(unittest.TestCase): + def test_clamps_for_both_mountings(self): + for reverse in (False, True): + g, _ = make_gripper(reverse=reverse) + cfg = g.config + lo, hi = sorted((cfg.pos_closed_rad, cfg.pos_open_rad)) + self.assertAlmostEqual(clamp_to_calibrated(cfg, lo - 5.0), lo) + self.assertAlmostEqual(clamp_to_calibrated(cfg, hi + 5.0), hi) + mid = (lo + hi) / 2.0 + self.assertAlmostEqual(clamp_to_calibrated(cfg, mid), mid) + + +class NonFiniteFrameTest(unittest.TestCase): + """A NaN frame must be dropped, never clamped onto a limit.""" + + def test_master_does_not_publish_a_nan_reading(self): + g, fake = make_gripper(start_rad=POS_OPEN_RAD) + fake.motor.step = lambda *a, **k: None + g.get_state = lambda wait=True: types.SimpleNamespace( + position_rad=float("nan"), position_mm=0.0, force_n=0.0, + error_code=1) + transport = InProcTeleopTransport() + sub = transport.sub(TOPIC) + mgr = GripperTeleop(g, transport, "master", TOPIC, rate_hz=200.0, + sleep_fn=_nop_sleep) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: mgr.status()["rejected"] > 0)) + self.assertIsNone(sub.drain_latest()) + finally: + mgr.stop() + + def test_slave_drops_a_nan_frame_and_holds(self): + g, fake, transport, mgr = SlaveLoopTest()._slave( + align=False, watchdog_s=5.0) + hold = clamp_to_calibrated(g.config, POS_CLOSED_RAD) + transport.pub(TOPIC, encode_frame(float("nan"), 60.0, 0.0, 0.0)) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: mgr.status()["rejected"] > 0)) + self.assertTrue(_wait_until(lambda: len(fake.frames) > 3)) + # Every frame is the hold position: the NaN was not folded onto an + # end stop. + for frame in fake.frames: + self.assertAlmostEqual(frame.q, hold) + finally: + mgr.stop() + + +class SlaveGuardTest(unittest.TestCase): + def test_align_skips_nan_and_keeps_sending_hold_frames(self): + g, fake, transport, mgr = SlaveLoopTest()._slave( + align=True, watchdog_s=5.0) + hold = clamp_to_calibrated(g.config, POS_CLOSED_RAD) + mgr.start() + try: + # Waiting for the first frame: hold frames must keep flowing. + self.assertTrue(_wait_until(lambda: len(fake.frames) > 3)) + self.assertAlmostEqual(fake.frames[-1].q, hold) + transport.pub(TOPIC, encode_frame(float("nan"), 60.0, 0.0, 0.0)) + self.assertTrue(_wait_until(lambda: mgr.status()["rejected"] > 0)) + self.assertAlmostEqual(fake.frames[-1].q, hold) + # A good frame then aligns normally. + transport.pub(TOPIC, encode_frame(0.5, 60.0, 0.0, 0.0)) + target = openness_to_rad(0.5, g.config) + self.assertTrue(_wait_until( + lambda: abs(fake.frames[-1].q - target) < 1e-9)) + finally: + mgr.stop() + + def test_counts_send_failures(self): + g, fake, transport, mgr = SlaveLoopTest()._slave( + align=False, watchdog_s=5.0) + g.send_mit_frame = lambda **kwargs: False + mgr.start() + try: + self.assertTrue(_wait_until(lambda: mgr.status()["send_failed"] > 0)) + finally: + mgr.stop() + + def test_reports_the_gripper_fault_code(self): + g, _ = make_gripper(start_rad=POS_CLOSED_RAD, err=0) + transport = _PreSubTransport() + mgr = GripperTeleop(g, transport, "slave", TOPIC, rate_hz=200.0, + sleep_fn=_nop_sleep, align=False, watchdog_s=5.0) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: bool(mgr.status()["fault"]))) + finally: + mgr.stop() + + def test_kp_kd_fall_back_to_the_calibration(self): + g, fake, transport, mgr = SlaveLoopTest()._slave( + align=False, watchdog_s=5.0) + g.config.kp, g.config.kd = 33.0, 4.0 + transport.pub(TOPIC, encode_frame(0.8, 96.0, 0.0, 0.0)) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 0)) + self.assertEqual((fake.frames[-1].kp, fake.frames[-1].kd), (33.0, 4.0)) + finally: + mgr.stop() + + def test_rejects_non_positive_watchdog(self): + g, _ = make_gripper() + with self.assertRaises(ValueError): + GripperTeleop(g, InProcTeleopTransport(), "slave", TOPIC, + watchdog_s=0.0) + if __name__ == "__main__": unittest.main() diff --git a/tests/test_zenoh_link.py b/tests/test_zenoh_link.py new file mode 100644 index 0000000..1ed480a --- /dev/null +++ b/tests/test_zenoh_link.py @@ -0,0 +1,171 @@ +"""Tests for the point-to-point zenoh link. + +Skipped entirely when the optional ``zenoh`` dependency is not installed — the +base SDK must stay usable without it, so this module is not part of the +mandatory suite. + +The loopback cases open real sockets. A hang here would hang the suite, so the +suite is expected to be run under a timeout (``timeout 300 python3 -m unittest`` +— which is what CI does); that timeout is the guard the design's "must call +close() or the process never exits" rule is checked against. +""" + +from __future__ import annotations + +import socket +import time +import unittest + +import _sdkpath # noqa: F401 + +try: + import zenoh # noqa: F401 + HAVE_ZENOH = True +except ImportError: # pragma: no cover + HAVE_ZENOH = False + +if HAVE_ZENOH: + from litegrip.teleop import (DEFAULT_GRIP_PORT, TeleopError, teleop_topic) + from litegrip.zenoh_link import (Connector, LatestSlot, Listener, + ZenohTeleopTransport, _base_config) + +KEY = teleop_topic("gripA") +WAIT_S = 5.0 + + +def _wait_until(predicate, timeout_s: float = WAIT_S) -> bool: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if predicate(): + return True + time.sleep(0.01) + return predicate() + + +def _free_port() -> int: + """A port nothing is listening on right now (close it, then hand it over).""" + s = socket.socket() + s.bind(("127.0.0.1", 0)) + port = s.getsockname()[1] + s.close() + return port + + +@unittest.skipUnless(HAVE_ZENOH, "zenoh is not installed") +class ConfigTest(unittest.TestCase): + def test_discovery_is_off_and_mode_is_peer(self): + # The whole point of the link: no broadcast discovery, explicit + # endpoints only. + cfg = _base_config() + self.assertEqual(cfg.get_json("scouting/multicast/enabled"), "false") + self.assertEqual(cfg.get_json("scouting/gossip/enabled"), "false") + # zenoh reports JSON values verbatim, so the mode arrives quoted. + self.assertEqual(cfg.get_json("mode"), '"peer"') + + +@unittest.skipUnless(HAVE_ZENOH, "zenoh is not installed") +class LatestSlotTest(unittest.TestCase): + def test_never_received_is_not_zero(self): + slot = LatestSlot() + self.assertFalse(slot.ever_received) + # ``None``, not ``0.0``: ``0.0`` is a legitimate age ("just arrived"), + # and using it for "never received" voids the startup diagnostic. + self.assertIsNone(slot.peek_age(100.0)) + self.assertEqual(slot.take(), (None, None)) + + def test_take_does_not_clear_the_timestamp(self): + slot = LatestSlot() + slot.put(b"a", 10.0) + self.assertTrue(slot.ever_received) + self.assertEqual(slot.take(), (b"a", 10.0)) + # Taking the payload must not erase "when we last received". + self.assertEqual(slot.take(), (None, 10.0)) + self.assertTrue(slot.ever_received) + self.assertAlmostEqual(slot.peek_age(10.5), 0.5) + + def test_age_is_clamped_at_zero(self): + # The loop may read ``now`` before a frame carrying a larger ``now`` + # lands; a negative age is a hair away from a spurious watchdog trip. + slot = LatestSlot() + slot.put(b"a", 10.0) + self.assertEqual(slot.peek_age(9.0), 0.0) + + def test_latest_wins_and_dropped_counts_overwrites(self): + slot = LatestSlot() + slot.put(b"old", 1.0) + slot.put(b"new", 2.0) + self.assertEqual(slot.dropped, 1) + self.assertEqual(slot.take(), (b"new", 2.0)) + + +@unittest.skipUnless(HAVE_ZENOH, "zenoh is not installed") +class LoopbackTest(unittest.TestCase): + def test_listener_to_connector_roundtrip(self): + port = _free_port() + slot = LatestSlot() + listener = Listener(port, KEY) + connector = Connector("127.0.0.1", port, KEY, + on_frame=lambda p: slot.put(p, time.monotonic())) + try: + self.assertTrue(_wait_until(lambda: listener.matching), + "publisher never matched a subscriber") + listener.put(b"frame-1") + self.assertTrue(_wait_until(lambda: slot.take()[0] == b"frame-1")) + self.assertEqual(connector.received, 1) + finally: + connector.close() + listener.close() + + def test_transport_pub_to_sub(self): + port = _free_port() + master = ZenohTeleopTransport("master", KEY, port=port) + slave = ZenohTeleopTransport("slave", KEY, port=port, host="127.0.0.1") + try: + sub = slave.sub(KEY) + self.assertTrue(_wait_until(lambda: master.matching)) + master.pub(KEY, b"payload") + self.assertTrue(_wait_until(lambda: sub.try_recv() == b"payload")) + # Latest wins: a subscriber that fell behind skips history. + master.pub(KEY, b"older") + master.pub(KEY, b"newer") + self.assertTrue(_wait_until(lambda: sub.try_recv() == b"newer")) + finally: + slave.close() + master.close() + + def test_roles_are_one_way(self): + port = _free_port() + master = ZenohTeleopTransport("master", KEY, port=port) + try: + with self.assertRaises(TeleopError): + master.sub(KEY) + finally: + master.close() + + def test_topic_must_match_the_key(self): + port = _free_port() + master = ZenohTeleopTransport("master", KEY, port=port) + try: + with self.assertRaises(TeleopError): + master.pub("some/other/topic", b"x") + finally: + master.close() + + def test_slave_requires_a_host(self): + with self.assertRaises(ValueError): + ZenohTeleopTransport("slave", KEY, port=_free_port()) + + def test_close_is_idempotent(self): + master = ZenohTeleopTransport("master", KEY, port=_free_port()) + master.close() + master.close() + + +@unittest.skipUnless(HAVE_ZENOH, "zenoh is not installed") +class DefaultsTest(unittest.TestCase): + def test_default_port_is_the_gripper_port(self): + self.assertEqual(DEFAULT_GRIP_PORT, 17448) + + +if __name__ == "__main__": + unittest.main() From bc6567d57256f49a7cda8e17d8997ab17da15bd4 Mon Sep 17 00:00:00 2001 From: yd-sl <166189879+yd-sl@users.noreply.github.com> Date: Tue, 29 Sep 2026 15:48:57 +0800 Subject: [PATCH 2/2] fix: import teleop_topic outside the zenoh guard in the tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `litegrip.teleop` has no zenoh dependency, so the import belongs at module level. Behind the `HAVE_ZENOH` guard it left `teleop_topic` undefined when zenoh is absent, and the module failed to import instead of skipping — which is exactly the machine CI runs on. --- tests/test_zenoh_link.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_zenoh_link.py b/tests/test_zenoh_link.py index 1ed480a..e1a76af 100644 --- a/tests/test_zenoh_link.py +++ b/tests/test_zenoh_link.py @@ -17,6 +17,7 @@ import unittest import _sdkpath # noqa: F401 +from litegrip.teleop import DEFAULT_GRIP_PORT, TeleopError, teleop_topic try: import zenoh # noqa: F401 @@ -25,7 +26,6 @@ HAVE_ZENOH = False if HAVE_ZENOH: - from litegrip.teleop import (DEFAULT_GRIP_PORT, TeleopError, teleop_topic) from litegrip.zenoh_link import (Connector, LatestSlot, Listener, ZenohTeleopTransport, _base_config)