From 70f12451f2ab580408708d02ce8016aba068564c Mon Sep 17 00:00:00 2001 From: AlexTemirov Date: Mon, 10 Aug 2026 21:37:32 -0700 Subject: [PATCH] Add existing ROS 2 robot hardware provider --- blacknode-package.toml | 6 +- blacknode_robot/devices/__init__.py | 4 + blacknode_robot/devices/adapters/__init__.py | 3 +- .../devices/adapters/existing_ros2.py | 144 +++++++++++++++ blacknode_robot/devices/device_config.py | 73 +++++++- blacknode_robot/devices/service/runtime.py | 2 + configure.sh | 2 +- pyproject.toml | 2 +- scripts/configure_device.py | 12 +- scripts/configure_devices.py | 131 +++++++++++++- scripts/hardware_service.py | 6 +- tests/test_robot_devices.py | 166 +++++++++++++++++- 12 files changed, 528 insertions(+), 23 deletions(-) create mode 100644 blacknode_robot/devices/adapters/existing_ros2.py diff --git a/blacknode-package.toml b/blacknode-package.toml index b643e90..1e8d5e6 100644 --- a/blacknode-package.toml +++ b/blacknode-package.toml @@ -1,6 +1,6 @@ [package] name = "blacknode-robot" -version = "0.5.2" +version = "0.5.3" description = "Robot contracts, connected-device lifecycle, normalized telemetry, profiles, and driver launch." requires-blacknode = ">=0.3.0" layer = "robot" @@ -83,8 +83,8 @@ nodes = ["blacknode_robot/devices/nodes"] node-types = ["HardwareCapabilities", "RobotServo"] [components.devices.dependencies] -pip = ["pyserial>=3.5", "feetech-servo-sdk>=1.0"] -imports = ["serial", "scservo_sdk"] +pip = ["pyserial>=3.5", "feetech-servo-sdk>=1.0", "roslibpy>=1.5"] +imports = ["serial", "scservo_sdk", "roslibpy"] [components.telemetry] description = "Normalized robot temperatures, voltage, faults, joint state, and device-status telemetry." diff --git a/blacknode_robot/devices/__init__.py b/blacknode_robot/devices/__init__.py index 45d8719..818208d 100644 --- a/blacknode_robot/devices/__init__.py +++ b/blacknode_robot/devices/__init__.py @@ -9,6 +9,8 @@ ) from .safety import SafetyGate, SafetyLimits from .adapters import ( + ExistingRos2Config, + ExistingRos2Monitor, I2CMecanumBase, I2CMecanumConfig, SerialJointConfig, @@ -30,6 +32,8 @@ "MobileBaseProvider", "SafetyGate", "SafetyLimits", + "ExistingRos2Config", + "ExistingRos2Monitor", "I2CMecanumBase", "I2CMecanumConfig", "SerialJointConfig", diff --git a/blacknode_robot/devices/adapters/__init__.py b/blacknode_robot/devices/adapters/__init__.py index 457a576..a0bb89e 100644 --- a/blacknode_robot/devices/adapters/__init__.py +++ b/blacknode_robot/devices/adapters/__init__.py @@ -1,6 +1,7 @@ """Replaceable hardware providers.""" from .i2c_mecanum import I2CMecanumBase, I2CMecanumConfig +from .existing_ros2 import ExistingRos2Config, ExistingRos2Monitor from .serial_joint import ( SerialJointConfig, SerialJointGroup, @@ -10,6 +11,6 @@ ) __all__ = [ - "I2CMecanumBase", "I2CMecanumConfig", "SerialJointConfig", + "ExistingRos2Config", "ExistingRos2Monitor", "I2CMecanumBase", "I2CMecanumConfig", "SerialJointConfig", "SerialJointGroup", "SerialJointMonitor", "SerialJointSpec", "probe_serial", ] diff --git a/blacknode_robot/devices/adapters/existing_ros2.py b/blacknode_robot/devices/adapters/existing_ros2.py new file mode 100644 index 0000000..b72903c --- /dev/null +++ b/blacknode_robot/devices/adapters/existing_ros2.py @@ -0,0 +1,144 @@ +"""Read-only provider for a robot already running its own ROS 2 stack.""" + +from __future__ import annotations + +from dataclasses import dataclass +import time +from typing import Any, Callable + +from ..contracts import DeviceState + + +@dataclass(frozen=True) +class ExistingRos2Config: + """Connection and observed-interface contract for an existing ROS robot.""" + + host: str = "127.0.0.1" + port: int = 9090 + required_topics: tuple[str, ...] = () + capabilities: tuple[str, ...] = () + connect_timeout: float = 5.0 + + +def load_roslibpy() -> Any: + try: + import roslibpy + except Exception as exc: + raise RuntimeError( + "install roslibpy to use the existing ROS 2 adapter" + ) from exc + return roslibpy + + +class ExistingRos2Monitor: + """Observe an existing ROS graph through rosbridge without publishing. + + This provider deliberately exposes no arm, command, or torque methods. The + vendor robot stack retains ownership of actuators and startup services. + """ + + exclusive_connection = False + + def __init__( + self, + config: ExistingRos2Config, + *, + device_id: str = "device", + client_factory: Callable[[str, int], Any] | None = None, + ) -> None: + if not str(config.host).strip(): + raise ValueError("ROSBridge host is required") + if not 1 <= int(config.port) <= 65535: + raise ValueError("ROSBridge port must be from 1 to 65535") + if not config.required_topics: + raise ValueError("at least one observed ROS topic is required") + self.config = config + self.capabilities = tuple(config.capabilities) + self.device_id = device_id + self._client_factory = client_factory + self._client: Any | None = None + self._state = DeviceState( + device_id=device_id, + connected=False, + armed=False, + capabilities=list(self.capabilities), + values={ + "transport": "rosbridge", + "host": config.host, + "port": config.port, + "required_topics": list(config.required_topics), + }, + ) + + def _new_client(self) -> Any: + if self._client_factory is not None: + return self._client_factory(self.config.host, self.config.port) + roslibpy = load_roslibpy() + return roslibpy.Ros(host=self.config.host, port=self.config.port) + + def connect(self) -> DeviceState: + return self.refresh() + + def refresh(self) -> DeviceState: + self._state.updated_at = time.time() + try: + if self._client is None: + self._client = self._new_client() + if not bool(getattr(self._client, "is_connected", False)): + self._client.run(timeout=float(self.config.connect_timeout)) + if not bool(getattr(self._client, "is_connected", False)): + raise ConnectionError( + f"ROSBridge did not connect at {self.config.host}:{self.config.port}" + ) + topics = sorted( + { + str(topic).strip() + for topic in (self._client.get_topics() or []) + if str(topic).strip() + } + ) + topic_set = set(topics) + missing = [ + topic for topic in self.config.required_topics if topic not in topic_set + ] + self._state.connected = not missing + self._state.armed = False + self._state.capabilities = list(self.capabilities) + self._state.values = { + "transport": "rosbridge", + "host": self.config.host, + "port": self.config.port, + "required_topics": list(self.config.required_topics), + "observed_topics": topics, + "missing_topics": missing, + "read_only": True, + "vendor_stack_preserved": True, + } + self._state.error = ( + "Required ROS topics are unavailable: " + ", ".join(missing) + if missing + else "" + ) + except Exception as exc: + self._state.connected = False + self._state.armed = False + self._state.error = str(exc) + self._discard_client() + return self._state + + def state(self) -> DeviceState: + return self._state + + def close(self) -> None: + self._discard_client() + self._state.connected = False + self._state.armed = False + self._state.updated_at = time.time() + + def _discard_client(self) -> None: + if self._client is not None: + try: + self._client.terminate() + except Exception: + pass + self._client = None diff --git a/blacknode_robot/devices/device_config.py b/blacknode_robot/devices/device_config.py index 089e2f8..fc5d7ec 100644 --- a/blacknode_robot/devices/device_config.py +++ b/blacknode_robot/devices/device_config.py @@ -8,6 +8,7 @@ import tempfile from typing import Any +from .adapters.existing_ros2 import ExistingRos2Config, ExistingRos2Monitor from .adapters.serial_joint import ( SerialJointConfig, SerialJointMonitor, @@ -31,21 +32,53 @@ def normalize_device_name(value: Any, *, fallback: str = "") -> str: def validate_device_config(value: dict[str, Any]) -> dict[str, Any]: - """Validate and normalize a serial read-only device configuration.""" + """Validate and normalize a read-only hardware provider configuration.""" if value.get("version") != CONFIG_VERSION: raise ValueError(f"configuration version must be {CONFIG_VERSION}") - if value.get("adapter") != "serial_joint": - raise ValueError("adapter must be serial_joint") if value.get("mode") != "read_only": raise ValueError("mode must be read_only") device_id = value.get("device_id") - port = value.get("port") - baudrate = value.get("baudrate") - servos = value.get("servos") if not isinstance(device_id, str) or not device_id.strip(): raise ValueError("device_id must be a non-empty string") name = normalize_device_name(value.get("name"), fallback=device_id) + adapter = value.get("adapter") + if adapter == "existing_ros2": + host = value.get("host") + port = value.get("rosbridge_port") + required_topics = value.get("required_topics") + capabilities = value.get("capabilities") + if not isinstance(host, str) or not host.strip(): + raise ValueError("host must be a non-empty string") + if isinstance(port, bool) or not isinstance(port, int) or not 1 <= port <= 65535: + raise ValueError("rosbridge_port must be a whole number from 1 to 65535") + if not isinstance(required_topics, list) or not required_topics: + raise ValueError("required_topics must contain at least one ROS topic") + if not isinstance(capabilities, list) or not capabilities: + raise ValueError("capabilities must contain at least one capability") + normalized_topics = _normalized_unique_strings( + required_topics, field="required_topics", require_ros_name=True + ) + normalized_capabilities = _normalized_unique_strings( + capabilities, field="capabilities" + ) + return { + "version": CONFIG_VERSION, + "device_id": device_id.strip(), + "name": name, + "adapter": "existing_ros2", + "mode": "read_only", + "host": host.strip(), + "rosbridge_port": port, + "required_topics": normalized_topics, + "capabilities": normalized_capabilities, + } + if adapter != "serial_joint": + raise ValueError("adapter must be serial_joint or existing_ros2") + + port = value.get("port") + baudrate = value.get("baudrate") + servos = value.get("servos") if not isinstance(port, str) or not port.strip(): raise ValueError("port must be a non-empty string") if isinstance(baudrate, bool) or not isinstance(baudrate, int) or baudrate <= 0: @@ -85,6 +118,21 @@ def validate_device_config(value: dict[str, Any]) -> dict[str, Any]: } +def _normalized_unique_strings( + values: list[Any], *, field: str, require_ros_name: bool = False +) -> list[str]: + normalized: list[str] = [] + for value in values: + clean = str(value or "").strip() + if not clean: + raise ValueError(f"{field} values must be non-empty strings") + if require_ros_name and not clean.startswith("/"): + raise ValueError(f"{field} values must be absolute ROS topic names") + if clean not in normalized: + normalized.append(clean) + return normalized + + def load_device_config(path: str | Path = DEFAULT_CONFIG_PATH) -> dict[str, Any]: config_path = Path(path) try: @@ -137,3 +185,16 @@ def serial_monitor_from_config(value: dict[str, Any]) -> SerialJointMonitor: joints=joints, ) return SerialJointMonitor(serial_config, device_id=config["device_id"]) + + +def provider_from_config(value: dict[str, Any]) -> Any: + config = validate_device_config(value) + if config["adapter"] == "serial_joint": + return serial_monitor_from_config(config) + ros_config = ExistingRos2Config( + host=config["host"], + port=config["rosbridge_port"], + required_topics=tuple(config["required_topics"]), + capabilities=tuple(config["capabilities"]), + ) + return ExistingRos2Monitor(ros_config, device_id=config["device_id"]) diff --git a/blacknode_robot/devices/service/runtime.py b/blacknode_robot/devices/service/runtime.py index b9d0c03..1f5e505 100644 --- a/blacknode_robot/devices/service/runtime.py +++ b/blacknode_robot/devices/service/runtime.py @@ -173,6 +173,8 @@ def stop(self) -> dict[str, Any]: def release(self) -> dict[str, Any]: if self.provider is None: return {"ok": False, "error": "no hardware adapter configured"} + if getattr(self.provider, "exclusive_connection", True) is False: + return {"ok": True, "status": self.status()} if hasattr(self.provider, "stop"): self.provider.stop() if hasattr(self.provider, "disarm"): diff --git a/configure.sh b/configure.sh index e703948..c0225ff 100755 --- a/configure.sh +++ b/configure.sh @@ -49,7 +49,7 @@ if [[ "${1:-}" == "--all" ]]; then && -f "$repo_dir/.blacknode-hardware/devices.json" \ && "$(uname -s)" == "Linux" \ && -x "$repo_dir/service.sh" ]]; then - echo "Stopping configured hardware services briefly so every serial bus can be rescanned..." + echo "Stopping configured Blacknode Hardware services briefly for provider discovery..." "$repo_dir/service.sh" --all stop || true restore_previous_fleet=true trap restore_fleet_on_failure EXIT diff --git a/pyproject.toml b/pyproject.toml index 3cb82b4..fa2053a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "blacknode-robot" -version = "0.5.2" +version = "0.5.3" description = "Robot contracts, connected-device lifecycle, and normalized telemetry for Blacknode." requires-python = ">=3.11" dependencies = ["pyserial>=3.5", "feetech-servo-sdk>=1.0", "roslibpy>=1.5"] diff --git a/scripts/configure_device.py b/scripts/configure_device.py index 8c49203..817fee6 100644 --- a/scripts/configure_device.py +++ b/scripts/configure_device.py @@ -47,9 +47,15 @@ def print_config(config: dict[str, Any], path: Path) -> None: print(f"Name: {config['name']}") print(f"Device ID: {config['device_id']}") print(f"Mode: {config['mode']}") - print(f"Port: {config['port']}") - print(f"Baudrate: {config['baudrate']}") - print(f"Servos: {', '.join(str(servo['id']) for servo in config['servos'])}") + print(f"Adapter: {config['adapter']}") + if config["adapter"] == "existing_ros2": + print(f"ROSBridge: {config['host']}:{config['rosbridge_port']}") + print(f"Observed topics: {', '.join(config['required_topics'])}") + print(f"Capabilities: {', '.join(config['capabilities'])}") + else: + print(f"Port: {config['port']}") + print(f"Baudrate: {config['baudrate']}") + print(f"Servos: {', '.join(str(servo['id']) for servo in config['servos'])}") def main() -> int: diff --git a/scripts/configure_devices.py b/scripts/configure_devices.py index 428282d..1c010c5 100644 --- a/scripts/configure_devices.py +++ b/scripts/configure_devices.py @@ -1,4 +1,4 @@ -"""Discover and configure every connected serial robot using read-only probes.""" +"""Discover and configure robots through safe, read-only hardware providers.""" from __future__ import annotations @@ -19,6 +19,10 @@ SerialJointSpec, probe_serial, ) +from blacknode_robot.devices.adapters.existing_ros2 import ( + ExistingRos2Config, + ExistingRos2Monitor, +) from blacknode_robot.devices.device_config import normalize_device_name, save_device_config @@ -86,6 +90,12 @@ def device_key(port: str) -> str: return f"{label[:40].rstrip('-')}-{digest}" +def existing_ros2_key(host: str, port: int) -> str: + identity = f"{host}:{port}" + digest = hashlib.sha256(identity.encode("utf-8")).hexdigest()[:8] + return f"existing-ros2-{digest}" + + def load_manifest(path: Path) -> dict[str, Any]: if not path.exists(): return {"version": FLEET_VERSION, "devices": []} @@ -231,12 +241,19 @@ def print_manifest(manifest: dict[str, Any], path: Path) -> None: print(f"Manifest: {path}") print(f"Robots: {len(devices)}") for index, device in enumerate(devices, start=1): - servo_ids = ", ".join(str(value) for value in device["servo_ids"]) print() print(f"{index}. {device.get('name') or device['device_id']}") print(f" Device ID: {device['device_id']}") - print(f" Serial: {device['serial_port']}") - print(f" Servos: {servo_ids}") + if device.get("adapter") == "existing_ros2": + print( + " ROSBridge: " + f"{device.get('rosbridge_host')}:{device.get('rosbridge_port')}" + ) + print(f" Topics: {', '.join(device.get('required_topics') or [])}") + else: + servo_ids = ", ".join(str(value) for value in device["servo_ids"]) + print(f" Serial: {device['serial_port']}") + print(f" Servos: {servo_ids}") print(f" Service: http://DEVICE_IP:{device['service_port']}") @@ -272,6 +289,25 @@ def main() -> int: default=20, help="scan servo IDs 1 through COUNT on every serial bus (default: 20)", ) + parser.add_argument( + "--existing-ros2", + action="store_true", + help="configure the robot already exposed through ROSBridge instead of scanning serial servos", + ) + parser.add_argument("--rosbridge-host", default="127.0.0.1") + parser.add_argument("--rosbridge-port", type=int, default=9090) + parser.add_argument( + "--required-topic", + action="append", + default=[], + help="observed ROS topic required for a healthy robot; repeat as needed", + ) + parser.add_argument( + "--capability", + action="append", + default=[], + help="capability confirmed from the live ROS graph; repeat as needed", + ) parser.add_argument("--baudrate", type=int, default=1_000_000) parser.add_argument("--base-port", type=int, default=8765) parser.add_argument( @@ -335,6 +371,93 @@ def main() -> int: if any(not 1 <= value <= 65535 for value in args.reserved_port): parser.error("--reserved-port must be from 1 to 65535") + if args.existing_ros2: + if not 1 <= args.rosbridge_port <= 65535: + parser.error("--rosbridge-port must be from 1 to 65535") + if not args.required_topic: + parser.error("--required-topic is required with --existing-ros2") + if not args.capability: + parser.error("--capability is required with --existing-ros2") + previous = load_manifest(manifest_path) + key = existing_ros2_key(args.rosbridge_host, args.rosbridge_port) + old = next( + ( + item for item in previous["devices"] + if isinstance(item, dict) and item.get("key") == key + ), + {}, + ) + occupied_ports = discover_occupied_service_ports() + service_port = old.get("service_port") + if ( + isinstance(service_port, bool) + or not isinstance(service_port, int) + or not 1 <= service_port <= 65535 + ): + service_port = args.base_port + while service_port in {args.runtime_port, *args.reserved_port, *occupied_ports}: + service_port += 1 + if service_port > 65535: + raise ValueError("no HTTP port remains for the ROS robot service") + device_id = str(old.get("device_id") or f"{socket.gethostname()}-{key}") + name = normalize_device_name( + args.name[0] if args.name else old.get("name"), + fallback="ROS 2 Robot", + ) + device_dir = args.root / "devices" / key + config_value = { + "version": 1, + "device_id": device_id, + "name": name, + "adapter": "existing_ros2", + "mode": "read_only", + "host": args.rosbridge_host, + "rosbridge_port": args.rosbridge_port, + "required_topics": args.required_topic, + "capabilities": args.capability, + } + provider = ExistingRos2Monitor( + ExistingRos2Config( + host=args.rosbridge_host, + port=args.rosbridge_port, + required_topics=tuple(args.required_topic), + capabilities=tuple(args.capability), + ), + device_id=device_id, + ) + state = provider.connect() + connected = state.connected + connection_error = state.error + provider.close() + if not connected: + raise ValueError( + "the existing ROS 2 robot was not healthy through ROSBridge: " + + (connection_error or "required topics were unavailable") + ) + config_path = device_dir / "device.json" + save_device_config(config_value, config_path) + entry = { + "key": key, + "name": name, + "device_id": device_id, + "adapter": "existing_ros2", + "rosbridge_host": args.rosbridge_host, + "rosbridge_port": args.rosbridge_port, + "required_topics": list(args.required_topic), + "service_port": service_port, + "config": str(config_path), + "token_file": str(device_dir / "auth.token"), + "unit": hardware_unit_name(key, stack_instance), + } + manifest = {"version": FLEET_VERSION, "devices": [entry]} + save_manifest(manifest_path, manifest) + print() + print_manifest(manifest, manifest_path) + print() + print("The existing ROS 2 robot was configured read-only. No ROS messages were published.") + print("Next: ./pair.sh --all") + return 0 + candidates = ( deduplicate_serial_ports(args.serial_port) if args.serial_port diff --git a/scripts/hardware_service.py b/scripts/hardware_service.py index 30a9bab..870fc44 100644 --- a/scripts/hardware_service.py +++ b/scripts/hardware_service.py @@ -12,7 +12,7 @@ ) from blacknode_robot.devices.auth import load_auth_token, token_fingerprint from blacknode_robot.devices.calibration import CalibrationStore -from blacknode_robot.devices.device_config import load_device_config, serial_monitor_from_config +from blacknode_robot.devices.device_config import load_device_config, provider_from_config from blacknode_robot.devices.service import HardwareRuntime from blacknode_robot.devices.service.server import serve from blacknode_robot.telemetry import TelemetryBus @@ -34,7 +34,7 @@ def main() -> int: if args.config: config = load_device_config(args.config) config_device_id = config["device_id"] - provider = serial_monitor_from_config(config) + provider = provider_from_config(config) provider.connect() auth_token = None token_path = Path(args.auth_token_file) if args.auth_token_file else None @@ -45,7 +45,7 @@ def main() -> int: elif args.require_auth: parser.error(f"pairing token not found: {token_path}") device_id = args.device_id or config_device_id - if config is not None and args.config: + if config is not None and args.config and config["adapter"] == "serial_joint": calibration_store = CalibrationStore( Path(args.config).parent / "active-calibration.json", device_id=device_id, diff --git a/tests/test_robot_devices.py b/tests/test_robot_devices.py index 2217e9c..1731c63 100644 --- a/tests/test_robot_devices.py +++ b/tests/test_robot_devices.py @@ -13,6 +13,8 @@ from blacknode_robot.devices import ( DeviceState, + ExistingRos2Config, + ExistingRos2Monitor, FaultState, I2CMecanumBase, JointState, @@ -34,7 +36,11 @@ ) from blacknode_robot.devices.version import service_version from blacknode_robot.devices.calibration import CalibrationError, CalibrationStore -from blacknode_robot.devices.device_config import load_device_config +from blacknode_robot.devices.device_config import ( + load_device_config, + provider_from_config, + save_device_config, +) from blacknode_robot.devices.auth import ( authorization_matches, load_auth_token, @@ -51,6 +57,7 @@ hardware_unit_name, normalize_stack_instance, ) +from scripts import configure_devices def calibration_fixture() -> tuple[dict, dict]: @@ -115,6 +122,163 @@ def test_i2c_kinematics_can_be_checked_without_hardware(): assert adapter._wheel_commands(MobileBaseCommand(linear_x=0.1)) == (-40, 40, -40, 40) +def test_existing_ros2_monitor_reports_confirmed_topics_without_publishing(): + class FakeRos: + is_connected = False + + def __init__(self): + self.terminated = False + + def run(self, timeout): + assert timeout == 5.0 + self.is_connected = True + + def get_topics(self): + return ["/scan", "/odom", "/cmd_vel"] + + def terminate(self): + self.terminated = True + self.is_connected = False + + client = FakeRos() + monitor = ExistingRos2Monitor( + ExistingRos2Config( + required_topics=("/odom", "/scan"), + capabilities=("mobile_base", "lidar", "odometry"), + ), + device_id="rosorin-01", + client_factory=lambda _host, _port: client, + ) + + state = monitor.connect() + + assert state.connected is True + assert state.armed is False + assert state.values["read_only"] is True + assert state.values["vendor_stack_preserved"] is True + assert state.values["observed_topics"] == ["/cmd_vel", "/odom", "/scan"] + assert not hasattr(monitor, "command") + monitor.close() + assert client.terminated is True + + +def test_existing_ros2_monitor_is_unhealthy_when_observed_topic_disappears(): + class FakeRos: + is_connected = True + + def get_topics(self): + return ["/scan"] + + def terminate(self): + self.is_connected = False + + monitor = ExistingRos2Monitor( + ExistingRos2Config( + required_topics=("/odom",), + capabilities=("odometry",), + ), + client_factory=lambda _host, _port: FakeRos(), + ) + + state = monitor.refresh() + + assert state.connected is False + assert state.armed is False + assert state.values["missing_topics"] == ["/odom"] + assert "/odom" in state.error + + +def test_existing_ros2_device_config_round_trips_and_builds_provider(tmp_path: Path): + path = tmp_path / "rosorin.json" + save_device_config( + { + "version": 1, + "device_id": "rosorin-01", + "name": "ROSOrin", + "adapter": "existing_ros2", + "mode": "read_only", + "host": "127.0.0.1", + "rosbridge_port": 9090, + "required_topics": ["/odom", "/scan", "/odom"], + "capabilities": ["mobile_base", "lidar", "mobile_base"], + }, + path, + ) + + config = load_device_config(path) + provider = provider_from_config(config) + + assert config["required_topics"] == ["/odom", "/scan"] + assert config["capabilities"] == ["mobile_base", "lidar"] + assert isinstance(provider, ExistingRos2Monitor) + assert provider.device_id == "rosorin-01" + + +def test_existing_ros2_fleet_configuration_skips_serial_probe( + tmp_path: Path, monkeypatch +): + class ConnectedMonitor: + def __init__(self, _config, *, device_id): + self.device_id = device_id + + def connect(self): + return DeviceState(device_id=self.device_id, connected=True, armed=False) + + def close(self): + return None + + root = tmp_path / ".blacknode-hardware" + monkeypatch.setattr(configure_devices, "ExistingRos2Monitor", ConnectedMonitor) + monkeypatch.setattr(configure_devices, "discover_occupied_service_ports", lambda: set()) + monkeypatch.setattr( + sys, + "argv", + [ + "configure_devices.py", + "--existing-ros2", + "--required-topic", + "/odom", + "--required-topic", + "/scan", + "--capability", + "mobile_base", + "--capability", + "lidar", + "--name", + "ROSOrin", + "--root", + str(root), + ], + ) + + assert configure_devices.main() == 0 + + manifest = json.loads((root / "devices.json").read_text(encoding="utf-8")) + entry = manifest["devices"][0] + config = load_device_config(entry["config"]) + assert entry["adapter"] == "existing_ros2" + assert entry["name"] == "ROSOrin" + assert "serial_port" not in entry + assert config["required_topics"] == ["/odom", "/scan"] + + +def test_nonexclusive_ros_provider_is_not_leased_away_from_status(): + class Provider: + exclusive_connection = False + capabilities = ("ros2_graph",) + + def refresh(self): + return DeviceState(device_id="rosorin", connected=True, armed=False) + + runtime = HardwareRuntime(Provider(), device_id="rosorin") + + released = runtime.release() + + assert released["ok"] is True + assert released["status"]["connected"] is True + assert released["status"]["leased_to_deployment"] is False + + def test_joint_command_tracks_freshness(): command = JointGroupCommand({"joint_1": 0.2}, expires_at=10.0) assert command.is_fresh(now=9.0)