diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py index c954f31..bb1877f 100644 --- a/orca_core/hardware_hand.py +++ b/orca_core/hardware_hand.py @@ -1339,19 +1339,29 @@ def _connect_sensor_with_fallback(self) -> tuple[bool, str]: self._tactile_client = self._create_tactile_client() try: self._tactile_client.connect() - return True, f"Sensor connected on {self.config.sensor_port}" + if self._tactile_client.get_tactile_configuration() is not None: + return True, f"Sensor connected on {self.config.sensor_port}" + print(f"Port {self.config.sensor_port} opened but sensor did not respond; " + "trying auto-detection.") + self._tactile_client.disconnect() + self._tactile_client = None except Exception as e: print(f"Sensor connection failed on {self.config.sensor_port}: {e}") self._tactile_client = None chosen = auto_detect_port("tactile_sensor") - if chosen and chosen != self.config.sensor_port: + if chosen: try: self.config = dataclasses.replace(self.config, sensor_port=chosen) self._tactile_client = self._create_tactile_client() self._tactile_client.connect() - self._persist_sensor_port(chosen) - return True, f"Sensor connected on auto-detected {chosen}" + if self._tactile_client.get_tactile_configuration() is None: + print(f"Auto-detected port {chosen} opened but sensor did not respond.") + self._tactile_client.disconnect() + self._tactile_client = None + else: + self._persist_sensor_port(chosen) + return True, f"Sensor connected on auto-detected {chosen}" except Exception as e: print(f"Auto-detected sensor port {chosen} also failed: {e}") self._tactile_client = None diff --git a/scripts/example_tactile.py b/scripts/example_tactile.py new file mode 100644 index 0000000..c1dc90b --- /dev/null +++ b/scripts/example_tactile.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python +"""Minimal example: connect to an OrcaHandTouch and stream resultant forces. + +Usage: + uv run python scripts/example_tactile.py + uv run python scripts/example_tactile.py path/to/config.yaml +""" + +import sys +import time +from pathlib import Path + +from orca_core import OrcaHandTouch + +DEFAULT_CONFIG = ( + Path(__file__).resolve().parents[1] + / "orca_core" / "models" / "v2" / "orcahand_touch_right" / "config.yaml" +) + + +def main(): + config_path = sys.argv[1] if len(sys.argv) > 1 else str(DEFAULT_CONFIG) + hand = OrcaHandTouch(config_path=config_path) + + ok, msg = hand.connect_sensors_only() + print(msg) + if not ok: + sys.exit(1) + + try: + hand.start_tactile_stream(resultant=True, taxels=False, min_sensors=1) + time.sleep(0.1) + + print("Streaming resultant forces — press Ctrl+C to stop.\n") + while True: + forces = hand.get_tactile_forces() + if forces is None: + time.sleep(0.01) + continue + parts = [f"{f}: [{v[0]:6.2f} {v[1]:6.2f} {v[2]:6.2f}]" + for f, v in forces.forces.items()] + print(" | ".join(parts), end="\r") + time.sleep(0.02) + except KeyboardInterrupt: + print() + finally: + hand.disconnect() + + +if __name__ == "__main__": + main()